Use ADC for switches

appconf/appconf_default.h

@@ -289,8 +289,11 @@
 #ifndef APPCONF_BALANCE_ROLL_FAULT
 #define APPCONF_BALANCE_ROLL_FAULT  		45
 #endif
-#ifndef APPCONF_BALANCE_USE_SWITCHES
-#define APPCONF_BALANCE_USE_SWITCHES  		false
+#ifndef APPCONF_BALANCE_ADC1
+#define APPCONF_BALANCE_ADC1  				0.0
+#endif
+#ifndef APPCONF_BALANCE_ADC2
+#define APPCONF_BALANCE_ADC2  				0.0
 #endif
 #ifndef APPCONF_BALANCE_OVERSPEED_DUTY
 #define APPCONF_BALANCE_OVERSPEED_DUTY  	0.9

applications/app.h

@@ -65,7 +65,9 @@ uint32_t app_balance_get_diff_time(void);
 float app_balance_get_motor_current(void);
 float app_balance_get_motor_position(void);
 uint16_t app_balance_get_state(void);
-uint16_t app_balance_get_switch_value(void);
+uint16_t app_balance_get_switch_state(void);
+float app_balance_get_adc1(void);
+float app_balance_get_adc2(void);
 
 // Custom apps
 void app_custom_start(void);

applications/app_balance.c

@@ -50,6 +50,12 @@ typedef enum {
 	TILTBACK
 } SetpointAdjustmentType;
 
+typedef enum {
+	OFF = 0,
+	HALF,
+	ON
+} SwitchState;
+
 // Balance thread
 static THD_FUNCTION(balance_thread, arg);
 static THD_WORKING_AREA(balance_thread_wa, 2048); // 2kb stack for this thread
@@ -74,7 +80,8 @@ static systime_t current_time, last_time, diff_time;
 static systime_t startup_start_time, startup_diff_time;
 static systime_t dead_start_time;
 static systime_t fault_start_time;
-static uint16_t switches_value;
+static float adc1, adc2;
+static SwitchState switch_state;
 
 // Values read to pass in app data to GUI
 static float motor_current;
@@ -96,7 +103,9 @@ void app_balance_start(void) {
 	gyro[2] = 0;
 	duty_cycle = 0;
 	abs_duty_cycle = 0;
-	switches_value = 0;
+	adc1 = 0;
+	adc2 = 0;
+	switch_state = OFF;
 	proportional = 0;
 	integral = 0;
 	derivative = 0;
@@ -113,14 +122,6 @@ void app_balance_start(void) {
 	startup_start_time = 0;
 	startup_diff_time = 0;
 
-#ifdef HW_SPI_PORT_SCK
-	// Configure pins
-	if(balance_conf.use_switches){
-		palSetPadMode(HW_SPI_PORT_SCK, HW_SPI_PIN_SCK, PAL_MODE_INPUT_PULLDOWN);
-		palSetPadMode(HW_SPI_PORT_MISO, HW_SPI_PIN_MISO, PAL_MODE_INPUT_PULLDOWN);
-	}
-#endif
-
 	// Start the balance thread
 	app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL);
 }
