yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

LandDetector: fix total system flight time (landed & takeoff logic)

Browse Source
This commit is contained in:
Beat Küng
2017-02-01 13:36:42 +01:00
committed by Lorenz Meier
parent f718b3a97a
commit 05b649cc86
1 changed files with 9 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/land_detector/LandDetector.cpp
View File

@@ -133,21 +133,22 @@ void LandDetector::_cycle()
(_landDetected.landed != landDetected) || (_landDetected.landed != landDetected) ||
(_landDetected.ground_contact != ground_contactDetected)) { (_landDetected.ground_contact != ground_contactDetected)) {
_landDetected.timestamp = hrt_absolute_time(); if (!landDetected && _landDetected.landed) {
_landDetected.freefall = (_state == LandDetectionState::FREEFALL);
_landDetected.landed = (_state == LandDetectionState::LANDED);
_landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT);
// We did take off // We did take off
if (landDetected && !_landDetected.landed) {
_takeoff_time = now; _takeoff_time = now;
} else if (_takeoff_time != 0 && !landDetected && _landDetected.landed) { } else if (_takeoff_time != 0 && landDetected && !_landDetected.landed) {
// We landed
_total_flight_time += now - _takeoff_time; _total_flight_time += now - _takeoff_time;
_takeoff_time = 0; _takeoff_time = 0;
param_set(_p_total_flight_time, &_total_flight_time); param_set(_p_total_flight_time, &_total_flight_time);
} }
_landDetected.timestamp = hrt_absolute_time();
_landDetected.freefall = (_state == LandDetectionState::FREEFALL);
_landDetected.landed = (_state == LandDetectionState::LANDED);
_landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT);
int instance; int instance;
orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected, orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected,
&instance, ORB_PRIO_DEFAULT); &instance, ORB_PRIO_DEFAULT);