From 857d905d9fb03b864bb00a7b60f939a8f8620d89 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 16 Nov 2020 21:35:15 +1100 Subject: [PATCH] AP_Airspeed: added MSP backend --- libraries/AP_Airspeed/AP_Airspeed.cpp | 36 ++++++++++-- libraries/AP_Airspeed/AP_Airspeed.h | 9 +++ libraries/AP_Airspeed/AP_Airspeed_Backend.h | 4 ++ libraries/AP_Airspeed/AP_Airspeed_MSP.cpp | 63 +++++++++++++++++++++ libraries/AP_Airspeed/AP_Airspeed_MSP.h | 35 ++++++++++++ 5 files changed, 142 insertions(+), 5 deletions(-) create mode 100644 libraries/AP_Airspeed/AP_Airspeed_MSP.cpp create mode 100644 libraries/AP_Airspeed/AP_Airspeed_MSP.h diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 137709ff78..98a9adc25a 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -35,9 +35,12 @@ #if HAL_ENABLE_LIBUAVCAN_DRIVERS #include "AP_Airspeed_UAVCAN.h" #endif -#if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) +#if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) #include "AP_Airspeed_NMEA.h" #endif +#if HAL_MSP_AIRSPEED_ENABLED +#include "AP_Airspeed_MSP.h" +#endif extern const AP_HAL::HAL &hal; #ifdef HAL_AIRSPEED_TYPE_DEFAULT @@ -80,7 +83,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @Param: _TYPE // @DisplayName: Airspeed type // @Description: Type of airspeed sensor - // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed + // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 0, AP_Airspeed, param[0].type, ARSPD_DEFAULT_TYPE, AP_PARAM_FLAG_ENABLE), @@ -187,7 +190,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @Param: 2_TYPE // @DisplayName: Second Airspeed type // @Description: Type of 2nd airspeed sensor - // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed + // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:UAVCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP // @User: Standard AP_GROUPINFO_FLAGS("2_TYPE", 11, AP_Airspeed, param[1].type, 0, AP_PARAM_FLAG_ENABLE), @@ -274,7 +277,7 @@ AP_Airspeed::AP_Airspeed() } void AP_Airspeed::init() -{ +{ if (sensor[0] != nullptr) { // already initialised return; @@ -361,10 +364,14 @@ void AP_Airspeed::init() case TYPE_NMEA_WATER: #if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) sensor[i] = new AP_Airspeed_NMEA(*this, i); +#endif + break; + case TYPE_MSP: +#if HAL_MSP_AIRSPEED_ENABLED + sensor[i] = new AP_Airspeed_MSP(*this, i, 0); #endif break; } - if (sensor[i] && !sensor[i]->init()) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u init failed", i + 1); delete sensor[i]; @@ -564,6 +571,25 @@ void AP_Airspeed::update(bool log) #endif } +#if HAL_MSP_AIRSPEED_ENABLED +/* + handle MSP airspeed data + */ +void AP_Airspeed::handle_msp(const MSP::msp_airspeed_data_message_t &pkt) +{ + + if (pkt.instance > 1) { + return; //supporting 2 airspeed sensors at most + } + + for (uint8_t i=0; ihandle_msp(pkt); + } + } +} +#endif + void AP_Airspeed::Log_Airspeed() { const uint64_t now = AP_HAL::micros64(); diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index bd44f6d281..1d6e5b7c6b 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -5,6 +5,7 @@ #include #include #include +#include class AP_Airspeed_Backend; @@ -16,6 +17,9 @@ class AP_Airspeed_Backend; #define AP_AIRSPEED_AUTOCAL_ENABLE !defined(HAL_BUILD_AP_PERIPH) #endif +#ifndef HAL_MSP_AIRSPEED_ENABLED +#define HAL_MSP_AIRSPEED_ENABLED HAL_MSP_SENSORS_ENABLED +#endif class Airspeed_Calibration { public: friend class AP_Airspeed; @@ -162,6 +166,7 @@ public: TYPE_I2C_DLVR_30IN=11, TYPE_I2C_DLVR_60IN=12, TYPE_NMEA_WATER=13, + TYPE_MSP=14, }; // get current primary sensor @@ -179,6 +184,10 @@ public: float get_corrected_pressure(void) const { return get_corrected_pressure(primary); } + +#if HAL_MSP_AIRSPEED_ENABLED + void handle_msp(const MSP::msp_airspeed_data_message_t &pkt); +#endif private: static AP_Airspeed *_singleton; diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h index 0fbffc362c..59528d1586 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.h +++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.h @@ -42,6 +42,10 @@ public: // return airspeed in m/s if available virtual bool get_airspeed(float& airspeed) {return false;} +#if HAL_MSP_AIRSPEED_ENABLED + virtual void handle_msp(const MSP::msp_airspeed_data_message_t &pkt) {} +#endif + protected: int8_t get_pin(void) const; float get_psi_range(void) const; diff --git a/libraries/AP_Airspeed/AP_Airspeed_MSP.cpp b/libraries/AP_Airspeed/AP_Airspeed_MSP.cpp new file mode 100644 index 0000000000..2e077d973d --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_MSP.cpp @@ -0,0 +1,63 @@ +#include "AP_Airspeed_MSP.h" + +#if HAL_MSP_AIRSPEED_ENABLED + +AP_Airspeed_MSP::AP_Airspeed_MSP(AP_Airspeed &_frontend, uint8_t _instance, uint8_t _msp_instance) : + AP_Airspeed_Backend(_frontend, _instance), + msp_instance(_msp_instance) +{ +} + +// return the current differential_pressure in Pascal +bool AP_Airspeed_MSP::get_differential_pressure(float &pressure) +{ + WITH_SEMAPHORE(sem); + if (press_count == 0) { + return false; + } + pressure = sum_pressure/press_count; + press_count = 0; + sum_pressure = 0; + return true; +} + +// get last temperature +bool AP_Airspeed_MSP::get_temperature(float &temperature) +{ + WITH_SEMAPHORE(sem); + if (temp_count == 0) { + return false; + } + temperature = sum_temp/temp_count; + temp_count = 0; + sum_temp = 0; + return true; +} + +void AP_Airspeed_MSP::handle_msp(const MSP::msp_airspeed_data_message_t &pkt) +{ + if (pkt.instance != msp_instance) { + // not for us + return; + } + WITH_SEMAPHORE(sem); + + sum_pressure += pkt.pressure; + press_count++; + if (press_count > 100) { + // prevent overflow + sum_pressure /= 2; + press_count /= 2; + } + + sum_temp += pkt.temp*0.01; + temp_count++; + if (temp_count > 100) { + // prevent overflow + sum_temp /= 2; + temp_count /= 2; + } + +} + +#endif // HAL_MSP_AIRSPEED_ENABLED diff --git a/libraries/AP_Airspeed/AP_Airspeed_MSP.h b/libraries/AP_Airspeed/AP_Airspeed_MSP.h new file mode 100644 index 0000000000..f0bf4d590f --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_MSP.h @@ -0,0 +1,35 @@ +/* + MSP airspeed backend + */ +#pragma once + +#include "AP_Airspeed_Backend.h" + +#if HAL_MSP_AIRSPEED_ENABLED + +class AP_Airspeed_MSP : public AP_Airspeed_Backend +{ +public: + AP_Airspeed_MSP(AP_Airspeed &airspeed, uint8_t instance, uint8_t msp_instance); + + bool init(void) override { + return true; + } + + void handle_msp(const MSP::msp_airspeed_data_message_t &pkt) override; + + // return the current differential_pressure in Pascal + bool get_differential_pressure(float &pressure) override; + + // temperature not available via analog backend + bool get_temperature(float &temperature) override; + +private: + const uint8_t msp_instance; + float sum_pressure; + uint8_t press_count; + float sum_temp; + uint8_t temp_count; +}; + +#endif // HAL_MSP_AIRSPEED_ENABLED