FlightModeManager: publish takeoff status

msg/CMakeLists.txt

@@ -131,6 +131,7 @@ set(msg_files
 	tecs_status.msg
 	telemetry_status.msg
 	test_motor.msg
+	takeoff_status.msg
 	timesync.msg
 	timesync_status.msg
 	trajectory_bezier.msg

msg/takeoff_status.msg

@@ -0,0 +1,11 @@
+# Status of the takeoff state machine currently just availble for multicopters
+
+uint64 timestamp # time since system start (microseconds)
+
+uint8 TAKEOFF_STATE_DISARMED = 0
+uint8 TAKEOFF_STATE_SPOOLUP = 1
+uint8 TAKEOFF_STATE_READY_FOR_TAKEOFF = 2
+uint8 TAKEOFF_STATE_RAMPUP = 3
+uint8 TAKEOFF_STATE_FLIGHT = 4
+
+uint8 takeoff_state

msg/tools/uorb_rtps_message_ids.yaml

@@ -310,6 +310,8 @@ rtps:
     id: 147
   - msg: mag_worker_data
     id: 148
+  - msg: takeoff_status
+    id: 149
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 170

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -577,6 +577,16 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 	}
 
 	_old_landing_gear_position = landing_gear.landing_gear;
+
+	// Publish takeoff status
+	takeoff_status_s takeoff_status;
+	takeoff_status.takeoff_state = static_cast<uint8_t>(_takeoff.getTakeoffState());
+
+	if (takeoff_status.takeoff_state != _old_takeoff_state) {
+		takeoff_status.timestamp = hrt_absolute_time();
+		_takeoff_status_pub.publish(takeoff_status);
+		_old_takeoff_state = takeoff_status.takeoff_state;
+	}
 }
 
 void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoint,

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -44,7 +44,9 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/Publication.hpp>
+#include <uORB/topics/landing_gear.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/takeoff_status.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
@@ -134,6 +136,7 @@ private:
 	Takeoff _takeoff; ///< state machine and ramp to bring the vehicle off the ground without a jump
 	WeatherVane *_wv_controller{nullptr};
 	int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
+	uint8_t _old_takeoff_state{0};
 	int _task_failure_count{0};
 	uint8_t _last_vehicle_nav_state{0};
 
@@ -153,6 +156,7 @@ private:
 	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
+	uORB::Publication<takeoff_status_s> _takeoff_status_pub{ORB_ID(takeoff_status)};
 	uORB::Publication<vehicle_local_position_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
 	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};

src/modules/flight_mode_manager/Takeoff/Takeoff.hpp

@@ -41,15 +41,16 @@
 
 #include <lib/hysteresis/hysteresis.h>
 #include <drivers/drv_hrt.h>
+#include <uORB/topics/takeoff_status.h>
 
 using namespace time_literals;
 
 enum class TakeoffState {
-	disarmed = 0,
-	spoolup,
-	ready_for_takeoff,
-	rampup,
-	flight
+	disarmed = takeoff_status_s::TAKEOFF_STATE_DISARMED,
+	spoolup = takeoff_status_s::TAKEOFF_STATE_SPOOLUP,
+	ready_for_takeoff = takeoff_status_s::TAKEOFF_STATE_READY_FOR_TAKEOFF,
+	rampup = takeoff_status_s::TAKEOFF_STATE_RAMPUP,
+	flight = takeoff_status_s::TAKEOFF_STATE_FLIGHT
 };
 
 class Takeoff