Moving nav state from commander to navigator, WIP

2 files changed, 18 insertions, 19 deletions

diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index e26fb97c8..5ce968f67 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -48,6 +48,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
+#include "vehicle_status.h"
 
 /**
  * @addtogroup topics @{
@@ -59,10 +60,25 @@
  *
  * Encodes the complete system state and is set by the commander app.
  */
+
+typedef enum {
+	NAV_STATE_INIT = 0,
+	NAV_STATE_NONE,
+	NAV_STATE_LOITER,
+	NAV_STATE_MISSION,
+	NAV_STATE_MISSION_LOITER,
+	NAV_STATE_RTL,
+	NAV_STATE_RTL_LOITER,
+	NAV_STATE_MAX
+} nav_state_t;
+
 struct vehicle_control_mode_s
 {
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
+	main_state_t main_state;
+	nav_state_t nav_state;
+
 	bool flag_armed;
 
 	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
@@ -79,9 +95,6 @@ struct vehicle_control_mode_s
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
 	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
 	bool flag_control_flighttermination_enabled;   /**< true if flighttermination is enabled */
-
-	bool flag_control_auto_enabled;		// TEMP
-	uint8_t auto_state;	// TEMP navigation state for AUTO modes
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 1c3763924..ae3a24a1b 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -66,20 +66,6 @@ typedef enum {
 	MAIN_STATE_AUTO,
 } main_state_t;
 
-/* navigation state machine */
-typedef enum {
-	NAVIGATION_STATE_DIRECT = 0,		// true manual control, no any stabilization
-	NAVIGATION_STATE_STABILIZE,		// attitude stabilization
-	NAVIGATION_STATE_ALTHOLD,		// attitude + altitude stabilization
-	NAVIGATION_STATE_VECTOR,		// attitude + altitude + position stabilization
-	NAVIGATION_STATE_AUTO_READY,	// AUTO, landed, reeady for takeoff
-	NAVIGATION_STATE_AUTO_TAKEOFF,	// detect takeoff using land detector and switch to desired AUTO mode
-	NAVIGATION_STATE_AUTO_LOITER,	// pause mission
-	NAVIGATION_STATE_AUTO_MISSION,	// fly mission
-	NAVIGATION_STATE_AUTO_RTL,		// Return To Launch, when home position switch to LAND
-	NAVIGATION_STATE_AUTO_LAND		// land and switch to AUTO_READY when landed (detect using land detector)
-} navigation_state_t;
-
 typedef enum {
 	ARMING_STATE_INIT = 0,
 	ARMING_STATE_STANDBY,
@@ -95,7 +81,6 @@ typedef enum {
 	HIL_STATE_ON
 } hil_state_t;
 
-
 typedef enum {
 	FLIGHTTERMINATION_STATE_OFF = 0,
 	FLIGHTTERMINATION_STATE_ON
@@ -182,7 +167,8 @@ struct vehicle_status_s
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	main_state_t main_state;				/**< main state machine */
-	navigation_state_t navigation_state;	/**< navigation state machine */
+	unsigned int set_nav_state;	/**< set navigation state machine to specified value */
+	uint64_t set_nav_state_timestamp;	/**< timestamp of latest change of set_nav_state */
 	arming_state_t arming_state;			/**< current arming state */
 	hil_state_t hil_state;					/**< current hil state */