diff --git a/msg/takeoff_status.msg b/msg/takeoff_status.msg index 9b55b08f4f..ed2f906d77 100644 --- a/msg/takeoff_status.msg +++ b/msg/takeoff_status.msg @@ -10,3 +10,5 @@ uint8 TAKEOFF_STATE_RAMPUP = 4 uint8 TAKEOFF_STATE_FLIGHT = 5 uint8 takeoff_state + +float32 tilt_limit # limited tilt feasability during takeoff, contains maximum tilt otherwise diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index ceeba50453..68c037b30a 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -461,13 +461,12 @@ void MulticopterPositionControl::Run() // Publish takeoff status const uint8_t takeoff_state = static_cast(_takeoff.getTakeoffState()); - if (takeoff_state != _old_takeoff_state) { - takeoff_status_s takeoff_status{}; - takeoff_status.takeoff_state = takeoff_state; - takeoff_status.timestamp = hrt_absolute_time(); - _takeoff_status_pub.publish(takeoff_status); - - _old_takeoff_state = takeoff_state; + if (takeoff_state != _takeoff_status_pub.get().takeoff_state + || !isEqualF(_tilt_limit_slew_rate.getState(), _takeoff_status_pub.get().tilt_limit)) { + _takeoff_status_pub.get().takeoff_state = takeoff_state; + _takeoff_status_pub.get().tilt_limit = _tilt_limit_slew_rate.getState(); + _takeoff_status_pub.get().timestamp = hrt_absolute_time(); + _takeoff_status_pub.update(); } // save latest reset counters diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index eff575bcb1..3b0f13228a 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -92,7 +92,7 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; - uORB::Publication _takeoff_status_pub{ORB_ID(takeoff_status)}; + uORB::PublicationData _takeoff_status_pub{ORB_ID(takeoff_status)}; uORB::Publication _vehicle_attitude_setpoint_pub; uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ @@ -200,8 +200,6 @@ private: systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ SlewRate _tilt_limit_slew_rate; - uint8_t _old_takeoff_state{}; - uint8_t _vxy_reset_counter{0}; uint8_t _vz_reset_counter{0}; uint8_t _xy_reset_counter{0};