diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 33d344a4d8..17bd30651a 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -62,7 +62,6 @@ FixedwingLandDetector::FixedwingLandDetector() void FixedwingLandDetector::_initialize_topics() { - _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _airspeedSub = orb_subscribe(ORB_ID(airspeed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); @@ -70,7 +69,6 @@ void FixedwingLandDetector::_initialize_topics() void FixedwingLandDetector::_update_topics() { - _orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); _orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); _orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors); _orb_update(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 20e1ac31d8..1a7e420e4a 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -42,7 +42,6 @@ #pragma once -#include #include #include #include @@ -86,12 +85,10 @@ private: float maxIntVelocity; } _params{}; - int _armingSub{-1}; int _airspeedSub{-1}; int _sensor_bias_sub{-1}; int _local_pos_sub{-1}; - actuator_armed_s _arming{}; airspeed_s _airspeed{}; sensor_bias_s _sensors{}; vehicle_local_position_s _local_pos{}; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 26d7cb4882..0fe2716f9b 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -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()) { diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 1604de4ecf..aadb4e32fa 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -48,6 +48,7 @@ #include #include #include +#include #include namespace land_detector @@ -150,6 +151,7 @@ protected: vehicle_land_detected_s _landDetected{}; int _parameterSub{-1}; + int _armingSub{-1}; LandDetectionState _state{LandDetectionState::LANDED}; @@ -158,6 +160,8 @@ protected: systemlib::Hysteresis _maybe_landed_hysteresis{true}; systemlib::Hysteresis _ground_contact_hysteresis{true}; + struct actuator_armed_s _arming {}; + private: static void _cycle_trampoline(void *arg); @@ -175,6 +179,8 @@ private: struct work_s _work {}; perf_counter_t _cycle_perf; + + bool _previous_arming_state{false}; ///< stores the previous _arming.armed state }; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 34b7281875..ab14652395 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -77,7 +77,6 @@ MulticopterLandDetector::MulticopterLandDetector() : _vehicleLocalPositionSub(-1), _vehicleLocalPositionSetpointSub(-1), _actuatorsSub(-1), - _armingSub(-1), _attitudeSub(-1), _sensor_bias_sub(-1), _vehicle_control_mode_sub(-1), @@ -85,7 +84,6 @@ MulticopterLandDetector::MulticopterLandDetector() : _vehicleLocalPosition{}, _vehicleLocalPositionSetpoint{}, _actuators{}, - _arming{}, _vehicleAttitude{}, _sensors{}, _control_mode{}, @@ -113,12 +111,11 @@ MulticopterLandDetector::MulticopterLandDetector() : void MulticopterLandDetector::_initialize_topics() { - // subscribe to position, attitude, arming and velocity changes + // subscribe to position, attitude and velocity changes _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); _vehicleLocalPositionSetpointSub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); _attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude)); _actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0)); - _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _parameterSub = orb_subscribe(ORB_ID(parameter_update)); _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -131,7 +128,6 @@ void MulticopterLandDetector::_update_topics() _orb_update(ORB_ID(vehicle_local_position_setpoint), _vehicleLocalPositionSetpointSub, &_vehicleLocalPositionSetpoint); _orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); _orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators); - _orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); _orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors); _orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode); _orb_update(ORB_ID(battery_status), _battery_sub, &_battery); diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index de0b1090b7..bf1e5e4326 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -49,7 +49,6 @@ #include #include #include -#include #include #include #include @@ -130,7 +129,6 @@ private: int _vehicleLocalPositionSub; int _vehicleLocalPositionSetpointSub; int _actuatorsSub; - int _armingSub; int _attitudeSub; int _sensor_bias_sub; int _vehicle_control_mode_sub; @@ -139,7 +137,6 @@ private: struct vehicle_local_position_s _vehicleLocalPosition; struct vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint; struct actuator_controls_s _actuators; - struct actuator_armed_s _arming; struct vehicle_attitude_s _vehicleAttitude; struct sensor_bias_s _sensors; struct vehicle_control_mode_s _control_mode;