Merge branch 'master' into balance

CHANGELOG

@@ -1,3 +1,6 @@
+=== FW 3.61 ===
+* Added PPM_CTRL_TYPE_CURRENT_SMART_REV mode.
+
 === FW 3.60 ===
 * Fixed IMU9x50 bug.
 * Unrigester ICM20948 terminal callbacks when unused.

appconf/appconf_default.h

@@ -113,6 +113,13 @@
 #ifndef APPCONF_PPM_MAX_ERPM_FOR_DIR
 #define APPCONF_PPM_MAX_ERPM_FOR_DIR		4000.0
 #endif
+#ifndef APPCONF_PPM_SMART_REV_MAX_DUTY
+#define APPCONF_PPM_SMART_REV_MAX_DUTY		0.07
+#endif
+#ifndef APPCONF_PPM_SMART_REV_RAMP_TIME
+#define APPCONF_PPM_SMART_REV_RAMP_TIME		3.0
+#endif
+
 
 // ADC app configureation
 #ifndef APPCONF_ADC_CTRL_TYPE

applications/app_nunchuk.c

@@ -369,7 +369,7 @@ static THD_FUNCTION(output_thread, arg) {
 		}
 
 		float rpm_lowest = rpm_local;
-		float current_highest_abs = current_now;
+		float current_highest_abs = fabsf(current_now);
 		float duty_highest_abs = fabsf(duty_now);
 
 		if (config.multi_esc) {
@@ -392,8 +392,8 @@ static THD_FUNCTION(output_thread, arg) {
 						msg_current = -msg_current;
 					}
 
-					if (fabsf(msg_current) > fabsf(current_highest_abs)) {
-						current_highest_abs = msg_current;
+					if (fabsf(msg_current) > current_highest_abs) {
+						current_highest_abs = fabsf(msg_current);
 					}
 
 					if (fabsf(msg->duty) > duty_highest_abs) {
@@ -408,7 +408,7 @@ static THD_FUNCTION(output_thread, arg) {
 			static bool was_duty_control = false;
 			static float duty_rev = 0.0;
 
-			if (out_val < -0.95 && duty_highest_abs < (mcconf->l_min_duty * 1.5) &&
+			if (out_val < -0.92 && duty_highest_abs < (mcconf->l_min_duty * 1.5) &&
 					current_highest_abs < (mcconf->l_current_max * mcconf->l_current_max_scale * 0.7)) {
 				duty_control = true;
 			}

applications/app_ppm.c

@@ -197,10 +197,12 @@ static THD_FUNCTION(ppm_thread, arg) {
 			ramp_time = fminf(config.ramp_time_pos, config.ramp_time_neg);
 		}
 
+		const float dt = (float)ST2MS(chVTTimeElapsedSinceX(last_time)) / 1000.0;
+		last_time = chVTGetSystemTimeX();
+
 		if (ramp_time > 0.01) {
-			const float ramp_step = (float)ST2MS(chVTTimeElapsedSinceX(last_time)) / (ramp_time * 1000.0);
+			const float ramp_step = dt / ramp_time;
 			utils_step_towards(&servo_val_ramp, servo_val, ramp_step);
-			last_time = chVTGetSystemTimeX();
 			servo_val = servo_val_ramp;
 		}
 
@@ -298,6 +300,7 @@ static THD_FUNCTION(ppm_thread, arg) {
 			break;
 
 		case PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE:
+		case PPM_CTRL_TYPE_CURRENT_SMART_REV:
 			current_mode = true;
 			if (servo_val >= 0.0) {
 				current = servo_val * mcconf->lo_current_motor_max_now;
@@ -349,21 +352,67 @@ static THD_FUNCTION(ppm_thread, arg) {
 			continue;
 		}
 
-		// Find lowest RPM
+		const float duty_now = mc_interface_get_duty_cycle_now();
+		float current_highest_abs = fabsf(mc_interface_get_tot_current_directional_filtered());
+		float duty_highest_abs = fabsf(duty_now);
+
 		if (config.multi_esc) {
 			for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
 				can_status_msg *msg = comm_can_get_status_msg_index(i);
 
 				if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
-					float rpm_tmp = msg->rpm;
+					if (fabsf(msg->rpm) < fabsf(rpm_lowest)) {
+						rpm_lowest = msg->rpm;
+					}
 
-					if (fabsf(rpm_tmp) < fabsf(rpm_lowest)) {
-						rpm_lowest = rpm_tmp;
+					if (fabsf(msg->current) > current_highest_abs) {
+						current_highest_abs = fabsf(msg->current);
+					}
+
+					if (fabsf(msg->duty) > duty_highest_abs) {
+						duty_highest_abs = fabsf(msg->duty);
 					}
 				}
 			}
 		}
 
+		if (config.ctrl_type == PPM_CTRL_TYPE_CURRENT_SMART_REV) {
+			bool duty_control = false;
+			static bool was_duty_control = false;
+			static float duty_rev = 0.0;
+
+			if (servo_val < -0.92 && duty_highest_abs < (mcconf->l_min_duty * 1.5) &&
+					current_highest_abs < (mcconf->l_current_max * mcconf->l_current_max_scale * 0.7)) {
+				duty_control = true;
+			}
+
+			if (duty_control || (was_duty_control && servo_val < -0.1)) {
+				was_duty_control = true;
+
+				float goal = config.smart_rev_max_duty * -servo_val;
+				utils_step_towards(&duty_rev, -goal,
+						config.smart_rev_max_duty * dt / config.smart_rev_ramp_time);
+
+				mc_interface_set_duty(duty_rev);
+
+				// Send the same duty cycle to the other controllers
+				if (config.multi_esc) {
+					for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+						can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+						if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
+							comm_can_set_duty(msg->id, duty_rev);
+						}
+					}
+				}
+
+				current_mode = false;
+			} else {
+				duty_rev = duty_now;
+				was_duty_control = false;
+			}
+		}
+
 		if ((send_current || send_duty) && config.multi_esc) {
 			float current_filtered = mc_interface_get_tot_current_directional_filtered();
 			float duty = mc_interface_get_duty_cycle_now();

conf_general.h

@@ -22,7 +22,7 @@
 
 // Firmware version
 #define FW_VERSION_MAJOR		3
-#define FW_VERSION_MINOR		60
+#define FW_VERSION_MINOR		61
 
 #include "datatypes.h"
 

confgenerator.c

@@ -174,6 +174,8 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
 	buffer[ind++] = conf->app_ppm_conf.tc;
 	buffer_append_float32_auto(buffer, conf->app_ppm_conf.tc_max_diff, &ind);
 	buffer_append_float32_auto(buffer, conf->app_ppm_conf.max_erpm_for_dir, &ind);
+	buffer_append_float32_auto(buffer, conf->app_ppm_conf.smart_rev_max_duty, &ind);
+	buffer_append_float32_auto(buffer, conf->app_ppm_conf.smart_rev_ramp_time, &ind);
 	buffer[ind++] = conf->app_adc_conf.ctrl_type;
 	buffer_append_float32_auto(buffer, conf->app_adc_conf.hyst, &ind);
 	buffer_append_float32_auto(buffer, conf->app_adc_conf.voltage_start, &ind);
@@ -441,6 +443,8 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
 	conf->app_ppm_conf.tc = buffer[ind++];
 	conf->app_ppm_conf.tc_max_diff = buffer_get_float32_auto(buffer, &ind);
 	conf->app_ppm_conf.max_erpm_for_dir = buffer_get_float32_auto(buffer, &ind);
+	conf->app_ppm_conf.smart_rev_max_duty = buffer_get_float32_auto(buffer, &ind);
+	conf->app_ppm_conf.smart_rev_ramp_time = buffer_get_float32_auto(buffer, &ind);
 	conf->app_adc_conf.ctrl_type = buffer[ind++];
 	conf->app_adc_conf.hyst = buffer_get_float32_auto(buffer, &ind);
 	conf->app_adc_conf.voltage_start = buffer_get_float32_auto(buffer, &ind);
@@ -692,6 +696,8 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
 	conf->app_ppm_conf.tc = APPCONF_PPM_TC;
 	conf->app_ppm_conf.tc_max_diff = APPCONF_PPM_TC_MAX_DIFF;
 	conf->app_ppm_conf.max_erpm_for_dir = APPCONF_PPM_MAX_ERPM_FOR_DIR;
+	conf->app_ppm_conf.smart_rev_max_duty = APPCONF_PPM_SMART_REV_MAX_DUTY;
+	conf->app_ppm_conf.smart_rev_ramp_time = APPCONF_PPM_SMART_REV_RAMP_TIME;
 	conf->app_adc_conf.ctrl_type = APPCONF_ADC_CTRL_TYPE;
 	conf->app_adc_conf.hyst = APPCONF_ADC_HYST;
 	conf->app_adc_conf.voltage_start = APPCONF_ADC_VOLTAGE_START;

confgenerator.h

@@ -9,7 +9,7 @@
 
 // Constants
 #define MCCONF_SIGNATURE		503309878
-#define APPCONF_SIGNATURE		183781183
+#define APPCONF_SIGNATURE		3900592330
 
 // Functions
 int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

datatypes.h

@@ -338,7 +338,8 @@ typedef enum {
 	PPM_CTRL_TYPE_DUTY_NOREV,
 	PPM_CTRL_TYPE_PID,
 	PPM_CTRL_TYPE_PID_NOREV,
-	PPM_CTRL_TYPE_CURRENT_BRAKE_REV_HYST
+	PPM_CTRL_TYPE_CURRENT_BRAKE_REV_HYST,
+	PPM_CTRL_TYPE_CURRENT_SMART_REV
 } ppm_control_type;
 
 typedef struct {
@@ -359,6 +360,8 @@ typedef struct {
 	bool tc;
 	float tc_max_diff;
 	float max_erpm_for_dir;
+	float smart_rev_max_duty;
+	float smart_rev_ramp_time;
 } ppm_config;
 
 // ADC control types