From a0afc370d0dbbaab301ef837c715094862ad93ae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 7 Dec 2017 11:14:08 +0100 Subject: [PATCH] land detector: move arming state into base class & set param when disarming 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. --- .../land_detector/FixedwingLandDetector.cpp | 2 -- .../land_detector/FixedwingLandDetector.h | 3 -- src/modules/land_detector/LandDetector.cpp | 28 +++++++++++-------- src/modules/land_detector/LandDetector.h | 6 ++++ .../land_detector/MulticopterLandDetector.cpp | 6 +--- .../land_detector/MulticopterLandDetector.h | 3 -- 6 files changed, 24 insertions(+), 24 deletions(-) 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;