Merge pull request vedderb#392 from luna-cycle/luna_bbshd_support

Luna bbshd support

Author: vedderb

applications/app.c

@@ -125,7 +125,7 @@ void app_set_configuration(app_configuration *conf) {
 		break;
 
 	case APP_ADC_PAS:
-		app_adc_start(true);
+		app_adc_start(false);
 		app_pas_start(false);
 		break;
 

applications/app.h

@@ -81,6 +81,7 @@ void app_pas_stop(void);
 bool app_pas_is_running(void);
 void app_pas_configure(pas_config *conf);
 float app_pas_get_current_target_rel(void);
+void app_pas_set_current_sub_scaling(float current_sub_scaling);
 
 // Custom apps
 void app_custom_start(void);

applications/app_pas.c

@@ -32,6 +32,7 @@
 
 // Settings
 #define PEDAL_INPUT_TIMEOUT				0.2
+#define MAX_MS_WITHOUT_CADENCE			5000
 #define MIN_MS_WITHOUT_POWER			500
 #define FILTER_SAMPLES					5
 #define RPM_FILTER_SAMPLES				8
@@ -42,6 +43,7 @@ static THD_WORKING_AREA(pas_thread_wa, 1024);
 
 // Private variables
 static volatile pas_config config;
+static volatile float sub_scaling = 1.0;
 static volatile float output_current_rel = 0.0;
 static volatile float ms_without_power = 0.0;
 static volatile float max_pulse_period = 0.0;
@@ -51,7 +53,14 @@ static volatile float pedal_rpm = 0;
 static volatile bool primary_output = false;
 static volatile bool stop_now = true;
 static volatile bool is_running = false;
+static volatile float torque_ratio = 0.0;
 
+/**
+ * Configure and initialize PAS application
+ *
+ * @param conf
+ * App config
+ */
 void app_pas_configure(pas_config *conf) {
 	config = *conf;
 	ms_without_power = 0.0;
@@ -63,10 +72,7 @@ void app_pas_configure(pas_config *conf) {
 	// if pedal spins at x3 the end rpm, assume its beyond limits
 	min_pedal_period = 1.0 / ((config.pedal_rpm_end * 3.0 / 60.0));
 
-	if (config.invert_pedal_direction == true )
-		direction_conf= -1.0;
-	else
-		direction_conf = 1.0;
+	(config.invert_pedal_direction) ? (direction_conf = -1.0) : (direction_conf = 1.0);
 }
 
 /**
@@ -101,19 +107,24 @@ void app_pas_stop(void) {
 	}
 }
 
+void app_pas_set_current_sub_scaling(float current_sub_scaling) {
+	sub_scaling = current_sub_scaling;
+}
+
 float app_pas_get_current_target_rel(void) {
 	return output_current_rel;
 }
 
 void pas_event_handler(void) {
 #ifdef HW_PAS1_PORT
 	const int8_t QEM[] = {0,-1,1,2,1,0,2,-1,-1,2,0,1,2,1,-1,0}; // Quadrature Encoder Matrix
-	float direction_qem;
+	int8_t direction_qem;
 	uint8_t new_state;
 	static uint8_t old_state = 0;
 	static float old_timestamp = 0;
 	static float inactivity_time = 0;
 	static float period_filtered = 0;
+	static int32_t correct_direction_counter = 0;
 
 	uint8_t PAS1_level = palReadPad(HW_PAS1_PORT, HW_PAS1_PIN);
 	uint8_t PAS2_level = palReadPad(HW_PAS2_PORT, HW_PAS2_PIN);
@@ -122,10 +133,19 @@ void pas_event_handler(void) {
 	direction_qem = (float) QEM[old_state * 4 + new_state];
 	old_state = new_state;
 
+	// Require several quadrature events in the right direction to prevent vibrations from
+	// engging PAS
+	int8_t direction = (direction_conf * direction_qem);
+	
+	switch(direction) {
+		case 1: correct_direction_counter++; break;
+		case -1:correct_direction_counter = 0; break;
+	}
+
 	const float timestamp = (float)chVTGetSystemTimeX() / (float)CH_CFG_ST_FREQUENCY;
 
 	// sensors are poorly placed, so use only one rising edge as reference
-	if(new_state == 3) {
+	if( (new_state == 3) && (correct_direction_counter >= 4) ) {
 		float period = (timestamp - old_timestamp) * (float)config.magnets;
 		old_timestamp = timestamp;
 
@@ -135,7 +155,7 @@ void pas_event_handler(void) {
 			return;
 		}
 		pedal_rpm = 60.0 / period_filtered;
-		pedal_rpm *= (direction_conf * direction_qem);
+		pedal_rpm *= (direction_conf * (float)direction_qem);
 		inactivity_time = 0.0;
 	}
 	else {
@@ -177,7 +197,7 @@ static THD_FUNCTION(pas_thread, arg) {
 			return;
 		}
 
-		pas_event_handler();	// this should happen inside an ISR instead of being polled
+		pas_event_handler();	// this could happen inside an ISR instead of being polled
 
 		// For safe start when fault codes occur
 		if (mc_interface_get_fault() != FAULT_CODE_NONE) {
@@ -188,28 +208,50 @@ static THD_FUNCTION(pas_thread, arg) {
 			continue;
 		}
 
-		// Map the rpm to assist level
 		switch (config.ctrl_type) {
 			case PAS_CTRL_TYPE_NONE:
 				output = 0.0;
 				break;
 			case PAS_CTRL_TYPE_CADENCE:
+				// Map pedal rpm to assist level
+
 				// NOTE: If the limits are the same a numerical instability is approached, so in that case
 				// just use on/off control (which is what setting the limits to the same value essentially means).
 				if (config.pedal_rpm_end > (config.pedal_rpm_start + 1.0)) {
-					output = utils_map(pedal_rpm, config.pedal_rpm_start, config.pedal_rpm_end, 0.0, config.current_scaling);
-					utils_truncate_number(&output, 0.0, config.current_scaling);
+					output = utils_map(pedal_rpm, config.pedal_rpm_start, config.pedal_rpm_end, 0.0, config.current_scaling * sub_scaling);
+					utils_truncate_number(&output, 0.0, config.current_scaling * sub_scaling);
 				} else {
 					if (pedal_rpm > config.pedal_rpm_end) {
-						output = config.current_scaling;
+						output = config.current_scaling * sub_scaling;
 					} else {
 						output = 0.0;
 					}
 				}
 				break;
-			case PAS_CTRL_TYPE_CONSTANT_TORQUE:
-				output = pedal_rpm > config.pedal_rpm_start ? config.current_scaling : 0;
-				break;
+
+#ifdef HW_HAS_PAS_TORQUE_SENSOR
+			case PAS_CTRL_TYPE_TORQUE:
+			{
+				torque_ratio = hw_get_PAS_torque();
+				output = torque_ratio * config.current_scaling * sub_scaling;
+				utils_truncate_number(&output, 0.0, config.current_scaling * sub_scaling);
+			}
+			/* fall through */
+			case PAS_CTRL_TYPE_TORQUE_WITH_CADENCE_TIMEOUT:
+			{
+				// disable assistance if torque has been sensed for >5sec without any pedal movement. Prevents
+				// motor overtemps when the rider is just resting on the pedals
+				static float ms_without_cadence_or_torque = 0.0;
+				if(output == 0.0 || pedal_rpm > 0) {
+					ms_without_cadence_or_torque = 0.0;
+				} else {
+					ms_without_cadence_or_torque += (1000.0 * (float)sleep_time) / (float)CH_CFG_ST_FREQUENCY;
+					if(ms_without_cadence_or_torque > MAX_MS_WITHOUT_CADENCE) {
+						output = 0.0;
+					}
+				}
+			}
+#endif
 			default:
 				break;
 		}
@@ -222,7 +264,7 @@ static THD_FUNCTION(pas_thread, arg) {
 		if (ramp_time > 0.01) {
 			const float ramp_step = (float)ST2MS(chVTTimeElapsedSinceX(last_time)) / (ramp_time * 1000.0);
 			utils_step_towards(&output_ramp, output, ramp_step);
-			utils_truncate_number(&output_ramp, 0.0, config.current_scaling);
+			utils_truncate_number(&output_ramp, 0.0, config.current_scaling * sub_scaling);
 
 			last_time = chVTGetSystemTimeX();
 			output = output_ramp;

datatypes.h

@@ -583,7 +583,8 @@ typedef enum {
 typedef enum {
 	PAS_CTRL_TYPE_NONE = 0,
 	PAS_CTRL_TYPE_CADENCE,
-	PAS_CTRL_TYPE_CONSTANT_TORQUE
+	PAS_CTRL_TYPE_TORQUE,
+	PAS_CTRL_TYPE_TORQUE_WITH_CADENCE_TIMEOUT
 } pas_control_type;
 
 // PAS sensor types