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

land detector: move arming state into base class & set param when disarming

Browse Source 
Before that, different modules (ekf2, commander & land detector) changed
params upon different events:
- ekf2 & commander set params after disarm
- land detector set params on land detected
If the 2 events were several 100ms appart, it led to 2 param saves, and
the latter param set could have been blocked by an ongoing save. And if
the land detector was blocked, it could lead to mag timeouts.

This patch makes all modules use the same event, thus only a single param
save will happen.

If we want to have guarantees to never block, we should introduce a
param_try_set() API method.
This commit is contained in:
Beat Küng
2017-12-07 11:55:51 +00:00
committed by Lorenz Meier
parent 1dbeec6a19
commit a0afc370d0
6 changed files with 24 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/LandDetector.cpp
+17 -11
View File
@@ -90,6 +90,7 @@ void LandDetector::_cycle()
_p_total_flight_time_low = param_find("LND_FLIGHT_T_LO");
// Initialize uORB topics.
_armingSub = orb_subscribe(ORB_ID(actuator_armed));
_initialize_topics();
_check_params(true);
@@ -98,6 +99,7 @@ void LandDetector::_cycle()
}
_check_params(false);
_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
_update_topics();
_update_state();
@@ -107,6 +109,8 @@ void LandDetector::_cycle()
const bool ground_contactDetected = (_state == LandDetectionState::GROUND_CONTACT);
const float alt_max = _get_max_altitude();
const hrt_abstime now = hrt_absolute_time();
// Only publish very first time or when the result has changed.
if ((_landDetectedPub == nullptr) ||
(_landDetected.landed != landDetected) ||
@@ -115,20 +119,9 @@ void LandDetector::_cycle()
(_landDetected.ground_contact != ground_contactDetected) ||
(fabsf(_landDetected.alt_max - alt_max) > FLT_EPSILON)) {
hrt_abstime now = hrt_absolute_time();
if (!landDetected && _landDetected.landed) {
// We did take off
_takeoff_time = now;
} else if (_takeoff_time != 0 && landDetected && !_landDetected.landed) {
// We landed
_total_flight_time += now - _takeoff_time;
_takeoff_time = 0;
uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff;
param_set_no_notification(_p_total_flight_time_high, &flight_time);
flight_time = _total_flight_time & 0xffffffff;
param_set_no_notification(_p_total_flight_time_low, &flight_time);
}
_landDetected.timestamp = hrt_absolute_time();
@@ -143,6 +136,19 @@ void LandDetector::_cycle()
&instance, ORB_PRIO_DEFAULT);
}
// 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 && !_arming.armed && _previous_arming_state) {
_total_flight_time += now - _takeoff_time;
_takeoff_time = 0;
uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff;
param_set_no_notification(_p_total_flight_time_high, &flight_time);
flight_time = _total_flight_time & 0xffffffff;
param_set_no_notification(_p_total_flight_time_low, &flight_time);
}
_previous_arming_state = _arming.armed;
perf_end(_cycle_perf);
if (!should_exit()) {