From c9e3f9714c86d23ab3928826a0302350f821368f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 28 Sep 2022 14:30:16 -0400 Subject: [PATCH] multicopter land detector move ground contact intent to mc_pos_control --- src/modules/commander/Commander.cpp | 2 +- .../land_detector/MulticopterLandDetector.cpp | 15 +++-- .../land_detector/MulticopterLandDetector.h | 4 +- .../MulticopterPositionControl.cpp | 57 ++++++++++++++++--- .../MulticopterPositionControl.hpp | 13 ++--- 5 files changed, 66 insertions(+), 25 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f6a4d1ed68..0cf8a13e0c 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3102,7 +3102,7 @@ void Commander::battery_status_check() // Compare estimate of RTL time to estimate of remaining flight time if (_vehicle_status_flags.battery_low_remaining_time && _arm_state_machine.isArmed() - && !_vehicle_land_detected.ground_contact // not in any landing stage + && !(_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed) // not in any landing stage && !_rtl_time_actions_done && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index eb21bb3bbb..35bfd36ed0 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -212,12 +212,17 @@ bool MulticopterLandDetector::_get_ground_contact_state() // if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present, // we then can assume that the vehicle hit ground if (_flag_control_climb_rate_enabled) { - vehicle_local_position_setpoint_s trajectory_setpoint; + vehicle_local_position_setpoint_s vehicle_local_position_setpoint; - if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { - // Setpoints can be NAN - _in_descend = PX4_ISFINITE(trajectory_setpoint.vz) - && (trajectory_setpoint.vz >= crawl_speed_threshold); + if (_vehicle_local_position_setpoint_sub.update(&vehicle_local_position_setpoint)) { + // setpoints can briefly be NAN to signal resets, TODO: fix in multicopter position controller + const bool descend_vel_sp = PX4_ISFINITE(vehicle_local_position_setpoint.vz) + && (vehicle_local_position_setpoint.vz >= crawl_speed_threshold); + + const bool descend_acc_sp = PX4_ISFINITE(vehicle_local_position_setpoint.acceleration[2]) + && (vehicle_local_position_setpoint.acceleration[2] >= 100.f); + + _in_descend = descend_vel_sp || descend_acc_sp; } // ground contact requires commanded descent until landed diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 855d6ff52d..1829a2ed12 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -44,7 +44,6 @@ #include #include -#include #include #include #include @@ -111,9 +110,8 @@ private: uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; - uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; - uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; hrt_abstime _hover_thrust_estimate_last_valid{0}; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index fa097c7cdf..ba919da2cd 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -360,7 +360,14 @@ void MulticopterPositionControl::Run() } } - _vehicle_land_detected_sub.update(&_vehicle_land_detected); + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + _ground_contact = vehicle_land_detected.ground_contact; + } + } if (_param_mpc_use_hte.get()) { hover_thrust_estimate_s hte; @@ -372,7 +379,24 @@ void MulticopterPositionControl::Run() } } - _trajectory_setpoint_sub.update(&_setpoint); + if (_trajectory_setpoint_sub.updated()) { + trajectory_setpoint_s trajectory_setpoint{}; + + if (_trajectory_setpoint_sub.copy(&trajectory_setpoint)) { + _setpoint.timestamp = trajectory_setpoint.timestamp; + _setpoint.x = trajectory_setpoint.position[0]; + _setpoint.y = trajectory_setpoint.position[1]; + _setpoint.z = trajectory_setpoint.position[2]; + _setpoint.vx = trajectory_setpoint.velocity[0]; + _setpoint.vy = trajectory_setpoint.velocity[1]; + _setpoint.vz = trajectory_setpoint.velocity[2]; + _setpoint.acceleration[0] = trajectory_setpoint.acceleration[0]; + _setpoint.acceleration[1] = trajectory_setpoint.acceleration[1]; + _setpoint.acceleration[2] = trajectory_setpoint.acceleration[2]; + _setpoint.yaw = trajectory_setpoint.yaw; + _setpoint.yawspeed = trajectory_setpoint.yawspeed; + } + } // adjust existing (or older) setpoint with any EKF reset deltas if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) { @@ -471,13 +495,30 @@ void MulticopterPositionControl::Run() } // handle smooth takeoff - _takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, + _takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _landed, _vehicle_constraints.want_takeoff, _vehicle_constraints.speed_up, false, vehicle_local_position.timestamp_sample); - const bool not_taken_off = (_takeoff.getTakeoffState() < TakeoffState::rampup); - const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight); - const bool flying_but_ground_contact = (flying && _vehicle_land_detected.ground_contact); + const bool not_taken_off = (_takeoff.getTakeoffState() < TakeoffState::rampup); + const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight); + const bool ground_contact = (flying && _ground_contact); + + bool descent_intended = false; + + if (flying && ground_contact) { + if (PX4_ISFINITE(_setpoint.z)) { + // position setpoint + descent_intended = (_setpoint.z > states.position(2)); + + } else if (PX4_ISFINITE(_setpoint.vz)) { + // velocity setpoint > 90% of MPC_LAND_CRWL + descent_intended = (_setpoint.vz > (0.9f * math::max(_param_mpc_land_crwl.get(), 0.1f))); + + } else if (PX4_ISFINITE(_setpoint.acceleration[2])) { + // acceleration setpoint + descent_intended = (_setpoint.acceleration[2] > 0.f); + } + } if (!flying) { _control.setHoverThrust(_param_mpc_thr_hover.get()); @@ -488,7 +529,7 @@ void MulticopterPositionControl::Run() _setpoint.acceleration[2] = NAN; } - if (not_taken_off || flying_but_ground_contact) { + if (not_taken_off || (flying && ground_contact && descent_intended)) { // we are not flying yet and need to avoid any corrections reset_setpoint_to_nan(_setpoint); _setpoint.timestamp = vehicle_local_position.timestamp_sample; @@ -567,7 +608,7 @@ void MulticopterPositionControl::Run() } else { // an update is necessary here because otherwise the takeoff state doesn't get skipped with non-altitude-controlled modes - _takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, true, + _takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _landed, false, 10.f, true, vehicle_local_position.timestamp_sample); } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 7a70ae0b13..b1f092e0e4 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -112,6 +112,9 @@ private: vehicle_local_position_setpoint_s _setpoint {}; vehicle_control_mode_s _vehicle_control_mode {}; + bool _landed{true}; + bool _ground_contact{true}; + vehicle_constraints_s _vehicle_constraints { .timestamp = 0, .speed_up = NAN, @@ -119,14 +122,6 @@ private: .want_takeoff = false, }; - vehicle_land_detected_s _vehicle_land_detected { - .timestamp = 0, - .freefall = false, - .ground_contact = true, - .maybe_landed = true, - .landed = true, - }; - DEFINE_PARAMETERS( // Position Control (ParamFloat) _param_mpc_xy_p, @@ -164,6 +159,8 @@ private: (ParamFloat) _param_mpc_thr_max, (ParamFloat) _param_mpc_thr_xy_marg, + (ParamFloat) _param_mpc_land_crwl, + (ParamFloat) _param_sys_vehicle_resp, (ParamFloat) _param_mpc_acc_hor, (ParamFloat) _param_mpc_acc_down_max,