Moving nav state from commander to navigator, WIP

1 files changed, 7 insertions, 182 deletions

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index ca3ec94b8..731e0e3ff 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -46,7 +46,6 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
@@ -64,12 +63,10 @@ static const int ERROR = -1;
 
 static bool arming_state_changed = true;
 static bool main_state_changed = true;
-static bool navigation_state_changed = true;
 static bool flighttermination_state_changed = true;
 
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
-	const struct vehicle_control_mode_s *control_mode,
 	arming_state_t new_arming_state, struct actuator_armed_s *armed)
 {
 	/*
@@ -86,7 +83,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 	} else {
 
 		/* enforce lockdown in HIL */
-		if (control_mode->flag_system_hil_enabled) {
+		if (status->hil_state == HIL_STATE_ON) {
 			armed->lockdown = true;
 		} else {
 			armed->lockdown = false;
@@ -109,7 +106,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 			/* allow coming from INIT and disarming from ARMED */
 			if (status->arming_state == ARMING_STATE_INIT
 			    || status->arming_state == ARMING_STATE_ARMED
-			    || control_mode->flag_system_hil_enabled) {
+			    || status->hil_state == HIL_STATE_ON) {
 
 				/* sensors need to be initialized for STANDBY state */
 				if (status->condition_system_sensors_initialized) {
@@ -126,7 +123,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 			/* allow arming from STANDBY and IN-AIR-RESTORE */
 			if ((status->arming_state == ARMING_STATE_STANDBY
 			     || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
-			    && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */
+			    && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */
 				ret = TRANSITION_CHANGED;
 				armed->armed = true;
 				armed->ready_to_arm = true;
@@ -289,169 +286,6 @@ check_main_state_changed()
 	}
 }
 
-transition_result_t
-navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
-{
-	transition_result_t ret = TRANSITION_DENIED;
-
-	/* only check transition if the new state is actually different from the current one */
-	if (new_navigation_state == status->navigation_state) {
-		ret = TRANSITION_NOT_CHANGED;
-
-	} else {
-
-		switch (new_navigation_state) {
-		case NAVIGATION_STATE_DIRECT:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = false;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = false;
-			control_mode->flag_control_climb_rate_enabled = false;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_STABILIZE:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = false;
-			control_mode->flag_control_climb_rate_enabled = false;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_ALTHOLD:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_VECTOR:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = true;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_AUTO_READY:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = false;
-			control_mode->flag_control_attitude_enabled = false;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = false;
-			control_mode->flag_control_climb_rate_enabled = false;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
-
-		case NAVIGATION_STATE_AUTO_TAKEOFF:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
-
-		case NAVIGATION_STATE_AUTO_LOITER:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = false;
-			break;
-
-		case NAVIGATION_STATE_AUTO_MISSION:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
-
-		case NAVIGATION_STATE_AUTO_RTL:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = true;
-			control_mode->flag_control_attitude_enabled = true;
-			control_mode->flag_control_velocity_enabled = true;
-			control_mode->flag_control_position_enabled = true;
-			control_mode->flag_control_altitude_enabled = true;
-			control_mode->flag_control_climb_rate_enabled = true;
-			control_mode->flag_control_manual_enabled = false;
-			control_mode->flag_control_auto_enabled = true;
-			break;
-
-		case NAVIGATION_STATE_AUTO_LAND:
-
-			/* deny transitions from landed state */
-			if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
-				ret = TRANSITION_CHANGED;
-				control_mode->flag_control_rates_enabled = true;
-				control_mode->flag_control_attitude_enabled = true;
-				control_mode->flag_control_velocity_enabled = true;
-				control_mode->flag_control_position_enabled = true;
-				control_mode->flag_control_altitude_enabled = true;
-				control_mode->flag_control_climb_rate_enabled = true;
-				control_mode->flag_control_manual_enabled = false;
-				control_mode->flag_control_auto_enabled = true;
-			}
-
-			break;
-
-		default:
-			break;
-		}
-
-		if (ret == TRANSITION_CHANGED) {
-			status->navigation_state = new_navigation_state;
-			control_mode->auto_state = status->navigation_state;
-			navigation_state_changed = true;
-		}
-	}
-
-	return ret;
-}
-
-bool
-check_navigation_state_changed()
-{
-	if (navigation_state_changed) {
-		navigation_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
 bool
 check_flighttermination_state_changed()
 {
@@ -464,16 +298,10 @@ check_flighttermination_state_changed()
 	}
 }
 
-void
-set_navigation_state_changed()
-{
-	navigation_state_changed = true;
-}
-
 /**
 * Transition from one hil state to another
 */
-int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd)
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	bool valid_transition = false;
 	int ret = ERROR;
@@ -502,7 +330,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 			    || current_status->arming_state == ARMING_STATE_STANDBY
 			    || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
 
-				current_control_mode->flag_system_hil_enabled = true;
 				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
 				valid_transition = true;
 			}
@@ -521,9 +348,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 		current_status->timestamp = hrt_absolute_time();
 		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
 
-		current_control_mode->timestamp = hrt_absolute_time();
-		orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
-
 		// XXX also set lockdown here
 
 		ret = OK;
@@ -539,7 +363,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 /**
 * Transition from one flightermination state to another
 */
-transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode)
+transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
@@ -566,7 +390,8 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s *
 
 			if (ret == TRANSITION_CHANGED) {
 				flighttermination_state_changed = true;
-				control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
+				// TODO
+				//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
 			}
 		}