From ee41e3181f83e08b5bd80f6a7ae67626aed6043a Mon Sep 17 00:00:00 2001 From: Simone Guscetti Date: Thu, 1 Nov 2018 11:06:56 +0100 Subject: [PATCH] mc_pos_ctrl: Replace landing gear logic - It takes the input from the constrains from the active flight task --- .../mc_pos_control/mc_pos_control_main.cpp | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) 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 ecdab84e7a..964c1f2ce8 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) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */ (ParamFloat) _vel_max_up, @@ -810,24 +812,23 @@ 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 (_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; - if (constraints.landing_gear == vehicle_constraints_s::GEAR_DOWN) { - _landing_gear_state.landing_gear = landing_gear_s::LANDING_GEAR_DOWN; + _landing_gear_state.timestamp = hrt_absolute_time(); + + 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); } } - _landing_gear_state.timestamp = hrt_absolute_time(); - - 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); - } + _old_landing_gear_position = constraints.landing_gear; } else { // no flighttask is active: set attitude setpoint to idle