@@ -146,8 +147,14 @@ float app_balance_get_motor_position(void) {
 uint16_t app_balance_get_state(void) {
 	return state;
 }
-uint16_t app_balance_get_switch_value(void) {
-	return switches_value;
+uint16_t app_balance_get_switch_state(void) {
+	return switch_state;
+}
+float app_balance_get_adc1(void) {
+	return adc1;
+}
+float app_balance_get_adc2(void) {
+	return adc2;
 }
 
 float get_setpoint_adjustment_step_size(void){
@@ -244,21 +251,39 @@ static THD_FUNCTION(balance_thread, arg) {
 		imu_get_gyro(gyro);
 		duty_cycle = mc_interface_get_duty_cycle_now();
 		abs_duty_cycle = fabsf(duty_cycle);
+		adc1 = (((float)ADC_Value[ADC_IND_EXT])/4095) * V_REG;
+#ifdef ADC_IND_EXT2
+		adc2 = (((float)ADC_Value[ADC_IND_EXT2])/4095) * V_REG;
+#else
+		adc2 = 0.0;
+#endif
 
-		if(!balance_conf.use_switches){
-			switches_value = 2;
-		}else{
-			switches_value = 0;
-#ifdef HW_SPI_PORT_SCK
-			if(palReadPad(HW_SPI_PORT_SCK, HW_SPI_PIN_SCK)){
-				switches_value += 1;
+		// Calculate switch state from ADC values
+		if(balance_conf.adc1 == 0 && balance_conf.adc2 == 0){ // No Switch
+			switch_state = ON;
+		}else if(balance_conf.adc2 == 0){ // Single switch on ADC1
+			if(adc1 > balance_conf.adc1){
+				switch_state = ON;
+			} else {
+				switch_state = OFF;
 			}
-			if(palReadPad(HW_SPI_PORT_MISO, HW_SPI_PIN_MISO)){
-				switches_value += 1;
+		}else if(balance_conf.adc1 == 0){ // Single switch on ADC2
+			if(adc2 > balance_conf.adc2){
+				switch_state = ON;
+			} else {
+				switch_state = OFF;
+			}
+		}else{ // Double switch
+			if(adc1 > balance_conf.adc1 && adc2 > balance_conf.adc2){
+				switch_state = ON;
+			}else if(adc1 > balance_conf.adc1 || adc2 > balance_conf.adc2){
+				switch_state = HALF;
+			}else{
+				switch_state = OFF;
 			}
-#endif
 		}
 
+
 		// State based logic
 		switch(state){
 			case (STARTUP):
@@ -286,8 +311,8 @@ static THD_FUNCTION(balance_thread, arg) {
 				if(
 					fabsf(pitch_angle) > balance_conf.pitch_fault || // Balnce axis tip over
 					fabsf(roll_angle) > balance_conf.roll_fault || // Cross axis tip over
-					app_balance_get_switch_value() == 0 || // Switch fully open
-					(app_balance_get_switch_value() == 1 && abs_duty_cycle < 0.003) // Switch partially open and stopped
+					switch_state == OFF || // Switch fully open
+					(switch_state == HALF && abs_duty_cycle < 0.003) // Switch partially open and stopped
 						){
 					if(ST2MS(current_time - fault_start_time) > balance_conf.fault_delay){
 						state = FAULT;
@@ -371,7 +396,7 @@ static THD_FUNCTION(balance_thread, arg) {
 				break;
 			case (FAULT):
 				// Check for valid startup position and switch state
-				if(fabsf(pitch_angle) < balance_conf.startup_pitch_tolerance && fabsf(roll_angle) < balance_conf.startup_roll_tolerance && app_balance_get_switch_value() == 2){
+				if(fabsf(pitch_angle) < balance_conf.startup_pitch_tolerance && fabsf(roll_angle) < balance_conf.startup_roll_tolerance && switch_state == ON){
 					setpoint = pitch_angle;
 					setpoint_target = 0;
 					setpointAdjustmentType = CENTERING;

commands.c

@@ -547,7 +547,9 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 		buffer_append_int32(send_buffer, (int32_t)(app_balance_get_motor_current() * 1000000.0), &ind);
 		buffer_append_int32(send_buffer, (int32_t)(app_balance_get_motor_position() * 1000000.0), &ind);
 		buffer_append_uint16(send_buffer, app_balance_get_state(), &ind);
-		buffer_append_uint16(send_buffer, app_balance_get_switch_value(), &ind);
+		buffer_append_uint16(send_buffer, app_balance_get_switch_state(), &ind);
+		buffer_append_int32(send_buffer, (int32_t)(app_balance_get_adc1() * 1000000.0), &ind);
+		buffer_append_int32(send_buffer, (int32_t)(app_balance_get_adc2() * 1000000.0), &ind);
 		reply_func(send_buffer, ind);
 	} break;
 

confgenerator.c

@@ -240,7 +240,8 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
 	buffer_append_uint16(buffer, conf->app_balance_conf.hertz, &ind);
 	buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_fault, &ind);
 	buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_fault, &ind);
-	buffer[ind++] = conf->app_balance_conf.use_switches;
+	buffer_append_float32_auto(buffer, conf->app_balance_conf.adc1, &ind);
+	buffer_append_float32_auto(buffer, conf->app_balance_conf.adc2, &ind);
 	buffer_append_float32_auto(buffer, conf->app_balance_conf.overspeed_duty, &ind);
 	buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_duty, &ind);
 	buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_angle, &ind);
@@ -527,7 +528,8 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
 	conf->app_balance_conf.hertz = buffer_get_uint16(buffer, &ind);
 	conf->app_balance_conf.pitch_fault = buffer_get_float32_auto(buffer, &ind);
 	conf->app_balance_conf.roll_fault = buffer_get_float32_auto(buffer, &ind);
-	conf->app_balance_conf.use_switches = buffer[ind++];
+	conf->app_balance_conf.adc1 = buffer_get_float32_auto(buffer, &ind);
+	conf->app_balance_conf.adc2 = buffer_get_float32_auto(buffer, &ind);
 	conf->app_balance_conf.overspeed_duty = buffer_get_float32_auto(buffer, &ind);
 	conf->app_balance_conf.tiltback_duty = buffer_get_float32_auto(buffer, &ind);
 	conf->app_balance_conf.tiltback_angle = buffer_get_float32_auto(buffer, &ind);
@@ -798,7 +800,8 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
 	conf->app_balance_conf.hertz = APPCONF_BALANCE_HERTZ;
 	conf->app_balance_conf.pitch_fault = APPCONF_BALANCE_PITCH_FAULT;
 	conf->app_balance_conf.roll_fault = APPCONF_BALANCE_ROLL_FAULT;
-	conf->app_balance_conf.use_switches = APPCONF_BALANCE_USE_SWITCHES;
+	conf->app_balance_conf.adc1 = APPCONF_BALANCE_ADC1;
+	conf->app_balance_conf.adc2 = APPCONF_BALANCE_ADC2;
 	conf->app_balance_conf.overspeed_duty = APPCONF_BALANCE_OVERSPEED_DUTY;
 	conf->app_balance_conf.tiltback_duty = APPCONF_BALANCE_TILTBACK_DUTY;
 	conf->app_balance_conf.tiltback_angle = APPCONF_BALANCE_TILTBACK_ANGLE;

datatypes.h

@@ -545,7 +545,8 @@ typedef struct {
 	uint16_t hertz;
 	float pitch_fault;
 	float roll_fault;
-	bool use_switches;
+	float adc1;
+	float adc2;
 	float overspeed_duty;
 	float tiltback_duty;
 	float tiltback_angle;