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/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 1d0a6b92dc4d..976b6b7f0127 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -54,17 +54,20 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _armingSub(-1), _parameterSub(-1), _attitudeSub(-1), + _adcSub(-1), _vehicleGlobalPosition{}, _vehicleStatus{}, _actuators{}, _arming{}, _vehicleAttitude{}, + _adc{}, _landTimer(0) { _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 +79,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 +92,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() @@ -141,7 +146,22 @@ 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) { + // 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 we have any lateral movement. + if (landedSwitchOff || rotating || horizontalMovement) { + _landTimer = now; + return false; + } + + return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME_FAST; + } + + if (verticalMovement || rotating || !minimalThrust || horizontalMovement || landedSwitchOff) { // sensed movement, so reset the land detector _landTimer = now; return false; @@ -167,5 +187,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..eaa833887aee 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,8 +96,15 @@ class MulticopterLandDetector : public LandDetector float maxVelocity; float maxRotation; float maxThrottle; + 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; @@ -103,12 +112,14 @@ 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*/ }; diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 57e616169bcf..d6d0294db294 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_INT32(LNDMC_SWITCH_ENABLE, 0); + /** * Fixedwing max horizontal velocity * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ce6f7939ad4d..cffe6061be07 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 */ @@ -511,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")), @@ -1632,10 +1637,19 @@ 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) + /* 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);