From ad6592f669be8300ad12dcceeb610e8678f7f86b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 9 Feb 2022 18:24:30 -0500 Subject: [PATCH] mc_pos_control: require current trajectory setpoint to run controller --- .../MulticopterPositionControl.cpp | 121 +++++++++++------- .../MulticopterPositionControl.hpp | 28 ++-- .../PositionControl/PositionControl.cpp | 2 +- 3 files changed, 91 insertions(+), 60 deletions(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 1225aed1a2..2c25383922 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -319,9 +319,21 @@ void MulticopterPositionControl::Run() // set _dt in controllib Block for BlockDerivative setDt(dt); - _in_failsafe = false; - _vehicle_control_mode_sub.update(&_vehicle_control_mode); + if (_vehicle_control_mode_sub.updated()) { + const bool position_control_enabled = _vehicle_control_mode.flag_multicopter_position_control_enabled; + + if (_vehicle_control_mode_sub.update(&_vehicle_control_mode)) { + if (!position_control_enabled && _vehicle_control_mode.flag_multicopter_position_control_enabled) { + _time_position_control_enabled = _vehicle_control_mode.timestamp; + + } else if (position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled) { + // clear existing setpoint when controller is no longer active + _setpoint = {}; + } + } + } + _vehicle_land_detected_sub.update(&_vehicle_land_detected); if (_param_mpc_use_hte.get()) { @@ -334,36 +346,66 @@ void MulticopterPositionControl::Run() } } - PositionControlStates states{set_vehicle_states(local_pos)}; + const bool is_trajectory_setpoint_updated = _trajectory_setpoint_sub.update(&_setpoint); - if (_vehicle_control_mode.flag_multicopter_position_control_enabled) { + // adjust existing (or older) setpoint with any EKF reset deltas + if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < local_pos.timestamp)) { + if (local_pos.vxy_reset_counter != _vxy_reset_counter) { + _setpoint.vx += local_pos.delta_vxy[0]; + _setpoint.vy += local_pos.delta_vxy[1]; + } - const bool is_trajectory_setpoint_updated = _trajectory_setpoint_sub.update(&_setpoint); + if (local_pos.vz_reset_counter != _vz_reset_counter) { + _setpoint.vz += local_pos.delta_vz; + } - // adjust existing (or older) setpoint with any EKF reset deltas - if (_setpoint.timestamp < local_pos.timestamp) { - if (local_pos.vxy_reset_counter != _vxy_reset_counter) { - _setpoint.vx += local_pos.delta_vxy[0]; - _setpoint.vy += local_pos.delta_vxy[1]; - } + if (local_pos.xy_reset_counter != _xy_reset_counter) { + _setpoint.x += local_pos.delta_xy[0]; + _setpoint.y += local_pos.delta_xy[1]; + } - if (local_pos.vz_reset_counter != _vz_reset_counter) { - _setpoint.vz += local_pos.delta_vz; - } + if (local_pos.z_reset_counter != _z_reset_counter) { + _setpoint.z += local_pos.delta_z; + } - if (local_pos.xy_reset_counter != _xy_reset_counter) { - _setpoint.x += local_pos.delta_xy[0]; - _setpoint.y += local_pos.delta_xy[1]; - } + if (local_pos.heading_reset_counter != _heading_reset_counter) { + _setpoint.yaw += local_pos.delta_heading; + } + } - if (local_pos.z_reset_counter != _z_reset_counter) { - _setpoint.z += local_pos.delta_z; - } + if (local_pos.vxy_reset_counter != _vxy_reset_counter) { + _vel_x_deriv.reset(); + _vel_y_deriv.reset(); + } - if (local_pos.heading_reset_counter != _heading_reset_counter) { - _setpoint.yaw += local_pos.delta_heading; - } + if (local_pos.vz_reset_counter != _vz_reset_counter) { + _vel_z_deriv.reset(); + } + + // save latest reset counters + _vxy_reset_counter = local_pos.vxy_reset_counter; + _vz_reset_counter = local_pos.vz_reset_counter; + _xy_reset_counter = local_pos.xy_reset_counter; + _z_reset_counter = local_pos.z_reset_counter; + _heading_reset_counter = local_pos.heading_reset_counter; + + + PositionControlStates states{set_vehicle_states(local_pos)}; + + + if (_vehicle_control_mode.flag_multicopter_position_control_enabled) { + // set failsafe setpoint if there hasn't been a new + // trajectory setpoint since position control started + if ((_setpoint.timestamp < _time_position_control_enabled) + && (time_stamp_now > _time_position_control_enabled)) { + + failsafe(time_stamp_now, _setpoint, states); + _setpoint.timestamp = local_pos.timestamp; } + } + + if (_vehicle_control_mode.flag_multicopter_position_control_enabled + && (_setpoint.timestamp >= _time_position_control_enabled)) { // update vehicle constraints and handle smooth takeoff _vehicle_constraints_sub.update(&_vehicle_constraints); @@ -377,7 +419,7 @@ void MulticopterPositionControl::Run() if (_vehicle_control_mode.flag_control_offboard_enabled) { bool want_takeoff = _vehicle_control_mode.flag_armed && _vehicle_land_detected.landed - && hrt_elapsed_time(&_setpoint.timestamp) < 1_s; + && (time_stamp_now < _setpoint.timestamp + 1_s); if (want_takeoff && PX4_ISFINITE(_setpoint.z) && (_setpoint.z < states.position(2))) { @@ -474,17 +516,9 @@ void MulticopterPositionControl::Run() } else { // Failsafe - // do not warn while we are disarmed, as we might not have valid setpoints yet - const bool warn_failsafe = ((time_stamp_now - _last_warn) > 2_s) && _vehicle_control_mode.flag_armed; - - if (warn_failsafe) { - PX4_WARN("invalid setpoints"); - _last_warn = time_stamp_now; - } - vehicle_local_position_setpoint_s failsafe_setpoint{}; - failsafe(time_stamp_now, failsafe_setpoint, states, warn_failsafe); + failsafe(time_stamp_now, failsafe_setpoint, states); // reset constraints _vehicle_constraints = {0, NAN, NAN, false, {}}; @@ -524,21 +558,22 @@ void MulticopterPositionControl::Run() _takeoff_status_pub.get().timestamp = hrt_absolute_time(); _takeoff_status_pub.update(); } - - // save latest reset counters - _vxy_reset_counter = local_pos.vxy_reset_counter; - _vz_reset_counter = local_pos.vz_reset_counter; - _xy_reset_counter = local_pos.xy_reset_counter; - _z_reset_counter = local_pos.z_reset_counter; - _heading_reset_counter = local_pos.heading_reset_counter; } perf_end(_cycle_perf); } void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, - const PositionControlStates &states, bool warn) + const PositionControlStates &states) { + // do not warn while we are disarmed, as we might not have valid setpoints yet + bool warn = _vehicle_control_mode.flag_armed && ((now - _last_warn) > 2_s); + + if (warn) { + PX4_WARN("invalid setpoints"); + _last_warn = now; + } + // Only react after a short delay _failsafe_land_hysteresis.set_state_and_update(true, now); @@ -578,8 +613,6 @@ void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_ PX4_WARN("Failsafe: blind descend"); } } - - _in_failsafe = true; } } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 856a4fa517..dbe9f97ec9 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -92,21 +92,22 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; - uORB::PublicationData _takeoff_status_pub {ORB_ID(takeoff_status)}; - uORB::Publication _vehicle_attitude_setpoint_pub {ORB_ID(vehicle_attitude_setpoint)}; - uORB::Publication _local_pos_sp_pub {ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ + uORB::PublicationData _takeoff_status_pub{ORB_ID(takeoff_status)}; + uORB::Publication _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ - uORB::SubscriptionCallbackWorkItem _local_pos_sub {this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ + uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ - uORB::SubscriptionInterval _parameter_update_sub {ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _hover_thrust_estimate_sub {ORB_ID(hover_thrust_estimate)}; - uORB::Subscription _trajectory_setpoint_sub {ORB_ID(trajectory_setpoint)}; - uORB::Subscription _vehicle_constraints_sub {ORB_ID(vehicle_constraints)}; - uORB::Subscription _vehicle_control_mode_sub {ORB_ID(vehicle_control_mode)}; - uORB::Subscription _vehicle_land_detected_sub {ORB_ID(vehicle_land_detected)}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; - hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ + hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ + hrt_abstime _time_position_control_enabled{0}; vehicle_local_position_setpoint_s _setpoint {}; vehicle_control_mode_s _vehicle_control_mode {}; @@ -183,8 +184,6 @@ private: hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */ - bool _in_failsafe{false}; /**< true if failsafe was entered within current cycle */ - bool _hover_thrust_initialized{false}; /** Timeout in us for trajectory data to get considered invalid */ @@ -228,8 +227,7 @@ private: * setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set * to true, the failsafe will be initiated immediately. */ - void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, - bool warn); + void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states); /** * Reset setpoints to NAN diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 1bf8f26210..8dd2c521e7 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -114,7 +114,7 @@ bool PositionControl::update(const float dt) _yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control } - // There has to be a valid output accleration and thrust setpoint otherwise something went wrong + // There has to be a valid output acceleration and thrust setpoint otherwise something went wrong valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2)); valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));