FW 2.17: as5047 support, change sensor port mode in conf, better enco…

…der detection, FOC ah and wh counter fixes
Ross-Robotics · Apr 27, 2016 · cb2a205 · cb2a205
1 parent 3e18879
commit cb2a205
Show file tree

Hide file tree

Showing 28 changed files with 535 additions and 210 deletions.
diff --git a/Makefile b/Makefile
@@ -270,8 +270,8 @@ build/$(PROJECT).bin: build/$(PROJECT).elf
 # Program
 upload: build/$(PROJECT).bin
 	#qstlink2 --cli --erase --write build/$(PROJECT).bin
-	openocd -f interface/stlink-v2.cfg -c "set WORKAREASIZE 0x2000" -f target/stm32f4x_stlink.cfg -c "program build/$(PROJECT).elf verify reset"
-	#openocd -f board/stm32f4discovery.cfg -c "reset_config trst_only combined" -c "program build/$(PROJECT).elf verify reset exit" # For openocd 0.9
+	#openocd -f interface/stlink-v2.cfg -c "set WORKAREASIZE 0x2000" -f target/stm32f4x_stlink.cfg -c "program build/$(PROJECT).elf verify reset" # Older openocd
+	openocd -f board/stm32f4discovery.cfg -c "reset_config trst_only combined" -c "program build/$(PROJECT).elf verify reset exit" # For openocd 0.9
 
 #program with olimex arm-usb-tiny-h and jtag-swd adapter board. needs openocd>=0.9
 upload-olimex: build/$(PROJECT).bin

diff --git a/appconf/appconf_custom.h b/appconf/appconf_custom.h
@@ -0,0 +1,31 @@
+/*
+	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+
+	This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+    */
+
+/*
+ * appconf_example_ppm.h
+ *
+ *  Created on: 9 sep 2015
+ *      Author: benjamin
+ */
+
+#ifndef APPCONF_APPCONF_EXAMPLE_PPM_H_
+#define APPCONF_APPCONF_EXAMPLE_PPM_H_
+
+// Use custom user application
+#define APPCONF_APP_TO_USE				APP_CUSTOM
+
+#endif
diff --git a/applications/app_sten.c b/applications/app_sten.c
@@ -32,6 +32,8 @@
 #include "utils.h"
 #include "hw.h"
 #include "timeout.h"
+#include "comm_can.h"
+
 #include <math.h>
 
 // Settings
@@ -147,15 +149,7 @@ static THD_FUNCTION(uart_thread, arg) {
 }
 
 static void set_output(float output) {
-	output /= (1.0 - HYST);
-
-	if (output > HYST) {
-		output -= HYST;
-	} else if (output < -HYST) {
-		output += HYST;
-	} else {
-		output = 0.0;
-	}
+	utils_deadband(&output, HYST, 1.0);
 
 	const float rpm = mc_interface_get_rpm();
 
@@ -182,13 +176,11 @@ static void set_output(float output) {
 		current_p2 = current_p1;
 		current_p1 = current;
 
-		if (fabsf(current) < mc_interface_get_configuration()->cc_min_current) {
-			current = -mc_interface_get_configuration()->cc_min_current;
-		}
-
 		mc_interface_set_current(current);
+		comm_can_set_current(255, current);
 	} else {
 		mc_interface_set_brake_current(output * mc_interface_get_configuration()->l_current_min);
+		comm_can_set_current_brake(255, output * mc_interface_get_configuration()->l_current_min);
 	}
 }
 

diff --git a/commands.c b/commands.c
@@ -314,7 +314,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		mcconf.foc_sl_d_current_factor = buffer_get_float32(data, 1e3, &ind);
 		memcpy(mcconf.foc_hall_table, data + ind, 8);
 		ind += 8;
-		mcconf.foc_hall_sl_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
+		mcconf.foc_sl_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
 
 		mcconf.s_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
 		mcconf.s_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
@@ -336,14 +336,11 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		mcconf.m_duty_ramp_step_rpm_lim = (float)buffer_get_float32(data, 1000000.0, &ind);
 		mcconf.m_current_backoff_gain = (float)buffer_get_float32(data, 1000000.0, &ind);
 		mcconf.m_encoder_counts = buffer_get_uint32(data, &ind);
+		mcconf.m_sensor_port_mode = data[ind++];
 
 		conf_general_store_mc_configuration(&mcconf);
 		mc_interface_set_configuration(&mcconf);
 
-#if ENCODER_ENABLE
-		encoder_set_counts(mcconf.m_encoder_counts);
-#endif
-
 		ind = 0;
 		send_buffer[ind++] = packet_id;
 		commands_send_packet(send_buffer, ind);
@@ -422,7 +419,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		buffer_append_float32(send_buffer, mcconf.foc_sl_d_current_factor, 1e3, &ind);
 		memcpy(send_buffer + ind, mcconf.foc_hall_table, 8);
 		ind += 8;
-		buffer_append_int32(send_buffer, (int32_t)(mcconf.foc_hall_sl_erpm * 1000.0), &ind);
+		buffer_append_int32(send_buffer, (int32_t)(mcconf.foc_sl_erpm * 1000.0), &ind);
 
 		buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kp * 1000000.0), &ind);
 		buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_ki * 1000000.0), &ind);
@@ -444,6 +441,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		buffer_append_float32(send_buffer, mcconf.m_duty_ramp_step_rpm_lim, 1000000.0, &ind);
 		buffer_append_float32(send_buffer, mcconf.m_current_backoff_gain, 1000000.0, &ind);
 		buffer_append_uint32(send_buffer, mcconf.m_encoder_counts, &ind);
