diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d5e07c830c18..9a63c8e65e2f 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1575,7 +1575,6 @@ Commander::run() /* Update land detector */ if (_land_detector_sub.updated()) { _was_landed = _land_detector.landed; - bool was_falling = _land_detector.freefall; _land_detector_sub.copy(&_land_detector); // Only take actions if armed @@ -1596,12 +1595,6 @@ Commander::run() _lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s; } } - - if (was_falling != _land_detector.freefall) { - if (_land_detector.freefall) { - mavlink_log_info(&mavlink_log_pub, "Freefall detected"); - } - } } } diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 37bc811cf965..845e6cd05830 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -51,16 +51,10 @@ FixedwingLandDetector::FixedwingLandDetector() _landed_hysteresis.set_hysteresis_time_from(true, FLYING_TRIGGER_TIME_US); } -void FixedwingLandDetector::_update_topics() -{ - LandDetector::_update_topics(); - _airspeed_validated_sub.update(&_airspeed_validated); -} - bool FixedwingLandDetector::_get_landed_state() { // Only trigger flight conditions if we are armed. - if (!_actuator_armed.armed) { + if (!_armed) { return true; } @@ -83,19 +77,20 @@ bool FixedwingLandDetector::_get_landed_state() _velocity_z_filtered = val; } + airspeed_validated_s airspeed_validated{}; + _airspeed_validated_sub.copy(&airspeed_validated); + // set _airspeed_filtered to 0 if airspeed data is invalid - if (!PX4_ISFINITE(_airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed_validated.timestamp) > 1_s) { + if (!PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) { _airspeed_filtered = 0.0f; } else { - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed_validated.true_airspeed_m_s; + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s; } // A leaking lowpass prevents biases from building up, but // gives a mostly correct response for short impulses. - const matrix::Vector3f accel{_vehicle_acceleration.xyz}; - const float acc_hor = sqrtf(accel(0) * accel(0) + accel(1) * accel(1)); - + const float acc_hor = matrix::Vector2f(_acceleration).norm(); _xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f; // Crude land detector for fixedwing. diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 19bff21757fa..b415de1b0d81 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -61,7 +61,6 @@ class FixedwingLandDetector final : public LandDetector protected: bool _get_landed_state() override; - void _update_topics() override; private: @@ -71,8 +70,6 @@ class FixedwingLandDetector final : public LandDetector uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; - airspeed_validated_s _airspeed_validated{}; - float _airspeed_filtered{0.0f}; float _velocity_xy_filtered{0.0f}; float _velocity_z_filtered{0.0f}; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 8a231b7b0dbb..ca98d577a962 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -57,21 +57,52 @@ LandDetector::~LandDetector() void LandDetector::start() { - _update_params(); - ScheduleOnInterval(LAND_DETECTOR_UPDATE_INTERVAL); + ScheduleDelayed(50_ms); + _vehicle_local_position_sub.registerCallback(); } void LandDetector::Run() { + // push backup schedule + ScheduleDelayed(50_ms); + perf_begin(_cycle_perf); - if (_parameter_update_sub.updated()) { + if (_parameter_update_sub.updated() || (_land_detected.timestamp == 0)) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); _update_params(); + + _total_flight_time = static_cast(_param_total_flight_time_high.get()) << 32; + _total_flight_time |= static_cast(_param_total_flight_time_low.get()); + } + + actuator_armed_s actuator_armed; + + if (_actuator_armed_sub.update(&actuator_armed)) { + _armed = actuator_armed.armed; } - _actuator_armed_sub.update(&_actuator_armed); + vehicle_acceleration_s vehicle_acceleration; + + if (_vehicle_acceleration_sub.update(&vehicle_acceleration)) { + _acceleration = matrix::Vector3f{vehicle_acceleration.xyz}; + } + + _vehicle_local_position_sub.update(&_vehicle_local_position); + _vehicle_status_sub.update(&_vehicle_status); + _update_topics(); - _update_state(); + + const hrt_abstime now_us = hrt_absolute_time(); + + _freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us); + _ground_contact_hysteresis.set_state_and_update(_get_ground_contact_state(), now_us); + _maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state(), now_us); + _landed_hysteresis.set_state_and_update(_get_landed_state(), now_us); + _ground_effect_hysteresis.set_state_and_update(_get_ground_effect_state(), now_us); const bool freefallDetected = _freefall_hysteresis.get_state(); const bool ground_contactDetected = _ground_contact_hysteresis.get_state(); @@ -80,8 +111,6 @@ void LandDetector::Run() const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : INFINITY; const bool in_ground_effect = _ground_effect_hysteresis.get_state(); - const hrt_abstime now = hrt_absolute_time(); - // publish at 1 Hz, very first time, or when the result has changed if ((hrt_elapsed_time(&_land_detected.timestamp) >= 1_s) || (_land_detected.landed != landDetected) || @@ -93,24 +122,23 @@ void LandDetector::Run() if (!landDetected && _land_detected.landed && _takeoff_time == 0) { /* only set take off time once, until disarming */ // We did take off - _takeoff_time = now; + _takeoff_time = now_us; } - _land_detected.timestamp = hrt_absolute_time(); _land_detected.landed = landDetected; _land_detected.freefall = freefallDetected; _land_detected.maybe_landed = maybe_landedDetected; _land_detected.ground_contact = ground_contactDetected; _land_detected.alt_max = alt_max; _land_detected.in_ground_effect = in_ground_effect; - + _land_detected.timestamp = hrt_absolute_time(); _vehicle_land_detected_pub.publish(_land_detected); } // set the flight time when disarming (not necessarily when landed, because all param changes should // happen on the same event and it's better to set/save params while not in armed state) - if (_takeoff_time != 0 && !_actuator_armed.armed && _previous_armed_state) { - _total_flight_time += now - _takeoff_time; + if (_takeoff_time != 0 && !_armed && _previous_armed_state) { + _total_flight_time += now_us - _takeoff_time; _takeoff_time = 0; uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff; @@ -124,7 +152,7 @@ void LandDetector::Run() _param_total_flight_time_low.commit_no_notification(); } - _previous_armed_state = _actuator_armed.armed; + _previous_armed_state = _armed; perf_end(_cycle_perf); @@ -134,36 +162,4 @@ void LandDetector::Run() } } -void LandDetector::_update_params() -{ - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - _update_total_flight_time(); -} - -void LandDetector::_update_state() -{ - const hrt_abstime now_us = hrt_absolute_time(); - _freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us); - _ground_contact_hysteresis.set_state_and_update(_get_ground_contact_state(), now_us); - _maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state(), now_us); - _landed_hysteresis.set_state_and_update(_get_landed_state(), now_us); - _ground_effect_hysteresis.set_state_and_update(_get_ground_effect_state(), now_us); -} - -void LandDetector::_update_topics() -{ - _actuator_armed_sub.update(&_actuator_armed); - _vehicle_acceleration_sub.update(&_vehicle_acceleration); - _vehicle_local_position_sub.update(&_vehicle_local_position); -} - -void LandDetector::_update_total_flight_time() -{ - _total_flight_time = static_cast(_param_total_flight_time_high.get()) << 32; - _total_flight_time |= static_cast(_param_total_flight_time_low.get()); -} - } // namespace land_detector diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index d9aa5b4e864f..dc78c23935f1 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -56,11 +56,13 @@ #include #include #include +#include #include #include #include #include #include +#include using namespace time_literals; @@ -70,11 +72,9 @@ namespace land_detector class LandDetector : public ModuleBase, ModuleParams, px4::ScheduledWorkItem { public: - LandDetector(); virtual ~LandDetector(); - /** @see ModuleBase */ static int custom_command(int argc, char *argv[]) { @@ -99,12 +99,12 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul /** * Updates parameters. */ - virtual void _update_params(); + virtual void _update_params() {}; /** * Updates subscribed uORB topics. */ - virtual void _update_topics(); + virtual void _update_topics() {}; /** * @return true if UAV is in a landed state. @@ -136,17 +136,22 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul */ virtual bool _get_ground_effect_state() { return false; } - /** Run main land detector loop at this interval. */ - static constexpr uint32_t LAND_DETECTOR_UPDATE_INTERVAL = 20_ms; - systemlib::Hysteresis _freefall_hysteresis{false}; systemlib::Hysteresis _landed_hysteresis{true}; systemlib::Hysteresis _maybe_landed_hysteresis{true}; systemlib::Hysteresis _ground_contact_hysteresis{true}; systemlib::Hysteresis _ground_effect_hysteresis{false}; - actuator_armed_s _actuator_armed{}; - vehicle_acceleration_s _vehicle_acceleration{}; + vehicle_local_position_s _vehicle_local_position{}; + vehicle_status_s _vehicle_status{}; + + matrix::Vector3f _acceleration{}; + + bool _armed{false}; + bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state + +private: + void Run() override; vehicle_land_detected_s _land_detected = { .timestamp = 0, @@ -157,29 +162,19 @@ class LandDetector : public ModuleBase, ModuleParams, px4::Schedul .landed = true, }; - vehicle_local_position_s _vehicle_local_position{}; - - uORB::Publication _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)}; - -private: - - void Run() override; - - void _update_state(); - - void _update_total_flight_time(); - - bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state - hrt_abstime _takeoff_time{0}; hrt_abstime _total_flight_time{0}; ///< total vehicle flight time in microseconds - perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, "land_detector_cycle")}; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + uORB::Publication _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; DEFINE_PARAMETERS_CUSTOM_PARENT( ModuleParams, diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 68365b87d070..8c68da1648a0 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -67,32 +67,40 @@ #include "MulticopterLandDetector.h" +using matrix::Vector2f; +using matrix::Vector3f; namespace land_detector { MulticopterLandDetector::MulticopterLandDetector() { - _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); + _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); _paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN"); - _paramHandle.minThrottle = param_find("MPC_THR_MIN"); + _paramHandle.minThrottle = param_find("MPC_THR_MIN"); _paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE"); - _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); + _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true). + _freefall_hysteresis.set_hysteresis_time_from(false, FREEFALL_TRIGGER_TIME_US); _ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US); - _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US); _maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US); + _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US); } void MulticopterLandDetector::_update_topics() { - LandDetector::_update_topics(); + actuator_controls_s actuator_controls; + + if (_actuator_controls_sub.update(&actuator_controls)) { + _actuator_controls_throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]; + } + + vehicle_control_mode_s vehicle_control_mode; - _actuator_controls_sub.update(&_actuator_controls); - _vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity); - _vehicle_control_mode_sub.update(&_vehicle_control_mode); - _vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint); + if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { + _flag_control_climb_rate_enabled = vehicle_control_mode.flag_control_climb_rate_enabled; + } if (_params.useHoverThrustEstimate) { hover_thrust_estimate_s hte; @@ -105,14 +113,18 @@ void MulticopterLandDetector::_update_topics() void MulticopterLandDetector::_update_params() { - LandDetector::_update_params(); - - _freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_lndmc_ffall_ttri.get())); - param_get(_paramHandle.minThrottle, &_params.minThrottle); param_get(_paramHandle.minManThrottle, &_params.minManThrottle); param_get(_paramHandle.landSpeed, &_params.landSpeed); + if (_param_lndmc_z_vel_max.get() > _params.landSpeed) { + PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_SPEED, updating %.3f -> %.3f", + (double)_param_lndmc_z_vel_max.get(), (double)_params.landSpeed); + + _param_lndmc_z_vel_max.set(_params.landSpeed); + _param_lndmc_z_vel_max.commit_no_notification(); + } + int32_t use_hover_thrust_estimate = 0; param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate); _params.useHoverThrustEstimate = (use_hover_thrust_estimate == 1); @@ -131,82 +143,120 @@ void MulticopterLandDetector::_update_params() bool MulticopterLandDetector::_get_freefall_state() { - if (_param_lndmc_ffall_thr.get() < 0.1f || - _param_lndmc_ffall_thr.get() > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection. - return false; - } - - if (_vehicle_acceleration.timestamp == 0) { - // _sensors is not valid yet, we have to assume we're not falling. - return false; - } - // norm of specific force. Should be close to 9.8 m/s^2 when landed. - const matrix::Vector3f accel{_vehicle_acceleration.xyz}; - - return (accel.norm() < _param_lndmc_ffall_thr.get()); // true if we are currently falling + return _acceleration.norm() < 2.f; } bool MulticopterLandDetector::_get_ground_contact_state() { - // When not armed, consider to have ground-contact - if (!_actuator_armed.armed) { - return true; - } + const bool lpos_available = (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s); - // land speed threshold - float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f); + // land speed threshold, 90% of MPC_LAND_SPEED + const float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f); - // Check if we are moving vertically - this might see a spike after arming due to - // throttle-up vibration. If accelerating fast the throttle thresholds will still give - // an accurate in-air indication. - bool vertical_movement = false; + bool vertical_movement = true; - if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { + if (lpos_available && _vehicle_local_position.v_z_valid) { + // Check if we are moving vertically - this might see a spike after arming due to + // throttle-up vibration. If accelerating fast the throttle thresholds will still give + // an accurate in-air indication. + float max_climb_rate = math::min(land_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get()); - // Widen acceptance thresholds for landed state right after arming - // so that motor spool-up and other effects do not trigger false negatives. - vertical_movement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get() * 2.5f; + if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { + // Widen acceptance thresholds for landed state right after arming + // so that motor spool-up and other effects do not trigger false negatives. + max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f; + } - } else { - // Adjust max_climb_rate if land_speed is lower than 2x max_climb_rate - float max_climb_rate = ((land_speed_threshold * 0.5f) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) : - _param_lndmc_z_vel_max.get(); - vertical_movement = fabsf(_vehicle_local_position.vz) > max_climb_rate; + vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate); } + // Check if we are moving horizontally. - _horizontal_movement = sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx - + _vehicle_local_position.vy * _vehicle_local_position.vy) > _param_lndmc_xy_vel_max.get(); + if (lpos_available && _vehicle_local_position.v_xy_valid) { + const Vector2f v_xy{_vehicle_local_position.vx, _vehicle_local_position.vy}; + _horizontal_movement = v_xy.longerThan(_param_lndmc_xy_vel_max.get()); + + } else { + _horizontal_movement = false; // not known + } + + + // low thrust: 30% of throttle range between min and hover + const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.3f; + + bool ground_contact = (_actuator_controls_throttle <= sys_low_throttle); // 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 - _in_descend = _is_climb_rate_enabled() - && (_vehicle_local_position_setpoint.vz >= land_speed_threshold); - bool hit_ground = _in_descend && !vertical_movement; + if (_flag_control_climb_rate_enabled) { + vehicle_local_position_setpoint_s vehicle_local_position_setpoint; + + 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 >= land_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 + if (!_maybe_landed_hysteresis.get_state() && !_landed_hysteresis.get_state()) { + ground_contact &= _in_descend; + } + + } else { + _in_descend = false; + } + + + // When not armed, consider to have ground-contact + if (!_armed) { + return true; + } + // TODO: we need an accelerometer based check for vertical movement for flying without GPS - return (_has_low_thrust() || hit_ground) && (!_horizontal_movement || !_has_position_lock()) - && (!vertical_movement || !_has_altitude_lock()); + return ground_contact && !_horizontal_movement && !vertical_movement; } bool MulticopterLandDetector::_get_maybe_landed_state() { // When not armed, consider to be maybe-landed - if (!_actuator_armed.armed) { + if (!_armed) { return true; } - if (_has_minimal_thrust()) { - if (_min_trust_start == 0) { - _min_trust_start = hrt_absolute_time(); + + // minimal throttle: initially 10% of throttle range between min and hover + float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f; + + // Determine the system min throttle based on flight mode + if (!_flag_control_climb_rate_enabled) { + sys_min_throttle = (_params.minManThrottle + 0.01f); + } + + // Check if thrust output is less than the minimum throttle. + if (_actuator_controls_throttle <= sys_min_throttle) { + if (_min_thrust_start == 0) { + _min_thrust_start = hrt_absolute_time(); } } else { - _min_trust_start = 0; + _min_thrust_start = 0; + return false; + } + + + if (_freefall_hysteresis.get_state()) { + return false; } - float landThresholdFactor = 1.0f; + + float landThresholdFactor = 1.f; // Widen acceptance thresholds for landed state right after landed if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { @@ -214,32 +264,26 @@ bool MulticopterLandDetector::_get_maybe_landed_state() } // Next look if all rotation angles are not moving. - float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor; - - bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > max_rotation_scaled) || - (fabsf(_vehicle_angular_velocity.xyz[1]) > max_rotation_scaled) || - (fabsf(_vehicle_angular_velocity.xyz[2]) > max_rotation_scaled); - - // Return status based on armed state and throttle if no position lock is available. - if (!_has_altitude_lock() && !rotating) { - // The system has minimum trust set (manual or in failsafe) - // if this persists for 8 seconds AND the drone is not - // falling consider it to be landed. This should even sustain - // quite acrobatic flight. - return (_min_trust_start > 0) && (hrt_elapsed_time(&_min_trust_start) > 8_s); + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + const Vector2f angular_velocity{vehicle_angular_velocity.xyz[0], vehicle_angular_velocity.xyz[1]}; + const float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor; + + if (angular_velocity.norm() > max_rotation_scaled) { + return false; + } + + // If vertical velocity is available: ground contact, no thrust, no movement -> landed + if ((hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.v_z_valid) { + return _ground_contact_hysteresis.get_state(); } - // Ground contact, no thrust and no movement -> landed - return _ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating; + // Otherwise, landed if the system has minimum thrust (manual or in failsafe) and no rotation for at least 8 seconds + return (_min_thrust_start > 0) && (hrt_elapsed_time(&_min_thrust_start) > 8_s); } bool MulticopterLandDetector::_get_landed_state() { - // When not armed, consider to be landed - if (!_actuator_armed.armed) { - return true; - } - // reset the landed_time if (!_maybe_landed_hysteresis.get_state()) { _landed_time = 0; @@ -248,6 +292,11 @@ bool MulticopterLandDetector::_get_landed_state() _landed_time = hrt_absolute_time(); } + // When not armed, consider to be landed + if (!_armed) { + return true; + } + // if we have maybe_landed, the mc_pos_control goes into idle (thrust_sp = 0.0) // therefore check if all other condition of the landed state remain true return _maybe_landed_hysteresis.get_state(); @@ -263,48 +312,6 @@ float MulticopterLandDetector::_get_max_altitude() } } -bool MulticopterLandDetector::_has_altitude_lock() -{ - return (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.z_valid; -} - -bool MulticopterLandDetector::_has_position_lock() -{ - return _has_altitude_lock() && _vehicle_local_position.xy_valid; -} - -bool MulticopterLandDetector::_is_climb_rate_enabled() -{ - bool has_updated = (hrt_elapsed_time(&_vehicle_local_position_setpoint.timestamp) < 1_s); - - return (_vehicle_control_mode.flag_control_climb_rate_enabled && has_updated - && PX4_ISFINITE(_vehicle_local_position_setpoint.vz)); -} - -bool MulticopterLandDetector::_has_low_thrust() -{ - // 30% of throttle range between min and hover - float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * - _param_lndmc_low_t_thr.get(); - - // Check if thrust output is less than the minimum auto throttle param. - return _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle; -} - -bool MulticopterLandDetector::_has_minimal_thrust() -{ - // 10% of throttle range between min and hover once we entered ground contact - float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f; - - // Determine the system min throttle based on flight mode - if (!_vehicle_control_mode.flag_control_climb_rate_enabled) { - sys_min_throttle = (_params.minManThrottle + 0.01f); - } - - // Check if thrust output is less than the minimum auto throttle param. - return _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle; -} - bool MulticopterLandDetector::_get_ground_effect_state() { return _in_descend && !_horizontal_movement; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index cc0e39cf2d35..11946f657c12 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -43,12 +43,10 @@ #pragma once #include -#include +#include #include -#include #include #include -#include #include "LandDetector.h" @@ -76,23 +74,17 @@ class MulticopterLandDetector : public LandDetector float _get_max_altitude() override; private: - /** Get control mode dependent pilot throttle threshold with which we should quit landed state and take off. */ - float _get_takeoff_throttle(); + /** Time in us that freefall has to hold before triggering freefall */ + static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms; - bool _has_low_thrust(); - bool _has_minimal_thrust(); - bool _has_altitude_lock(); - bool _has_position_lock(); - bool _is_climb_rate_enabled(); - - /** Time in us that landing conditions have to hold before triggering a land. */ - static constexpr hrt_abstime LAND_DETECTOR_TRIGGER_TIME_US = 300_ms; + /** Time in us that ground contact condition have to hold before triggering contact ground */ + static constexpr hrt_abstime GROUND_CONTACT_TRIGGER_TIME_US = 350_ms; /** Time in us that almost landing conditions have to hold before triggering almost landed . */ static constexpr hrt_abstime MAYBE_LAND_DETECTOR_TRIGGER_TIME_US = 250_ms; - /** Time in us that ground contact condition have to hold before triggering contact ground */ - static constexpr hrt_abstime GROUND_CONTACT_TRIGGER_TIME_US = 350_ms; + /** Time in us that landing conditions have to hold before triggering a land. */ + static constexpr hrt_abstime LAND_DETECTOR_TRIGGER_TIME_US = 300_ms; /** Time interval in us in which wider acceptance thresholds are used after landed. */ static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s; @@ -115,21 +107,17 @@ class MulticopterLandDetector : public LandDetector } _params{}; uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; - uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; - uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; - - actuator_controls_s _actuator_controls {}; - vehicle_angular_velocity_s _vehicle_angular_velocity{}; - vehicle_control_mode_s _vehicle_control_mode {}; - vehicle_local_position_setpoint_s _vehicle_local_position_setpoint {}; + bool _flag_control_climb_rate_enabled{false}; bool _hover_thrust_initialized{false}; - hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first + float _actuator_controls_throttle{0.f}; + + hrt_abstime _min_thrust_start{0}; ///< timestamp when minimum trust was applied first hrt_abstime _landed_time{0}; bool _in_descend{false}; ///< vehicle is desending @@ -138,9 +126,6 @@ class MulticopterLandDetector : public LandDetector DEFINE_PARAMETERS_CUSTOM_PARENT( LandDetector, (ParamFloat) _param_lndmc_alt_max, - (ParamFloat) _param_lndmc_ffall_thr, - (ParamFloat) _param_lndmc_ffall_ttri, - (ParamFloat) _param_lndmc_low_t_thr, (ParamFloat) _param_lndmc_rot_max, (ParamFloat) _param_lndmc_xy_vel_max, (ParamFloat) _param_lndmc_z_vel_max diff --git a/src/modules/land_detector/RoverLandDetector.cpp b/src/modules/land_detector/RoverLandDetector.cpp index bb1b5e2b4a16..e9942c4eae01 100644 --- a/src/modules/land_detector/RoverLandDetector.cpp +++ b/src/modules/land_detector/RoverLandDetector.cpp @@ -44,7 +44,6 @@ namespace land_detector { - bool RoverLandDetector::_get_ground_contact_state() { return true; @@ -52,14 +51,11 @@ bool RoverLandDetector::_get_ground_contact_state() bool RoverLandDetector::_get_landed_state() { - - _vehicle_status_sub.update(&_vehicle_status); - if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { return true; // If Landing has been requested then say we have landed. } else { - return !_actuator_armed.armed; // If we are armed we are not landed. + return !_armed; // If we are armed we are not landed. } } diff --git a/src/modules/land_detector/RoverLandDetector.h b/src/modules/land_detector/RoverLandDetector.h index 77abda2e1ac8..6ed05b64c92f 100644 --- a/src/modules/land_detector/RoverLandDetector.h +++ b/src/modules/land_detector/RoverLandDetector.h @@ -42,7 +42,6 @@ #pragma once #include "LandDetector.h" -#include namespace land_detector { @@ -57,12 +56,6 @@ class RoverLandDetector : public LandDetector bool _get_ground_contact_state() override; bool _get_landed_state() override; - -private: - // Program crashes when Subscriptor declared here - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - vehicle_status_s _vehicle_status{}; /**< vehicle status */ - }; } // namespace land_detector diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index 1f6e962acf44..a3a4ae3380b1 100644 --- a/src/modules/land_detector/VtolLandDetector.cpp +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -50,15 +50,13 @@ namespace land_detector void VtolLandDetector::_update_topics() { MulticopterLandDetector::_update_topics(); - _airspeed_validated_sub.update(&_airspeed_validated); - _vehicle_status_sub.update(&_vehicle_status); } bool VtolLandDetector::_get_maybe_landed_state() { // If in Fixed-wing mode, only trigger if disarmed - if ((_vehicle_status.timestamp != 0) && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - return !_actuator_armed.armed; + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return !_armed; } return MulticopterLandDetector::_get_maybe_landed_state(); @@ -67,17 +65,20 @@ bool VtolLandDetector::_get_maybe_landed_state() bool VtolLandDetector::_get_landed_state() { // If in Fixed-wing mode, only trigger if disarmed - if ((_vehicle_status.timestamp != 0) && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - return !_actuator_armed.armed; + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return !_armed; } // this is returned from the mutlicopter land detector bool landed = MulticopterLandDetector::_get_landed_state(); // for vtol we additionally consider airspeed - if (hrt_elapsed_time(&_airspeed_validated.timestamp) < 1_s && PX4_ISFINITE(_airspeed_validated.true_airspeed_m_s)) { + airspeed_validated_s airspeed_validated{}; + _airspeed_validated_sub.copy(&airspeed_validated); - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed_validated.true_airspeed_m_s; + if (hrt_elapsed_time(&airspeed_validated.timestamp) < 1_s && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) { + + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s; } else { // if airspeed does not update, set it to zero and rely on multicopter land detector @@ -97,8 +98,8 @@ bool VtolLandDetector::_get_landed_state() bool VtolLandDetector::_get_freefall_state() { - bool free_fall_detected = - MulticopterLandDetector::_get_freefall_state(); // true if falling or in a parabolic flight (low gravity) + // true if falling or in a parabolic flight (low gravity) + bool free_fall_detected = MulticopterLandDetector::_get_freefall_state(); // only return a positive free fall detected if not in fixed-wing mode return _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING && free_fall_detected; diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h index d159728636e7..16b34b6ce768 100644 --- a/src/modules/land_detector/VtolLandDetector.h +++ b/src/modules/land_detector/VtolLandDetector.h @@ -42,7 +42,6 @@ #pragma once #include -#include #include "MulticopterLandDetector.h" @@ -62,12 +61,7 @@ class VtolLandDetector : public MulticopterLandDetector bool _get_freefall_state() override; private: - uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - - airspeed_validated_s _airspeed_validated{}; - vehicle_status_s _vehicle_status{}; bool _was_in_air{false}; /**< indicates whether the vehicle was in the air in the previous iteration */ float _airspeed_filtered{0.0f}; /**< low pass filtered airspeed */ diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 752af2c47927..0924519bdfdd 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -54,8 +54,6 @@ namespace land_detector { -extern "C" __EXPORT int land_detector_main(int argc, char *argv[]); - static char _currentMode[12]; int LandDetector::task_spawn(int argc, char *argv[]) @@ -145,8 +143,7 @@ The module runs periodically on the HP work queue. return 0; } - -int land_detector_main(int argc, char *argv[]) +extern "C" __EXPORT int land_detector_main(int argc, char *argv[]) { return LandDetector::main(argc, argv); } diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index e5e4449837b5..30d3c42d5de5 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -67,35 +67,6 @@ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.5f); */ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); -/** - * Multicopter specific force threshold - * - * Multicopter threshold on the specific force measured by accelerometers in m/s^2 for free-fall detection - * - * @unit m/s^2 - * @min 0.1 - * @max 10 - * @decimal 2 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDMC_FFALL_THR, 2.0f); - -/** - * Multicopter free-fall trigger time - * - * Seconds (decimal) that freefall conditions have to met before triggering a freefall. - * Minimal value is limited by LAND_DETECTOR_UPDATE_RATE=50Hz in landDetector.h - * - * @unit s - * @min 0.02 - * @max 5 - * @decimal 2 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDMC_FFALL_TTRI, 0.3); - /** * Maximum altitude for multicopters * @@ -113,21 +84,3 @@ PARAM_DEFINE_FLOAT(LNDMC_FFALL_TTRI, 0.3); * */ PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f); - -/** - * Low throttle detection threshold. - * - * Defines the commanded throttle value below which the land detector - * considers the vehicle to have "low thrust". This is one condition that - * is used to detect the ground contact state. The value is calculated as - * val = (MPC_THR_HOVER - MPC_THR_MIN) * LNDMC_LOW_T_THR + MPC_THR_MIN - * Increase this value if the system takes long time to detect landing. - * - * @unit norm - * @min 0.1 - * @max 0.9 - * @decimal 2 - * @group Land Detector - * - */ -PARAM_DEFINE_FLOAT(LNDMC_LOW_T_THR, 0.3);