mtecs publish state

4 files changed, 15 insertions, 13 deletions

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index e43469b70..7fd7ed07f 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -386,7 +386,7 @@ private:
 			bool climbout_mode, float climbout_pitch_min_rad,
 			float altitude,
 			const math::Vector<3> &ground_speed,
-			fwPosctrl::mTecs::tecs_mode mode = fwPosctrl::mTecs::TECS_MODE_NORMAL);
+			tecs_mode mode = TECS_MODE_NORMAL);
 
 };
 
@@ -1018,7 +1018,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						0.0f, throttle_max, throttle_land,
 						false, flare_pitch_angle_rad,
 						_pos_sp_triplet.current.alt + relative_alt, ground_speed,
-						land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND);
+						land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
 
 				if (!land_noreturn_vertical) {
 					mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
@@ -1111,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 							math::radians(10.0f)),
 							_global_pos.alt,
 							ground_speed,
-							fwPosctrl::mTecs::TECS_MODE_TAKEOFF);
+							TECS_MODE_TAKEOFF);
 
 					/* limit roll motion to ensure enough lift */
 					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
@@ -1449,7 +1449,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		bool climbout_mode, float climbout_pitch_min_rad,
 		float altitude,
 		const math::Vector<3> &ground_speed,
-		fwPosctrl::mTecs::tecs_mode mode)
+		tecs_mode mode)
 {
 	if (_mTecs.getEnabled()) {
 		/* Using mtecs library: prepare arguments for mtecs call */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index d370bf906..039fc34a8 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -224,6 +224,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	_status.totalEnergyRate = totalEnergyRate;
 	_status.energyDistributionRateSp = energyDistributionRateSp;
 	_status.energyDistributionRate = energyDistributionRate;
+	_status.mode = mode;
 
 	/** update control blocks **/
 	/* update total energy rate control block */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index 1a787df72..b7d4af0f9 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -60,15 +60,6 @@ public:
 	mTecs();
 	virtual ~mTecs();
 
-	typedef enum {
-		TECS_MODE_NORMAL,
-		TECS_MODE_UNDERSPEED,
-		TECS_MODE_TAKEOFF,
-		TECS_MODE_LAND,
-		TECS_MODE_LAND_THROTTLELIM
-	} tecs_mode;
-
-
 	/* A small class which provides helper fucntions to override control output limits which are usually set by
 	 * parameters in special cases
 	 */
diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h
index f3d33ec20..fc530b295 100644
--- a/src/modules/uORB/topics/tecs_status.h
+++ b/src/modules/uORB/topics/tecs_status.h
@@ -50,6 +50,14 @@
  * @{
  */
 
+typedef enum {
+	TECS_MODE_NORMAL,
+	TECS_MODE_UNDERSPEED,
+	TECS_MODE_TAKEOFF,
+	TECS_MODE_LAND,
+	TECS_MODE_LAND_THROTTLELIM
+} tecs_mode;
+
  /**
  * Internal values of the (m)TECS fixed wing speed alnd altitude control system
  */
@@ -69,6 +77,8 @@ struct tecs_status_s {
 	float totalEnergyRate;
 	float energyDistributionRateSp;
 	float energyDistributionRate;
+
+	tecs_mode mode;
 };
 
 /**