From 16594bffa9ce2cc79421e64e89b3e987582a7ca9 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 19 Dec 2022 15:36:31 +0100 Subject: [PATCH] Rework landing gear logic - remove deprecated actuator_controls[INDEX_LANDING_GEAR] - remove dead code in mc rate controller that used to prevent it from being retracted on the ground (anyway had no effect as it only affected the actuator_control[LANDING_GEAR] which wasn't sent to the control allocation) - for VTOLs handle deployment/retraction of landing gear in AUTO as a MC (retract if more than 2m above ground, deploy if WP is a landing WP), plus additionally when transition flight task is called (ALTITUDE mode and higher) - for FW in AUTO: add logic in FW Position Controller, depending on waypoint type mainly - manual landing gear settings always come through (a manual command overrides a previous auto command, and vice-versa) Signed-off-by: Silvan Fuhrer --- msg/ActuatorControls.msg | 1 - .../tasks/Descend/FlightTaskDescend.cpp | 7 ++++++ .../tasks/Descend/FlightTaskDescend.hpp | 1 + .../tasks/Transition/FlightTaskTransition.cpp | 16 ++++++++++--- .../tasks/Transition/FlightTaskTransition.hpp | 4 ++++ .../FixedwingPositionControl.cpp | 23 +++++++++++++++++++ .../FixedwingPositionControl.hpp | 5 ++++ .../fw_rate_control/FixedwingRateControl.cpp | 2 -- src/modules/logger/logged_topics.cpp | 1 + .../MulticopterRateControl.cpp | 19 --------------- .../MulticopterRateControl.hpp | 6 ----- src/modules/vtol_att_control/standard.cpp | 4 ---- src/modules/vtol_att_control/tailsitter.cpp | 10 -------- src/modules/vtol_att_control/tiltrotor.cpp | 10 -------- 14 files changed, 54 insertions(+), 55 deletions(-) diff --git a/msg/ActuatorControls.msg b/msg/ActuatorControls.msg index 89580b22e7..ef3d13861d 100644 --- a/msg/ActuatorControls.msg +++ b/msg/ActuatorControls.msg @@ -8,7 +8,6 @@ uint8 INDEX_THROTTLE = 3 uint8 INDEX_FLAPS = 4 uint8 INDEX_SPOILERS = 5 uint8 INDEX_AIRBRAKES = 6 -uint8 INDEX_LANDING_GEAR = 7 uint8 INDEX_GIMBAL_SHUTTER = 3 uint8 INDEX_CAMERA_ZOOM = 4 diff --git a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp index d07570e6ad..3ebc783581 100644 --- a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp +++ b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp @@ -36,6 +36,13 @@ #include "FlightTaskDescend.hpp" +bool FlightTaskDescend::activate(const trajectory_setpoint_s &last_setpoint) +{ + bool ret = FlightTask::activate(last_setpoint); + _gear.landing_gear = landing_gear_s::GEAR_DOWN; + return ret; +} + bool FlightTaskDescend::update() { bool ret = FlightTask::update(); diff --git a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp index f07fb9601d..e341766104 100644 --- a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp +++ b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp @@ -49,6 +49,7 @@ public: virtual ~FlightTaskDescend() = default; bool update() override; + bool activate(const trajectory_setpoint_s &last_setpoint) override; private: Sticks _sticks{this}; diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp index ef9f3a215c..790836d896 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp @@ -57,7 +57,7 @@ bool FlightTaskTransition::updateInitialize() void FlightTaskTransition::updateParameters() { -// check for parameter updates + // check for parameter updates if (_parameter_update_sub.updated()) { // clear update parameter_update_s pupdate; @@ -85,9 +85,19 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) _velocity_setpoint(2) = _vel_z_filter.getState(); + _sub_vehicle_status.update(); + + const bool is_vtol_front_transition = _sub_vehicle_status.get().in_transition_mode + && _sub_vehicle_status.get().in_transition_to_fw; + + if (is_vtol_front_transition) { + _gear.landing_gear = landing_gear_s::GEAR_UP; + + } else { + _gear.landing_gear = landing_gear_s::GEAR_DOWN; + } + return ret; - - } bool FlightTaskTransition::update() diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp index 123c961635..6e255dbd1c 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp @@ -43,6 +43,7 @@ #include #include #include +#include #include using namespace time_literals; @@ -63,6 +64,9 @@ private: static constexpr float _vel_z_filter_time_const = 2.0f; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; + param_t _param_handle_pitch_cruise_degrees{PARAM_INVALID}; float _param_pitch_cruise_degrees{0.f}; diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index f8b639a516..9d830f9dbc 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -67,6 +67,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : _pos_ctrl_landing_status_pub.advertise(); _tecs_status_pub.advertise(); _launch_detection_status_pub.advertise(); + _landing_gear_pub.advertise(); _airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE); @@ -1232,6 +1233,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons airspeed_sp = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : _param_fw_airspd_min.get(); _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND; _att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND; + _new_landing_gear_position = landing_gear_s::GEAR_DOWN; } @@ -1395,6 +1397,11 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo // apply flaps for takeoff according to the corresponding scale factor set via FW_FLAPS_TO_SCL _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_TAKEOFF; + // retract ladning gear once passed the climbout state + if (_runway_takeoff.getState() > RunwayTakeoffState::CLIMBOUT) { + _new_landing_gear_position = landing_gear_s::GEAR_UP; + } + } else { /* Perform launch detection */ if (!_skipping_takeoff_detection && _param_fw_laun_detcn_on.get() && @@ -1710,6 +1717,9 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND; _att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND; + // deploy gear as soon as we're in land mode, if not already done before + _new_landing_gear_position = landing_gear_s::GEAR_DOWN; + if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(pos_sp_curr); } @@ -2298,6 +2308,9 @@ FixedwingPositionControl::Run() reset_takeoff_state(); } + int8_t old_landing_gear_position = _new_landing_gear_position; + _new_landing_gear_position = landing_gear_s::GEAR_KEEP; // is overwritten in Takeoff and Land + switch (_control_mode_current) { case FW_POSCTRL_MODE_AUTO: { control_auto(control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current, @@ -2381,6 +2394,16 @@ FixedwingPositionControl::Run() } } + // if there's any change in landing gear setpoint publish it + if (_new_landing_gear_position != old_landing_gear_position + && _new_landing_gear_position != landing_gear_s::GEAR_KEEP) { + + landing_gear_s landing_gear = {}; + landing_gear.landing_gear = _new_landing_gear_position; + landing_gear.timestamp = hrt_absolute_time(); + _landing_gear_pub.publish(landing_gear); + } + perf_end(_loop_perf); } } diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp index 7504bba3aa..8323ac1813 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp @@ -71,6 +71,7 @@ #include #include #include +#include #include #include #include @@ -212,6 +213,7 @@ private: uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; uORB::Publication _launch_detection_status_pub{ORB_ID(launch_detection_status)}; uORB::PublicationMulti _orbit_status_pub{ORB_ID(orbit_status)}; + uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; manual_control_setpoint_s _manual_control_setpoint{}; position_setpoint_triplet_s _pos_sp_triplet{}; @@ -414,6 +416,9 @@ private: // nonlinear path following guidance - lateral-directional position control NPFG _npfg; + // LANDING GEAR + int8_t _new_landing_gear_position{landing_gear_s::GEAR_KEEP}; + hrt_abstime _time_in_fixed_bank_loiter{0}; // [us] float _min_current_sp_distance_xy{FLT_MAX}; float _target_bearing{0.0f}; // [rad] diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index c66a54d4c3..d1b7e4c098 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -437,8 +437,6 @@ void FixedwingRateControl::Run() _actuator_controls.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState(); _actuator_controls.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState(); _actuator_controls.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f; - // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future - _actuator_controls.control[actuator_controls_s::INDEX_LANDING_GEAR] = _manual_control_setpoint.aux3; /* lazily publish the setpoint only once available */ _actuator_controls.timestamp = hrt_absolute_time(); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index ad91bc13dd..95e9b61dce 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -79,6 +79,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("internal_combustion_engine_status", 10); add_optional_topic("iridiumsbd_status", 1000); add_optional_topic("irlock_report", 1000); + add_optional_topic("landing_gear", 200); add_optional_topic("landing_gear_wheel", 100); add_optional_topic("landing_target_pose", 1000); add_optional_topic("launch_detection_status", 200); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 6890d15888..3a3219d3f3 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -145,24 +145,6 @@ MulticopterRateControl::Run() _vehicle_status_sub.update(&_vehicle_status); - if (_landing_gear_sub.updated()) { - landing_gear_s landing_gear; - - if (_landing_gear_sub.copy(&landing_gear)) { - if (landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) { - if (landing_gear.landing_gear == landing_gear_s::GEAR_UP && (_landed || _maybe_landed)) { - mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear\t"); - events::send(events::ID("mc_rate_control_not_retract_landing_gear_landed"), - {events::Log::Error, events::LogInternal::Info}, - "Landed, unable to retract landing gear"); - - } else { - _landing_gear = landing_gear.landing_gear; - } - } - } - } - // use rates setpoint topic vehicle_rates_setpoint_s vehicle_rates_setpoint{}; @@ -246,7 +228,6 @@ MulticopterRateControl::Run() actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f; actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint(2)) ? -_thrust_setpoint( 2) : 0.0f; - actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear; actuators.timestamp_sample = angular_velocity.timestamp_sample; if (!_vehicle_status.is_vtol) { diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 7f95138561..ab1aa7926f 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -50,7 +50,6 @@ #include #include #include -#include #include #include #include @@ -98,7 +97,6 @@ private: uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; - uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; @@ -116,8 +114,6 @@ private: uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; - orb_advert_t _mavlink_log_pub{nullptr}; - vehicle_control_mode_s _vehicle_control_mode{}; vehicle_status_s _vehicle_status{}; @@ -138,8 +134,6 @@ private: float _energy_integration_time{0.0f}; float _control_energy[4] {}; - int8_t _landing_gear{landing_gear_s::GEAR_DOWN}; - DEFINE_PARAMETERS( (ParamFloat) _param_mc_rollrate_p, (ParamFloat) _param_mc_rollrate_i, diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index d225071953..8c4ba178bd 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -45,7 +45,6 @@ #include "vtol_att_control_main.h" #include -#include using namespace matrix; @@ -292,7 +291,6 @@ void Standard::fill_actuator_outputs() mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH]; mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW]; mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE]; - mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN; // FW out = 0, other than roll and pitch depending on elevon lock fw_out[actuator_controls_s::INDEX_ROLL] = _param_vt_elev_mc_lock.get() ? 0 : @@ -316,7 +314,6 @@ void Standard::fill_actuator_outputs() mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; - mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_UP; // FW out = FW in, with VTOL transition controlling throttle and airbrakes fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL] * (1.f - _mc_roll_weight); @@ -334,7 +331,6 @@ void Standard::fill_actuator_outputs() mc_out[actuator_controls_s::INDEX_PITCH] = 0; mc_out[actuator_controls_s::INDEX_YAW] = 0; mc_out[actuator_controls_s::INDEX_THROTTLE] = 0; - mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_UP; // FW out = FW in fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL]; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 026c648ebc..4ba2c73d2a 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -42,8 +42,6 @@ #include "tailsitter.h" #include "vtol_att_control_main.h" -#include - #define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 #define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC @@ -328,14 +326,6 @@ void Tailsitter::fill_actuator_outputs() _thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE]; } - // Landing Gear - if (_vtol_mode == vtol_mode::MC_MODE) { - mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN; - - } else { - mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_UP; - } - if (_param_vt_elev_mc_lock.get() && _vtol_mode == vtol_mode::MC_MODE) { fw_out[actuator_controls_s::INDEX_ROLL] = 0; fw_out[actuator_controls_s::INDEX_PITCH] = 0; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 7528d85297..158fd62f83 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -42,8 +42,6 @@ #include "tiltrotor.h" #include "vtol_att_control_main.h" -#include - using namespace matrix; using namespace time_literals; @@ -450,14 +448,6 @@ void Tiltrotor::fill_actuator_outputs() collective_thrust_normalized_setpoint = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; } - // Landing gear - if (_vtol_mode == vtol_mode::MC_MODE) { - mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN; - - } else { - mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_UP; - } - // Fixed wing output if (_param_vt_elev_mc_lock.get() && _vtol_mode == vtol_mode::MC_MODE) { fw_out[actuator_controls_s::INDEX_ROLL] = 0;