diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index effd0c144c..64cfd389d4 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/takeoff_status.msg b/msg/takeoff_status.msg new file mode 100644 index 0000000000..346de764eb --- /dev/null +++ b/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 diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 69dabf85c1..55f82d7994 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/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 diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 1d668e1d6f..9481062d34 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/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(_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, diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index 04b67278e4..8c1278dcbe 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -44,7 +44,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -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_sub{ORB_ID(vehicle_status)}; uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; + uORB::Publication _takeoff_status_pub{ORB_ID(takeoff_status)}; uORB::Publication _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)}; uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; diff --git a/src/modules/flight_mode_manager/Takeoff/Takeoff.hpp b/src/modules/flight_mode_manager/Takeoff/Takeoff.hpp index 5253164c88..d396d19859 100644 --- a/src/modules/flight_mode_manager/Takeoff/Takeoff.hpp +++ b/src/modules/flight_mode_manager/Takeoff/Takeoff.hpp @@ -41,15 +41,16 @@ #include #include +#include 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