From 84f7a0f0c25076ded9b5314f09c639bac8044a1b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 29 Sep 2015 23:23:31 +0200 Subject: [PATCH 1/3] Extend the landing detector to support pressure switches. --- msg/adc.msg | 5 ++++ .../land_detector/MulticopterLandDetector.cpp | 29 +++++++++++++++++-- .../land_detector/MulticopterLandDetector.h | 6 ++++ .../land_detector/land_detector_params.c | 13 +++++++++ src/modules/sensors/sensors.cpp | 15 ++++++++++ src/modules/uORB/objects_common.cpp | 3 ++ 6 files changed, 69 insertions(+), 2 deletions(-) create mode 100644 msg/adc.msg diff --git a/msg/adc.msg b/msg/adc.msg new file mode 100644 index 000000000000..4d51364753b3 --- /dev/null +++ b/msg/adc.msg @@ -0,0 +1,5 @@ +float32 virtual_pin_13 # Virtual pin 13 on ADC 3.3V connector pin 4. +float32 virtual_pin_14 # Virtual pin 14 on ADC 3.3V connector pin 2. +float32 virtual_pin_15 # Virtual pin 15 on ADC 6.6V connector pin 2. +uint64 timestamp +uint64 error_count diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 1d0a6b92dc4d..6c1ade1a56b3 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -54,17 +54,21 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _armingSub(-1), _parameterSub(-1), _attitudeSub(-1), + _adcSub(-1), _vehicleGlobalPosition{}, _vehicleStatus{}, _actuators{}, _arming{}, _vehicleAttitude{}, - _landTimer(0) + _adc{}, + _landTimer(0), + _landedSwitchHealty(true) { _paramHandle.maxRotation = param_find("LNDMC_ROT_MAX"); _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); _paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX"); _paramHandle.maxThrottle = param_find("LNDMC_THR_MAX"); + _paramHandle.landingSwitchEnable = param_find("LNDMC_SWITCH_ENABLE"); } void MulticopterLandDetector::initialize() @@ -76,6 +80,7 @@ void MulticopterLandDetector::initialize() _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _parameterSub = orb_subscribe(ORB_ID(parameter_update)); + _adcSub = orb_subscribe(ORB_ID(adc)); // download parameters updateParameterCache(true); @@ -88,6 +93,7 @@ void MulticopterLandDetector::updateSubscriptions() orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus); orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + orb_update(ORB_ID(adc), _adcSub, &_adc); } bool MulticopterLandDetector::update() @@ -121,6 +127,14 @@ bool MulticopterLandDetector::update() armThresholdFactor = 2.5f; } + // Check if landing gear switch support is enabled and if they are on. + bool landedSwitchOn = (_landedSwitchHealthy && _paramHandle.landingSwitchEnable > 0f && _adc.virtual_pin_15 > 4.5f); + + // If set via parameter, signal landing detection state using only the switch immediately. + if (_paramHandle.landingSwitchEnable == 2f) { + return landedSwitchOn; + } + // check if we are moving vertically - this might see a spike after arming due to // throttle-up vibration. If accelerating fast the throttle thresholds will still give // an accurate in-air indication @@ -141,7 +155,17 @@ bool MulticopterLandDetector::update() // check if thrust output is minimal (about half of default) bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; - if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + // Let's do some paranoid checking until we really trust switch triggered landing detection and check if the switches + // indicate we are landed but everything else says we are in air. + if (_landedSwitchHealthy && landedSwitchOn && (verticalMovement || rotating || !minimalThrust || horizontalMovement)) { + // Everything else says we are not landed, let's assume the switch is not healthy. + _landedSwitchHealthy = false; + landedSwitchOff = false; + warnx("The landing switch system appears to have failed. Ignoring."); + } + + + if (verticalMovement || rotating || !minimalThrust || horizontalMovement || !landedSwitchOn) { // sensed movement, so reset the land detector _landTimer = now; return false; @@ -167,5 +191,6 @@ void MulticopterLandDetector::updateParameterCache(const bool force) param_get(_paramHandle.maxRotation, &_params.maxRotation); _params.maxRotation = math::radians(_params.maxRotation); param_get(_paramHandle.maxThrottle, &_params.maxThrottle); + param_get(_paramHandle.landingSwitchEnable, &_params.landingSwitchEnable); } } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index d001be4e7f49..385d0e51c186 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -49,6 +49,7 @@ #include #include #include +#include #include class MulticopterLandDetector : public LandDetector @@ -87,6 +88,7 @@ class MulticopterLandDetector : public LandDetector param_t maxVelocity; param_t maxRotation; param_t maxThrottle; + param_t landingSwitchEnable; } _paramHandle; struct { @@ -94,6 +96,7 @@ class MulticopterLandDetector : public LandDetector float maxVelocity; float maxRotation; float maxThrottle; + bool landingSwitchEnable; } _params; private: @@ -103,14 +106,17 @@ class MulticopterLandDetector : public LandDetector int _armingSub; int _parameterSub; int _attitudeSub; + int _adcSub; struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ struct vehicle_status_s _vehicleStatus; struct actuator_controls_s _actuators; struct actuator_armed_s _arming; struct vehicle_attitude_s _vehicleAttitude; + struct adc_s _adc; uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ + bool _landedSwitchHealthy; }; #endif //__MULTICOPTER_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 57e616169bcf..ae2b8ce3db85 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -85,6 +85,19 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); */ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.15f); +/** + * Multicopter landing switch + * + * Enable landing switches if present as an additional signal by setting to 1. + * To trigger landing state based on the switch state immediately set to 2. + * + * @min 0 + * @max 2 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_SWITCH_ENABLE, 0f); + /** * Fixedwing max horizontal velocity * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ce6f7939ad4d..591ebea2c94b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -94,6 +94,7 @@ #include #include #include +#include #include #include @@ -215,6 +216,7 @@ class Sensors int _rc_sub; /**< raw rc channels data subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ + int _adc_sub; /**< ADC subscription */ int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _rc_parameter_map_sub; /**< rc parameter map subscription */ @@ -227,6 +229,7 @@ class Sensors orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ orb_advert_t _diff_pres_pub; /**< differential_pressure */ + orb_advert_t _adc_pub; /**< ADC */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -234,6 +237,7 @@ class Sensors struct battery_status_s _battery_status; /**< battery status */ struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _diff_pres; + struct adc_s _adc; /**< ADC raw values */ struct airspeed_s _airspeed; struct rc_parameter_map_s _rc_parameter_map; float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */ @@ -1636,6 +1640,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* calculate airspeed, raw is the difference from */ float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor) + /* Announce the ADC state if needed, otherwise just publish */ + _adc.timestamp = t; + _adc.virtual_pin_15 = voltage; + + if (_adc_pub != nullptr) { + orb_publish(ORB_ID(adc), _adc_pub, &_adc); + } else { + _adc_pub = orb_advertise(ORB_ID(adc), &_adc); + } + /** * The voltage divider pulls the signal down, only act on * a valid voltage from a connected sensor. Also assume a non- @@ -2021,6 +2035,7 @@ Sensors::task_main() _rc_sub = orb_subscribe(ORB_ID(input_rc)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + _adc_sub = orb_subscribe(ORB_ID(adc)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index ac980487712f..5f97caf385c9 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -263,3 +263,6 @@ ORB_DEFINE(distance_sensor, struct distance_sensor_s); #include "topics/camera_trigger.h" ORB_DEFINE(camera_trigger, struct camera_trigger_s); + +#include "topics/adc.h" +ORB_DEFINE(adc, struct adc_s); From e0e5bd8f00bb718bcf2124924f3a5382ac0639b5 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 30 Sep 2015 15:46:50 +0200 Subject: [PATCH 2/3] Remove attempted switch fault detection. Needs more work to be reliable enough. --- src/modules/land_detector/LandDetector.h | 5 +-- .../land_detector/MulticopterLandDetector.cpp | 34 ++++++++----------- .../land_detector/MulticopterLandDetector.h | 9 +++-- .../land_detector/land_detector_params.c | 2 +- src/modules/sensors/sensors.cpp | 2 +- 5 files changed, 27 insertions(+), 25 deletions(-) diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 2ade4ac8c0e9..8265c1e714db 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -94,8 +94,9 @@ class LandDetector static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ - static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold - before triggering a land */ + static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_FAST = 1000000; /**< usec that landing conditions have to hold when confident before triggering a land */ + static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = 1000000; /**< time interval in which wider acceptance thresholds are used after arming */ protected: diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 6c1ade1a56b3..8afdc58b3d11 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -61,8 +61,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _arming{}, _vehicleAttitude{}, _adc{}, - _landTimer(0), - _landedSwitchHealty(true) + _landTimer(0) { _paramHandle.maxRotation = param_find("LNDMC_ROT_MAX"); _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); @@ -127,14 +126,6 @@ bool MulticopterLandDetector::update() armThresholdFactor = 2.5f; } - // Check if landing gear switch support is enabled and if they are on. - bool landedSwitchOn = (_landedSwitchHealthy && _paramHandle.landingSwitchEnable > 0f && _adc.virtual_pin_15 > 4.5f); - - // If set via parameter, signal landing detection state using only the switch immediately. - if (_paramHandle.landingSwitchEnable == 2f) { - return landedSwitchOn; - } - // check if we are moving vertically - this might see a spike after arming due to // throttle-up vibration. If accelerating fast the throttle thresholds will still give // an accurate in-air indication @@ -155,17 +146,22 @@ bool MulticopterLandDetector::update() // check if thrust output is minimal (about half of default) bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; - // Let's do some paranoid checking until we really trust switch triggered landing detection and check if the switches - // indicate we are landed but everything else says we are in air. - if (_landedSwitchHealthy && landedSwitchOn && (verticalMovement || rotating || !minimalThrust || horizontalMovement)) { - // Everything else says we are not landed, let's assume the switch is not healthy. - _landedSwitchHealthy = false; - landedSwitchOff = false; - warnx("The landing switch system appears to have failed. Ignoring."); - } + // Check if landing gear switch support is enabled, if the switch(es) seem to be working and if they are active/on. + bool landedSwitchOff = (_params.landingSwitchEnable != SWITCH_OFF && _adc.virtual_pin_15 < 4.5f); + + // If set via parameter, signal landing detection state using only the switch after a shorter wait time. We + // can't trigger immediately as there might be some bouncing on landing. + if (_params.landingSwitchEnable == SWITCH_TRUST) { + // Remain in a non-landed state if the switch is off or if something has gone wrong with the switches. + if (landedSwitchOff) { + _landTimer = now; + return false; + } + return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME_FAST; + } - if (verticalMovement || rotating || !minimalThrust || horizontalMovement || !landedSwitchOn) { + if (verticalMovement || rotating || !minimalThrust || horizontalMovement || landedSwitchOff) { // sensed movement, so reset the land detector _landTimer = now; return false; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 385d0e51c186..eaa833887aee 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -96,9 +96,15 @@ class MulticopterLandDetector : public LandDetector float maxVelocity; float maxRotation; float maxThrottle; - bool landingSwitchEnable; + int32_t landingSwitchEnable; } _params; + enum { + SWITCH_OFF = 0, // Disable the switch, it will be ignored. + SWITCH_ON = 1, // Use switch in combination with other signals. + SWITCH_TRUST = 2 // Fully trust the switch, disregard other signals. + }; + private: int _vehicleGlobalPositionSub; /**< notification of global position */ int _vehicleStatusSub; @@ -116,7 +122,6 @@ class MulticopterLandDetector : public LandDetector struct adc_s _adc; uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ - bool _landedSwitchHealthy; }; #endif //__MULTICOPTER_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index ae2b8ce3db85..d6d0294db294 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -96,7 +96,7 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.15f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_SWITCH_ENABLE, 0f); +PARAM_DEFINE_INT32(LNDMC_SWITCH_ENABLE, 0); /** * Fixedwing max horizontal velocity diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 591ebea2c94b..cffe6061be07 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -515,6 +515,7 @@ Sensors::Sensors() : _battery_pub(nullptr), _airspeed_pub(nullptr), _diff_pres_pub(nullptr), + _adc_pub(nullptr), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), @@ -1636,7 +1637,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _battery_current_timestamp = t; } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { - /* calculate airspeed, raw is the difference from */ float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor) From 5c6eb6adf2ace7339c84d580d16dccecc5b83b4e Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 1 Oct 2015 07:21:57 +0200 Subject: [PATCH 3/3] Still look for some movement when using a switch only landing trigger. --- src/modules/land_detector/MulticopterLandDetector.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 8afdc58b3d11..976b6b7f0127 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -152,8 +152,8 @@ bool MulticopterLandDetector::update() // If set via parameter, signal landing detection state using only the switch after a shorter wait time. We // can't trigger immediately as there might be some bouncing on landing. if (_params.landingSwitchEnable == SWITCH_TRUST) { - // Remain in a non-landed state if the switch is off or if something has gone wrong with the switches. - if (landedSwitchOff) { + // Remain in a non-landed state if the switch is off or we have any lateral movement. + if (landedSwitchOff || rotating || horizontalMovement) { _landTimer = now; return false; }