dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-12-19 15:36:31 +01:00
parent 3e884116c4
commit 16594bffa9
14 changed files with 54 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/ActuatorControls.msg
View File

@ -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

7
src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp
View File

@ -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();

1
src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp
View File

@ -49,6 +49,7 @@ public:
virtual ~FlightTaskDescend() = default;
bool update() override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
private:
Sticks _sticks{this};

16
src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp
View File

@ -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()

4
src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp
View File

@ -43,6 +43,7 @@
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <drivers/drv_hrt.h>
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<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)};
param_t _param_handle_pitch_cruise_degrees{PARAM_INVALID};
float _param_pitch_cruise_degrees{0.f};

23
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
View File

@ -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);
}
}

5
src/modules/fw_path_navigation/FixedwingPositionControl.hpp
View File

@ -71,6 +71,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/launch_detection_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/npfg_status.h>
@ -212,6 +213,7 @@ private:
uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
uORB::Publication<launch_detection_status_s> _launch_detection_status_pub{ORB_ID(launch_detection_status)};
uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
uORB::Publication<landing_gear_s> _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]

2
src/modules/fw_rate_control/FixedwingRateControl.cpp
View File

@ -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();

1
src/modules/logger/logged_topics.cpp
View File

@ -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);

19
src/modules/mc_rate_control/MulticopterRateControl.cpp
View File

@ -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) {

6
src/modules/mc_rate_control/MulticopterRateControl.hpp
View File

@ -50,7 +50,6 @@
#include <uORB/topics/actuator_controls_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_allocator_status.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
@ -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_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _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<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,

4
src/modules/vtol_att_control/standard.cpp
View File

@ -45,7 +45,6 @@
#include "vtol_att_control_main.h"
#include <float.h>
#include <uORB/topics/landing_gear.h>
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];

10
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -42,8 +42,6 @@
#include "tailsitter.h"
#include "vtol_att_control_main.h"
#include <uORB/topics/landing_gear.h>
#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;

10
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -42,8 +42,6 @@
#include "tiltrotor.h"
#include "vtol_att_control_main.h"
#include <uORB/topics/landing_gear.h>
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;