FW 2.7: Better encoder support, small UART and CAN fixes, NRF command…

…s preparation
huhaihui · Dec 28, 2015 · 42c1b79 · 42c1b79
1 parent d995cf6
commit 42c1b79
Show file tree

Hide file tree

Showing 16 changed files with 288 additions and 42 deletions.
diff --git a/applications/app_uartcomm.c b/applications/app_uartcomm.c
@@ -88,7 +88,9 @@ static void rxchar(UARTDriver *uartp, uint16_t c) {
 		serial_rx_write_pos = 0;
 	}
 
-	chEvtSignal(process_tp, (eventmask_t) 1);
+	chSysLockFromISR();
+	chEvtSignalI(process_tp, (eventmask_t) 1);
+	chSysUnlockFromISR();
 }
 
 /*

diff --git a/comm_can.c b/comm_can.c
@@ -351,7 +351,7 @@ void comm_can_send_buffer(uint8_t controller_id, uint8_t *data, unsigned int len
 		for (unsigned int i = 0;i < len;i += 7) {
 			end_a = i;
 
-			if (i > 256) {
+			if (i > 255) {
 				break;
 			}
 

diff --git a/commands.c b/commands.c
@@ -64,6 +64,7 @@ static float detect_low_duty;
 static int8_t detect_hall_table[8];
 static int detect_hall_res;
 static void(*send_func)(unsigned char *data, unsigned int len) = 0;
+static disp_pos_mode display_position_mode;
 
 void commands_init(void) {
 	chThdCreateStatic(detect_thread_wa, sizeof(detect_thread_wa), NORMALPRIO, detect_thread, NULL);
@@ -213,7 +214,19 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		break;
 
 	case COMM_SET_DETECT:
-		mcpwm_set_detect();
+		mcconf = *mc_interface_get_configuration();
+
+		ind = 0;
+		display_position_mode = data[ind++];
+
+		if (mcconf.motor_type == MOTOR_TYPE_BLDC) {
+			if (display_position_mode == DISP_POS_MODE_NONE) {
+				mc_interface_release_motor();
+			} else if (display_position_mode == DISP_POS_MODE_INDUCTANCE) {
+				mcpwm_set_detect();
+			}
+		}
+
 		timeout_reset();
 		break;
 
@@ -577,9 +590,6 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		mcconf = *mc_interface_get_configuration();
 		mcconf_old = mcconf;
 
-		ind = 0;
-		send_buffer[ind++] = COMM_DETECT_MOTOR_R_L;
-
 		mcconf.motor_type = MOTOR_TYPE_FOC;
 		mc_interface_set_configuration(&mcconf);
 
@@ -593,6 +603,8 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 			l = 0.0;
 		}
 
+		ind = 0;
+		send_buffer[ind++] = COMM_DETECT_MOTOR_R_L;
 		buffer_append_float32(send_buffer, r, 1e6, &ind);
 		buffer_append_float32(send_buffer, l, 1e3, &ind);
 		commands_send_packet(send_buffer, ind);
@@ -620,6 +632,43 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 	}
 	break;
 
+	case COMM_DETECT_ENCODER: {
+#if ENCODER_ENABLE
+		mcconf = *mc_interface_get_configuration();
+		mcconf_old = mcconf;
+
+		ind = 0;
+		float current = buffer_get_float32(data, 1e3, &ind);
+
+		mcconf.motor_type = MOTOR_TYPE_FOC;
+		mcconf.foc_f_sw = 10000.0;
+		mcconf.foc_current_kp = 0.01;
+		mcconf.foc_current_ki = 10.0;
+		mc_interface_set_configuration(&mcconf);
+
+		float offset = 0.0;
+		float ratio = 0.0;
+		bool inverted = false;
+		mcpwm_foc_encoder_detect(current, false, &offset, &ratio, &inverted);
+		mc_interface_set_configuration(&mcconf_old);
+
+		ind = 0;
+		send_buffer[ind++] = COMM_DETECT_ENCODER;
+		buffer_append_float32(send_buffer, offset, 1e6, &ind);
+		buffer_append_float32(send_buffer, ratio, 1e6, &ind);
+		send_buffer[ind++] = inverted;
+		commands_send_packet(send_buffer, ind);
+#else
+		ind = 0;
+		send_buffer[ind++] = COMM_DETECT_ENCODER;
+		buffer_append_float32(send_buffer, 1001.0, 1e6, &ind);
+		buffer_append_float32(send_buffer, 0.0, 1e6, &ind);
+		send_buffer[ind++] = false;
+		commands_send_packet(send_buffer, ind);
+#endif
+	}
+	break;
+
 	case COMM_REBOOT:
 		// Lock the system and enter an infinite loop. The watchdog will reboot.
 		__disable_irq();
@@ -717,6 +766,10 @@ void commands_send_experiment_samples(float *samples, int len) {
 	commands_send_packet(buffer, index);
 }
 
+disp_pos_mode commands_get_disp_pos_mode(void) {
+	return display_position_mode;
+}
+
 static THD_FUNCTION(detect_thread, arg) {
 	(void)arg;
 

diff --git a/commands.h b/commands.h
@@ -36,5 +36,6 @@ void commands_printf(char* format, ...);
 void commands_send_samples(uint8_t *data, int len);
 void commands_send_rotor_pos(float rotor_pos);
 void commands_send_experiment_samples(float *samples, int len);
+disp_pos_mode commands_get_disp_pos_mode(void);
 
 #endif /* COMMANDS_H_ */