+		send_buffer[ind++] = mcconf.m_sensor_port_mode;
 
 		commands_send_packet(send_buffer, ind);
 		break;
@@ -663,66 +661,74 @@ 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
+		if (encoder_is_configured()) {
+			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);
+		}
 	}
 	break;
 
 	case COMM_DETECT_HALL_FOC: {
 		mcconf = *mc_interface_get_configuration();
-		mcconf_old = mcconf;
 
-		ind = 0;
-		float current = buffer_get_float32(data, 1e3, &ind);
+		if (mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_HALL) {
+			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);
+			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);
 
-		uint8_t hall_tab[8];
-		bool res = mcpwm_foc_hall_detect(current, hall_tab);
-		mc_interface_set_configuration(&mcconf_old);
+			uint8_t hall_tab[8];
+			bool res = mcpwm_foc_hall_detect(current, hall_tab);
+			mc_interface_set_configuration(&mcconf_old);
 
-		ind = 0;
-		send_buffer[ind++] = COMM_DETECT_HALL_FOC;
-		memcpy(send_buffer + ind, hall_tab, 8);
-		ind += 8;
-		send_buffer[ind++] = res ? 0 : 1;
+			ind = 0;
+			send_buffer[ind++] = COMM_DETECT_HALL_FOC;
+			memcpy(send_buffer + ind, hall_tab, 8);
+			ind += 8;
+			send_buffer[ind++] = res ? 0 : 1;
 
-		commands_send_packet(send_buffer, ind);
+			commands_send_packet(send_buffer, ind);
+		} else {
+			ind = 0;
+			send_buffer[ind++] = COMM_DETECT_HALL_FOC;
+			memset(send_buffer, 255, 8);
+			ind += 8;
+			send_buffer[ind++] = 0;
+		}
 	}
 	break;
 

diff --git a/conf_general.c b/conf_general.c
@@ -218,7 +218,7 @@ void conf_general_get_default_mc_configuration(mc_configuration *conf) {
 	conf->foc_hall_table[5] = MCCONF_FOC_HALL_TAB_5;
 	conf->foc_hall_table[6] = MCCONF_FOC_HALL_TAB_6;
 	conf->foc_hall_table[7] = MCCONF_FOC_HALL_TAB_7;
-	conf->foc_hall_sl_erpm = MCCONF_FOC_HALL_ERPM;
+	conf->foc_sl_erpm = MCCONF_FOC_SL_ERPM;
 
 	conf->s_pid_kp = MCCONF_S_PID_KP;
 	conf->s_pid_ki = MCCONF_S_PID_KI;
@@ -240,6 +240,7 @@ void conf_general_get_default_mc_configuration(mc_configuration *conf) {
 	conf->m_duty_ramp_step_rpm_lim = MCCONF_M_RAMP_STEP_RPM_LIM;
 	conf->m_current_backoff_gain = MCCONF_M_CURRENT_BACKOFF_GAIN;
 	conf->m_encoder_counts = MCCONF_M_ENCODER_COUNTS;
+	conf->m_sensor_port_mode = MCCONF_M_SENSOR_PORT_MODE;
 }
 
 /**

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	16
+#define FW_VERSION_MINOR	17
 
 #include "datatypes.h"
 
@@ -38,6 +38,7 @@
 
 // Settings and parameters to override
 //#define VIN_R1				33000.0
+//#define VIN_R1				39200.0
 //#define VIN_R2				2200.0
 //#define CURRENT_AMP_GAIN	10.0
 //#define CURRENT_SHUNT_RES	0.0005
@@ -65,11 +66,13 @@
  * Select default user motor configuration
  */
 //#define MCCONF_DEFAULT_USER		"mcconf_sten.h"
+//#define MCCONF_DEFAULT_USER		"mcconf_sp_540kv.h"
 
 /*
  * Select default user app configuration
  */
 //#define APPCONF_DEFAULT_USER		"appconf_example_ppm.h"
+//#define APPCONF_DEFAULT_USER		"appconf_custom.h"
 
 /*
  * Select which custom application to use. To configure the default applications and
@@ -78,13 +81,6 @@
  */
 //#define USE_APP_STEN
 
-/*
- * Use encoder
- */
-#ifndef ENCODER_ENABLE
-#define ENCODER_ENABLE			0
-#endif
-
 /*
  * Enable CAN-bus
  */

diff --git a/datatypes.h b/datatypes.h
@@ -95,6 +95,12 @@ typedef enum {
 	DISP_POS_MODE_ENCODER_OBSERVER_ERROR
 } disp_pos_mode;
 
+typedef enum {
+	SENSOR_PORT_MODE_HALL = 0,
+	SENSOR_PORT_MODE_ABI,
+	SENSOR_PORT_MODE_AS5047_SPI
+} sensor_port_mode;
+
 typedef struct {
 	float cycle_int_limit;
 	float cycle_int_limit_running;
@@ -172,7 +178,7 @@ typedef struct {
 	float foc_sl_d_current_factor;
 	mc_foc_sensor_mode foc_sensor_mode;
 	uint8_t foc_hall_table[8];
-	float foc_hall_sl_erpm;
+	float foc_sl_erpm;
 	// Speed PID
 	float s_pid_kp;
 	float s_pid_ki;
@@ -194,6 +200,7 @@ typedef struct {
 	float m_duty_ramp_step_rpm_lim;
 	float m_current_backoff_gain;
 	uint32_t m_encoder_counts;
+	sensor_port_mode m_sensor_port_mode;
 } mc_configuration;
 
 // Applications to use