WIP - Weird control structure

quad/sw/modular_quad_pid/src/control_algorithm.c

@@ -23,15 +23,20 @@ void connect_autonomous(parameter_t* ps) {
 	graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->clamp_pitch, PID_CORRECTION);
 	graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->clamp_roll, PID_CORRECTION);
 	graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
-	graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
+	//graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
 }
 
 void connect_manual(parameter_t* ps) {
 	struct computation_graph* graph = ps->graph;
+	/*
 	graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->rc_pitch, CONST_VAL);
 	graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL);
 	graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
 	graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, CONST_VAL);
+	*/
+	connect_autonomous(ps);
+	graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, CONST_VAL);
+	graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
 }
 
 int control_algorithm_init(parameter_t * ps)
@@ -216,8 +221,12 @@ int control_algorithm_init(parameter_t * ps)
 	// reset flight_mode to MANUAL right away if the flap switch is in manual position
 	// to engage AUTO mode the code waits for a new packet after the flap is switched to auto
 	// before actually engaging AUTO mode
-	if(cur_fm_switch == MANUAL_FLIGHT_MODE)
+	if(cur_fm_switch == MANUAL_FLIGHT_MODE) {
+		if (user_defined_struct->flight_mode == MANUAL_FLIGHT_MODE) {
+			connect_manual(parameter_struct);
+		}
 		user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
+	}
 
 	// flap switch was just toggled to auto flight mode
 	if((last_fm_switch != cur_fm_switch) && (cur_fm_switch == AUTO_FLIGHT_MODE))
@@ -251,6 +260,7 @@ int control_algorithm_init(parameter_t * ps)
 		// finally engage the AUTO_FLIGHT_MODE
 		// this ensures that we've gotten a new update packet right after the switch was set to auto mode
 		user_defined_struct->flight_mode = AUTO_FLIGHT_MODE;
+		connect_autonomous(parameter_struct);
 	}
 
 	//PIDS///////////////////////////////////////////////////////////////////////
@@ -287,9 +297,9 @@ int control_algorithm_init(parameter_t * ps)
 	//memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);
 
 	// don't use the PID corrections if the throttle is less than about 10% of its range
-	if((user_input_struct->rc_commands[THROTTLE] >
-	118000) || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE))
-	{
+	//if((user_input_struct->rc_commands[THROTTLE] >
+	//118000) || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE))
+	//{
 			//THROTTLE
 			actuator_struct->pwms[0] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM0);
 
@@ -298,14 +308,14 @@ int control_algorithm_init(parameter_t * ps)
 			actuator_struct->pwms[2] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM2);
 
 			actuator_struct->pwms[3] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM3);
-	}
-	else
+	//}
+	/*else
 	{
 		actuator_struct->pwms[0] = user_input_struct->rc_commands[THROTTLE];
 		actuator_struct->pwms[1] = user_input_struct->rc_commands[THROTTLE];
 		actuator_struct->pwms[2] = user_input_struct->rc_commands[THROTTLE];
 		actuator_struct->pwms[3] = user_input_struct->rc_commands[THROTTLE];
-	}
+	}*/
 
 	//logging
 	// here we are not actually duplicating the logging from the PID computation