diff --git a/conf_general.h b/conf_general.h
@@ -27,7 +27,7 @@
 
 // Firmware version
 #define FW_VERSION_MAJOR	2
-#define FW_VERSION_MINOR	6
+#define FW_VERSION_MINOR	7
 
 #include "datatypes.h"
 

diff --git a/datatypes.h b/datatypes.h
@@ -84,6 +84,15 @@ typedef enum {
 	CONTROL_MODE_NONE
 } mc_control_mode;
 
+typedef enum {
+	DISP_POS_MODE_NONE = 0,
+	DISP_POS_MODE_INDUCTANCE,
+	DISP_POS_MODE_OBSERVER,
+	DISP_POS_MODE_ENCODER,
+	DISP_POS_MODE_ENCODER_POS_ERROR,
+	DISP_POS_MODE_ENCODER_OBSERVER_ERROR
+} disp_pos_mode;
+
 typedef struct {
 	float cycle_int_limit;
 	float cycle_int_limit_running;
@@ -325,6 +334,7 @@ typedef enum {
 	COMM_DETECT_MOTOR_PARAM,
 	COMM_DETECT_MOTOR_R_L,
 	COMM_DETECT_MOTOR_FLUX_LINKAGE,
+	COMM_DETECT_ENCODER,
 	COMM_REBOOT,
 	COMM_ALIVE,
 	COMM_GET_DECODED_PPM,
@@ -406,7 +416,11 @@ typedef struct {
 typedef enum {
 	MOTE_PACKET_BATT_LEVEL = 0,
 	MOTE_PACKET_BUTTONS,
-	MOTE_PACKET_ALIVE
+	MOTE_PACKET_ALIVE,
+	MOTE_PACKET_FILL_RX_BUFFER,
+	MOTE_PACKET_FILL_RX_BUFFER_LONG,
+	MOTE_PACKET_PROCESS_RX_BUFFER,
+	MOTE_PACKET_PROCESS_SHORT_BUFFER,
 } MOTE_PACKET;
 
 typedef struct {

diff --git a/main.c b/main.c
@@ -149,11 +149,35 @@ static THD_FUNCTION(periodic_thread, arg) {
 			commands_send_rotor_pos(mcpwm_get_detect_pos());
 		}
 
-#if ENCODER_ENABLE
-//		commands_send_rotor_pos(utils_angle_difference(mcpwm_foc_get_phase_observer(), mcpwm_foc_get_phase_encoder()));
-//		commands_send_rotor_pos(encoder_read_deg());
-//		comm_can_set_pos(0, encoder_read_deg());
-#endif
+		disp_pos_mode display_mode = commands_get_disp_pos_mode();
+
+		switch (display_mode) {
+			case DISP_POS_MODE_ENCODER:
+				commands_send_rotor_pos(encoder_read_deg());
+				break;
+
+			case DISP_POS_MODE_ENCODER_POS_ERROR:
+				commands_send_rotor_pos(utils_angle_difference(mc_interface_get_pos_set(), encoder_read_deg()));
+				break;
+
+			default:
+				break;
+		}
+
+		if (mc_interface_get_configuration()->motor_type == MOTOR_TYPE_FOC) {
+			switch (display_mode) {
+			case DISP_POS_MODE_OBSERVER:
+				commands_send_rotor_pos(mcpwm_foc_get_phase_observer());
+				break;
+
+			case DISP_POS_MODE_ENCODER_OBSERVER_ERROR:
+				commands_send_rotor_pos(utils_angle_difference(mcpwm_foc_get_phase_observer(), mcpwm_foc_get_phase_encoder()));
+				break;
+
+			default:
+				break;
+		}
+		}
 
 		chThdSleepMilliseconds(10);
 	}
@@ -341,6 +365,10 @@ int main(void) {
 	chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
 
 	for(;;) {
-		chThdSleepMilliseconds(5000);
+		chThdSleepMilliseconds(10);
+
+#if ENCODER_ENABLE
+//		comm_can_set_pos(0, encoder_read_deg());
+#endif
 	}
 }
diff --git a/mc_interface.c b/mc_interface.c
@@ -53,6 +53,7 @@ static volatile float amp_seconds;
 static volatile float amp_seconds_charged;
 static volatile float watt_seconds;
 static volatile float watt_seconds_charged;
+static volatile float position_set;
 
 // Private functions
 static void update_override_limits(volatile mc_configuration *conf);
@@ -79,6 +80,7 @@ void mc_interface_init(mc_configuration *configuration) {
 	amp_seconds_charged = 0.0;
 	watt_seconds = 0.0;
 	watt_seconds_charged = 0.0;
+	position_set = 0.0;
 
 	// Start threads
 	chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
@@ -286,6 +288,8 @@ void mc_interface_set_pid_pos(float pos) {
 		return;
 	}
 
+	position_set = pos;
+
 	switch (conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
@@ -705,6 +709,10 @@ float mc_interface_read_reset_avg_input_current(void) {
 	return res;
 }
 
+float mc_interface_get_pos_set(void) {
+	return position_set;
+}
+
 // MC implementation functions
 /**
  * A helper function that should be called before sending commands to control

diff --git a/mc_interface.h b/mc_interface.h
@@ -65,6 +65,7 @@ int mc_interface_get_tachometer_abs_value(bool reset);
 float mc_interface_get_last_inj_adc_isr_duration(void);
 float mc_interface_read_reset_avg_motor_current(void);
 float mc_interface_read_reset_avg_input_current(void);
+float mc_interface_get_pos_set(void);
 
 // MC implementation functions
 void mc_interface_fault_stop(mc_fault_code fault);

diff --git a/mcconf/mcconf_default.h b/mcconf/mcconf_default.h
@@ -113,13 +113,13 @@
 
 // Position PID parameters
 #ifndef MCCONF_P_PID_KP
-#define MCCONF_P_PID_KP					0.01	// Proportional gain
+#define MCCONF_P_PID_KP					0.03	// Proportional gain
 #endif
 #ifndef MCCONF_P_PID_KI
 #define MCCONF_P_PID_KI					0.0		// Integral gain
 #endif
 #ifndef MCCONF_P_PID_KD
-#define MCCONF_P_PID_KD					0.0006	// Derivative gain
+#define MCCONF_P_PID_KD					0.0004	// Derivative gain
 #endif
 
 // Current control parameters