Checkpoint: implement new state machine, compiling, WIP

1 files changed, 43 insertions, 41 deletions

diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index d94c0a69c..da7550f79 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -296,38 +296,39 @@ mc_thread_main(int argc, char *argv[])
 							}
 
 							/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
-							if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
-							    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-
-								if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
-									rates_sp.yaw = manual.yaw;
-									control_yaw_position = false;
-
-								} else {
-									/*
-									 * This mode SHOULD be the default mode, which is:
-									 * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
-									 *
-									 * However, we fall back to this setting for all other (nonsense)
-									 * settings as well.
-									 */
-
-									/* only move setpoint if manual input is != 0 */
-									if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
-										rates_sp.yaw = manual.yaw;
-										control_yaw_position = false;
-										first_time_after_yaw_speed_control = true;
-
-									} else {
-										if (first_time_after_yaw_speed_control) {
-											att_sp.yaw_body = att.yaw;
-											first_time_after_yaw_speed_control = false;
-										}
-
-										control_yaw_position = true;
-									}
-								}
-							}
+#warning fix this
+//							if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
+//							    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+//
+//								if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
+//									rates_sp.yaw = manual.yaw;
+//									control_yaw_position = false;
+//
+//								} else {
+//									/*
+//									 * This mode SHOULD be the default mode, which is:
+//									 * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
+//									 *
+//									 * However, we fall back to this setting for all other (nonsense)
+//									 * settings as well.
+//									 */
+//
+//									/* only move setpoint if manual input is != 0 */
+//									if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
+//										rates_sp.yaw = manual.yaw;
+//										control_yaw_position = false;
+//										first_time_after_yaw_speed_control = true;
+//
+//									} else {
+//										if (first_time_after_yaw_speed_control) {
+//											att_sp.yaw_body = att.yaw;
+//											first_time_after_yaw_speed_control = false;
+//										}
+//
+//										control_yaw_position = true;
+//									}
+//								}
+//							}
 
 							att_sp.thrust = manual.throttle;
 							att_sp.timestamp = hrt_absolute_time();
@@ -348,16 +349,17 @@ mc_thread_main(int argc, char *argv[])
 						}
 
 					} else {
+#warning fix this
 						/* manual rate inputs, from RC control or joystick */
-						if (state.flag_control_rates_enabled &&
-						    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
-							rates_sp.roll = manual.roll;
-
-							rates_sp.pitch = manual.pitch;
-							rates_sp.yaw = manual.yaw;
-							rates_sp.thrust = manual.throttle;
-							rates_sp.timestamp = hrt_absolute_time();
-						}
+//						if (state.flag_control_rates_enabled &&
+//						    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
+//							rates_sp.roll = manual.roll;
+//
+//							rates_sp.pitch = manual.pitch;
+//							rates_sp.yaw = manual.yaw;
+//							rates_sp.thrust = manual.throttle;
+//							rates_sp.timestamp = hrt_absolute_time();
+//						}
 					}
 
 				}