fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 17:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

land_detector: multicopter ground contact fixes, improvements, and cleanup

Browse Source 
- if "landed" and "maybe_landed" states are false then both the "hit_ground" and the "low_thrust" condition need to be true in order to detect landing
 - ground contact MC NAN setpoint workaround
 - ground contact additionally check acceleration setpoint
 - schedule with vehicle_local_position updates (most updates require valid local position)
 - don't allow LNDMC_Z_VEL_MAX to exceed MPC_LAND_SPEED
 - ground contact horizontal movement checks default to failed if estimates aren't available
This commit is contained in:
Daniel Agar
2020-08-02 13:37:23 -04:00
committed by GitHub
parent 63a23957b1
commit f5f2897486
13 changed files with 223 additions and 321 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/LandDetector.cpp
+41 -45
View File
@@ -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(&param_update);
updateParams();
_update_params();
_total_flight_time = static_cast<uint64_t>(_param_total_flight_time_high.get()) << 32;
_total_flight_time |= static_cast<uint32_t>(_param_total_flight_time_low.get());
}
_actuator_armed_sub.update(&_actuator_armed);
actuator_armed_s actuator_armed;
if (_actuator_armed_sub.update(&actuator_armed)) {
_armed = actuator_armed.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(&param_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<uint64_t>(_param_total_flight_time_high.get()) << 32;
_total_flight_time |= static_cast<uint32_t>(_param_total_flight_time_low.get());
}
} // namespace land_detector