diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg index be0cb98204..6ab67c5081 100644 --- a/msg/vehicle_land_detected.msg +++ b/msg/vehicle_land_detected.msg @@ -1,3 +1,4 @@ bool landed # true if vehicle is currently landed on the ground bool freefall # true if vehicle is currently in free-fall bool ground_contact # true if vehicle has ground contact but is not landed +float32 alt_max # maximum altitude in [m] that can be reached \ No newline at end of file diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 6e9eef4fb6..d0cd2ed490 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -92,6 +92,12 @@ void FixedwingLandDetector::_update_params() param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity); } +float FixedwingLandDetector::_get_max_altitude() +{ + //ToDo + return 0.0f; +} + bool FixedwingLandDetector::_get_freefall_state() { // TODO diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 7c963fce32..299b46590c 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -68,6 +68,8 @@ protected: virtual bool _get_ground_contact_state() override; virtual bool _get_freefall_state() override; + + virtual float _get_max_altitude() override; private: struct { param_t maxVelocity; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 11bbc7c3a9..c54dfdf474 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include "LandDetector.h" @@ -123,6 +124,9 @@ void LandDetector::_cycle() _update_state(); + float alt_max_prev = _altitude_max; + _update_max_altitude(); + bool freefallDetected = (_state == LandDetectionState::FREEFALL); bool landDetected = (_state == LandDetectionState::LANDED); bool ground_contactDetected = (_state == LandDetectionState::GROUND_CONTACT); @@ -132,7 +136,8 @@ void LandDetector::_cycle() if ((_landDetectedPub == nullptr) || (_landDetected.freefall != freefallDetected) || (_landDetected.landed != landDetected) || - (_landDetected.ground_contact != ground_contactDetected)) { + (_landDetected.ground_contact != ground_contactDetected) || + (fabsf(_landDetected.alt_max - alt_max_prev) > FLT_EPSILON)) { if (!landDetected && _landDetected.landed) { // We did take off @@ -152,6 +157,7 @@ void LandDetector::_cycle() _landDetected.freefall = (_state == LandDetectionState::FREEFALL); _landDetected.landed = (_state == LandDetectionState::LANDED); _landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT); + _landDetected.alt_max = _altitude_max; int instance; orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected, @@ -168,7 +174,6 @@ void LandDetector::_cycle() _taskIsRunning = false; } } - void LandDetector::_check_params(const bool force) { bool updated; @@ -216,6 +221,12 @@ void LandDetector::_update_state() } } +void LandDetector::_update_max_altitude() +{ + _altitude_max = _get_max_altitude(); +} + + bool LandDetector::_orb_update(const struct orb_metadata *meta, int handle, void *buffer) { bool newData = false; diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 857cf74e10..01ba9a5a58 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -61,6 +61,10 @@ public: GROUND_CONTACT = 3 }; + enum class BatteryLevel { + + }; + LandDetector(); virtual ~LandDetector(); @@ -123,6 +127,11 @@ protected: */ virtual bool _get_freefall_state() = 0; + /** + * @return + */ + virtual float _get_max_altitude() = 0; + /** * Convenience function for polling uORB subscriptions. * @@ -153,6 +162,9 @@ protected: systemlib::Hysteresis _landed_hysteresis; systemlib::Hysteresis _ground_contact_hysteresis; + float _altitude_max; + + private: static void _cycle_trampoline(void *arg); @@ -163,6 +175,9 @@ private: void _update_state(); + void _update_max_altitude(); + + bool _taskShouldExit; bool _taskIsRunning; @@ -171,6 +186,7 @@ private: uint64_t _total_flight_time; ///< in microseconds hrt_abstime _takeoff_time; + struct work_s _work; }; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 5e64325b72..58a508d1a0 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -59,6 +59,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _manualSub(-1), _ctrl_state_sub(-1), _vehicle_control_mode_sub(-1), + _battery_sub(-1), _vehicleLocalPosition{}, _actuators{}, _arming{}, @@ -66,6 +67,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _manual{}, _ctrl_state{}, _control_mode{}, + _battery{}, _min_trust_start(0), _arming_time(0) { @@ -79,6 +81,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _paramHandle.freefall_acc_threshold = param_find("LNDMC_FFALL_THR"); _paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI"); _paramHandle.manual_stick_down_threshold = param_find("LNDMC_MAN_DWNTHR"); + _paramHandle.altitude_max = param_find("LNDMC_ALT_MAX"); } void MulticopterLandDetector::_initialize_topics() @@ -92,6 +95,7 @@ void MulticopterLandDetector::_initialize_topics() _manualSub = orb_subscribe(ORB_ID(manual_control_setpoint)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _battery_sub = orb_subscribe(ORB_ID(battery_status)); } void MulticopterLandDetector::_update_topics() @@ -103,6 +107,7 @@ void MulticopterLandDetector::_update_topics() _orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual); _orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); _orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode); + _orb_update(ORB_ID(battery_status), _battery_sub, &_battery); } void MulticopterLandDetector::_update_params() @@ -119,6 +124,8 @@ void MulticopterLandDetector::_update_params() param_get(_paramHandle.freefall_trigger_time, &_params.freefall_trigger_time); _freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _params.freefall_trigger_time)); param_get(_paramHandle.manual_stick_down_threshold, &_params.manual_stick_down_threshold); + param_get(_paramHandle.altitude_max, &_params.altitude_max); + } @@ -279,6 +286,26 @@ float MulticopterLandDetector::_get_takeoff_throttle() return 0.0f; } +float MulticopterLandDetector::_get_max_altitude() +{ + /* ToDo: add meaningful heights */ + float valid_altitude_max = _params.altitude_max; + + if (_battery.warning == battery_status_s::BATTERY_WARNING_LOW) { + valid_altitude_max = _params.altitude_max * 0.75f; + } + + if (_battery.warning == battery_status_s::BATTERY_WARNING_CRITICAL) { + valid_altitude_max = _params.altitude_max * 0.5f; + } + + if (_battery.warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { + valid_altitude_max = _params.altitude_max * 0.25f; + } + + return valid_altitude_max; +} + bool MulticopterLandDetector::_has_position_lock() { return !(_vehicleLocalPosition.timestamp == 0 || diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index fc9104dab9..65d20a5bcc 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -51,6 +51,7 @@ #include #include #include +#include #include "LandDetector.h" @@ -74,6 +75,8 @@ protected: virtual bool _get_ground_contact_state() override; virtual bool _get_freefall_state() override; + + virtual float _get_max_altitude() override; private: /** @@ -90,6 +93,7 @@ private: param_t freefall_acc_threshold; param_t freefall_trigger_time; param_t manual_stick_down_threshold; + param_t altitude_max; } _paramHandle; struct { @@ -103,6 +107,7 @@ private: float freefall_acc_threshold; float freefall_trigger_time; float manual_stick_down_threshold; + float altitude_max; } _params; int _vehicleLocalPositionSub; @@ -112,6 +117,7 @@ private: int _manualSub; int _ctrl_state_sub; int _vehicle_control_mode_sub; + int _battery_sub; struct vehicle_local_position_s _vehicleLocalPosition; struct actuator_controls_s _actuators; @@ -120,6 +126,7 @@ private: struct manual_control_setpoint_s _manual; struct control_state_s _ctrl_state; struct vehicle_control_mode_s _control_mode; + struct battery_status_s _battery; uint64_t _min_trust_start; ///< timestamp when minimum trust was applied first uint64_t _arming_time; diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 61a15eb1fb..875947510b 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -213,3 +213,15 @@ PARAM_DEFINE_INT32(LND_FLIGHT_T_HI, 0); * */ PARAM_DEFINE_INT32(LND_FLIGHT_T_LO, 0); + +/** + * Maximum altitude that can be reached prior to subconditions + * + * @unit m/s + * @min 10 + * @max 150 + * @decimal 2 + * @group Land Detector + * + */ +PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, 15.0f);