diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ecdab84e7ad5661bec03e61488c4f146317ca19d..964c1f2ce8d7b1cca359b83bd85391ac6ba2a828 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -139,6 +139,8 @@ private: vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */ landing_gear_s _landing_gear_state{}; + int8_t _old_landing_gear_position; + DEFINE_PARAMETERS( (ParamFloat<px4::params::MPC_TKO_RAMP_T>) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */ (ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _vel_max_up, @@ -810,25 +812,24 @@ MulticopterPositionControl::run() // they might conflict with each other such as in offboard attitude control. publish_attitude(); - if (!constraints.landing_gear) { - if (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) { - _landing_gear_state.landing_gear = landing_gear_s::LANDING_GEAR_UP; - } - - if (constraints.landing_gear == vehicle_constraints_s::GEAR_DOWN) { - _landing_gear_state.landing_gear = landing_gear_s::LANDING_GEAR_DOWN; - } - } + if (_old_landing_gear_position != constraints.landing_gear) { + _landing_gear_state.landing_gear = + (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) ? + landing_gear_s::LANDING_GEAR_UP : + landing_gear_s::LANDING_GEAR_DOWN; - _landing_gear_state.timestamp = hrt_absolute_time(); + _landing_gear_state.timestamp = hrt_absolute_time(); - if (_landing_gear_pub != nullptr) { - orb_publish(ORB_ID(landing_gear), _landing_gear_pub, &_landing_gear_state); + if (_landing_gear_pub != nullptr) { + orb_publish(ORB_ID(landing_gear), _landing_gear_pub, &_landing_gear_state); - } else { - _landing_gear_pub = orb_advertise(ORB_ID(landing_gear), &_landing_gear_state); + } else { + _landing_gear_pub = orb_advertise(ORB_ID(landing_gear), &_landing_gear_state); + } } + _old_landing_gear_position = constraints.landing_gear; + } else { // no flighttask is active: set attitude setpoint to idle _att_sp.roll_body = _att_sp.pitch_body = 0.0f;