diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 9ae623639e..389ad4344f 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -103,6 +103,7 @@ void LandDetector::_cycle() _landDetected.freefall = false; _landDetected.landed = false; _landDetected.ground_contact = false; + _p_total_flight_time = param_find("LND_FLIGHT_TIME"); // Initialize uORB topics. _initialize_topics(); @@ -117,6 +118,8 @@ void LandDetector::_cycle() _update_topics(); + hrt_abstime now = hrt_absolute_time(); + _update_state(); bool freefallDetected = (_state == LandDetectionState::FREEFALL); @@ -135,6 +138,16 @@ void LandDetector::_cycle() _landDetected.landed = (_state == LandDetectionState::LANDED); _landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT); + // We did take off + if (landDetected && !_landDetected.landed) { + _takeoff_time = now; + + } else if (_takeoff_time != 0 && !landDetected && _landDetected.landed) { + _total_flight_time = now - _takeoff_time; + _takeoff_time = 0; + param_set(_p_total_flight_time, &_total_flight_time); + } + int instance; orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected, &instance, ORB_PRIO_DEFAULT); @@ -164,6 +177,7 @@ void LandDetector::_check_params(const bool force) if (updated || force) { _update_params(); + param_get(_p_total_flight_time, &_total_flight_time); } } @@ -191,8 +205,6 @@ void LandDetector::_update_state() } else { _state = LandDetectionState::FLYING; } - - return; } bool LandDetector::_orb_update(const struct orb_metadata *meta, int handle, void *buffer) diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 717106e400..47a127dd6b 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -166,6 +166,10 @@ private: bool _taskShouldExit; bool _taskIsRunning; + param_t _p_total_flight_time; + int32_t _total_flight_time; + hrt_abstime _takeoff_time; + struct work_s _work; }; diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 1297f45d60..e69b0d4265 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -174,3 +174,14 @@ PARAM_DEFINE_FLOAT(LNDFW_VELI_MAX, 4.0f); * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 8.00f); + +/** + * Total flight time in microseconds + * + * Total flight time of this autopilot. + * + * @min 0 + * @group Land Detector + * + */ +PARAM_DEFINE_INT32(LND_FLIGHT_TIME, 0);