From 2aff8787c17f2f82b6e39de8683a0d61338f23b3 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 18 Jul 2020 20:49:08 +0100 Subject: [PATCH] AP_Airspeed: add NMEA water speed backend --- libraries/AP_Airspeed/AP_Airspeed.cpp | 28 ++- libraries/AP_Airspeed/AP_Airspeed.h | 1 + libraries/AP_Airspeed/AP_Airspeed_Backend.h | 17 +- libraries/AP_Airspeed/AP_Airspeed_NMEA.cpp | 218 ++++++++++++++++++++ libraries/AP_Airspeed/AP_Airspeed_NMEA.h | 67 ++++++ 5 files changed, 321 insertions(+), 10 deletions(-) create mode 100644 libraries/AP_Airspeed/AP_Airspeed_NMEA.cpp create mode 100644 libraries/AP_Airspeed/AP_Airspeed_NMEA.h diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 3c2007d66b..fd1695cc8e 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include "AP_Airspeed.h" #include "AP_Airspeed_MS4525.h" #include "AP_Airspeed_MS5525.h" @@ -34,7 +35,9 @@ #if HAL_ENABLE_LIBUAVCAN_DRIVERS #include "AP_Airspeed_UAVCAN.h" #endif - +#if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) +#include "AP_Airspeed_NMEA.h" +#endif extern const AP_HAL::HAL &hal; #ifdef HAL_AIRSPEED_TYPE_DEFAULT @@ -77,7 +80,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 + // @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 // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 0, AP_Airspeed, param[0].type, ARSPD_DEFAULT_TYPE, AP_PARAM_FLAG_ENABLE), @@ -184,7 +187,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 + // @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 // @User: Standard AP_GROUPINFO_FLAGS("2_TYPE", 11, AP_Airspeed, param[1].type, 0, AP_PARAM_FLAG_ENABLE), @@ -353,6 +356,11 @@ void AP_Airspeed::init() case TYPE_UAVCAN: #if HAL_ENABLE_LIBUAVCAN_DRIVERS sensor[i] = AP_Airspeed_UAVCAN::probe(*this, i); +#endif + break; + 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; } @@ -457,17 +465,24 @@ void AP_Airspeed::update_calibration(uint8_t i, float raw_pressure) // read one airspeed sensor void AP_Airspeed::read(uint8_t i) { - float airspeed_pressure; if (!enabled(i) || !sensor[i]) { return; } + state[i].last_update_ms = AP_HAL::millis(); + + // try and get a direct reading of airspeed + if (sensor[i]->has_airspeed()) { + state[i].healthy = sensor[i]->get_airspeed(state[i].airspeed); + return; + } + bool prev_healthy = state[i].healthy; float raw_pressure = get_pressure(i); if (state[i].cal.start_ms != 0) { update_calibration(i, raw_pressure); } - - airspeed_pressure = raw_pressure - param[i].offset; + + float airspeed_pressure = raw_pressure - param[i].offset; // remember raw pressure for logging state[i].corrected_pressure = airspeed_pressure; @@ -511,7 +526,6 @@ void AP_Airspeed::read(uint8_t i) state[i].healthy = false; } - state[i].last_update_ms = AP_HAL::millis(); } // read all airspeed sensors diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 7c6f2f5038..ece7697001 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -156,6 +156,7 @@ public: TYPE_I2C_DLVR_20IN=10, TYPE_I2C_DLVR_30IN=11, TYPE_I2C_DLVR_60IN=12, + TYPE_NMEA_WATER=13, }; // get current primary sensor diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h index 73248d6802..0fbffc362c 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.h +++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.h @@ -31,11 +31,17 @@ public: virtual bool init(void) = 0; // return the current differential_pressure in Pascal - virtual bool get_differential_pressure(float &pressure) = 0; + virtual bool get_differential_pressure(float &pressure) {return false;} // return the current temperature in degrees C, if available virtual bool get_temperature(float &temperature) = 0; + // true if sensor reads airspeed directly, not via pressue + virtual bool has_airspeed() {return false;} + + // return airspeed in m/s if available + virtual bool get_airspeed(float& airspeed) {return false;} + protected: int8_t get_pin(void) const; float get_psi_range(void) const; @@ -44,7 +50,7 @@ protected: AP_Airspeed::pitot_tube_order get_tube_order(void) const { return AP_Airspeed::pitot_tube_order(frontend.param[instance].tube_order.get()); } - + // semaphore for access to shared frontend data HAL_Semaphore sem; @@ -66,7 +72,12 @@ protected: void set_offset(float ofs) { frontend.param[instance].offset.set(ofs); } - + + // set use + void set_use(int8_t use) { + frontend.param[instance].use.set(use); + } + private: AP_Airspeed &frontend; uint8_t instance; diff --git a/libraries/AP_Airspeed/AP_Airspeed_NMEA.cpp b/libraries/AP_Airspeed/AP_Airspeed_NMEA.cpp new file mode 100644 index 0000000000..39decdb259 --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_NMEA.cpp @@ -0,0 +1,218 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + * NMEA Sensor driver for VHW ans MTW messages over Serial + * https://gpsd.gitlab.io/gpsd/NMEA.html#_vhw_water_speed_and_heading + * https://gpsd.gitlab.io/gpsd/NMEA.html#_mtw_mean_temperature_of_water + */ + +#include +#if APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) + +#include "AP_Airspeed_NMEA.h" +#include "AP_Airspeed.h" + +#define TIMEOUT_MS 2000 + +extern const AP_HAL::HAL &hal; + +AP_Airspeed_NMEA::AP_Airspeed_NMEA(AP_Airspeed &_frontend, uint8_t _instance) : + AP_Airspeed_Backend(_frontend, _instance) +{ +} + +bool AP_Airspeed_NMEA::init() +{ + const AP_SerialManager& serial_manager = AP::serialmanager(); + + _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_AirSpeed, 0); + if (_uart == nullptr) { + return false; + } + + _uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_AirSpeed, 0)); + + // make sure this sensor cannot be used in the EKF + set_use(0); + + // must set use zero offset to pass offset check for health + set_use_zero_offset(); + + return true; +} + +// read the from the sensor +bool AP_Airspeed_NMEA::get_airspeed(float &airspeed) +{ + if (_uart == nullptr) { + return false; + } + + uint32_t now = AP_HAL::millis(); + + // read any available lines from the sensor + float sum = 0.0f; + uint16_t count = 0; + int16_t nbytes = _uart->available(); + while (nbytes-- > 0) { + char c = _uart->read(); + if (decode(c)) { + _last_update_ms = now; + if (_sentence_type == TYPE_VHW) { + sum += _speed; + count++; + } else { + _temp_sum += _temp; + _temp_count++; + } + } + } + + if (count == 0) { + // Cant return false because updates are too slow, return previous reading + // Could return false after some timeout, however testing shows that the DST800 just stops sending the message at zero speed + airspeed = _last_speed; + } else { + // return average of all measurements + airspeed = sum / count; + _last_speed = airspeed; + } + + return (now - _last_update_ms) < TIMEOUT_MS; +} + +// return the current temperature in degrees C +// the main update is done in the get_pressue function +// this just reports the value +bool AP_Airspeed_NMEA::get_temperature(float &temperature) +{ + if (_uart == nullptr) { + return false; + } + + if (_temp_count == 0) { + temperature = _last_temp; + } else { + // return average of all measurements + temperature = _temp_sum / _temp_count; + _last_temp = temperature; + _temp_count = 0; + _temp_sum = 0; + } + + return true; +} + + +// add a single character to the buffer and attempt to decode +// returns true if a complete sentence was successfully decoded +bool AP_Airspeed_NMEA::decode(char c) +{ + switch (c) { + case ',': + // end of a term, add to checksum + _checksum ^= c; + FALLTHROUGH; + case '\r': + case '\n': + case '*': + { + if (!_sentence_done && _sentence_valid) { + // null terminate and decode latest term + _term[_term_offset] = 0; + bool valid_sentence = decode_latest_term(); + + // move onto next term + _term_number++; + _term_offset = 0; + _term_is_checksum = (c == '*'); + return valid_sentence; + } + return false; + } + + case '$': // sentence begin + _term_number = 0; + _term_offset = 0; + _checksum = 0; + _term_is_checksum = false; + _sentence_done = false; + _sentence_valid = true; + return false; + } + + // ordinary characters are added to term + if (_term_offset < sizeof(_term) - 1) { + _term[_term_offset++] = c; + } + if (!_term_is_checksum) { + _checksum ^= c; + } + + return false; +} + +// decode the most recently consumed term +// returns true if new sentence has just passed checksum test and is validated +bool AP_Airspeed_NMEA::decode_latest_term() +{ + // handle the last term in a message + if (_term_is_checksum) { + _sentence_done = true; + uint8_t nibble_high = 0; + uint8_t nibble_low = 0; + if (!hex_to_uint8(_term[0], nibble_high) || !hex_to_uint8(_term[1], nibble_low)) { + return false; + } + const uint8_t checksum = (nibble_high << 4u) | nibble_low; + return checksum == _checksum; + } + + // the first term determines the sentence type + if (_term_number == 0) { + // the first two letters of the NMEA term are the talker ID. + // we accept any two characters here. + // actually expecting YX for MTW and VW for VHW + if (_term[0] < 'A' || _term[0] > 'Z' || + _term[1] < 'A' || _term[1] > 'Z') { + return false; + } + const char *term_type = &_term[2]; + if (strcmp(term_type, "MTW") == 0) { + _sentence_type = TPYE_MTW; + } else if (strcmp(term_type, "VHW") == 0) { + _sentence_type = TYPE_VHW; + } else { + _sentence_valid = false; + } + return false; + } + + if (_sentence_type == TPYE_MTW) { + // parse MTW messages + if (_term_number == 1) { + _temp = strtof(_term, NULL); + } + } else if (_sentence_type == TYPE_VHW) { + // parse VHW messages + if (_term_number == 7) { + _speed = strtof(_term, NULL) * KM_PER_HOUR_TO_M_PER_SEC; + } + } + + return false; +} + +#endif // APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub) diff --git a/libraries/AP_Airspeed/AP_Airspeed_NMEA.h b/libraries/AP_Airspeed/AP_Airspeed_NMEA.h new file mode 100644 index 0000000000..f54f2b2481 --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_NMEA.h @@ -0,0 +1,67 @@ +#pragma once + +#include "AP_Airspeed_Backend.h" +#include +#include + +class AP_Airspeed_NMEA : public AP_Airspeed_Backend +{ +public: + AP_Airspeed_NMEA(AP_Airspeed &frontend, uint8_t _instance); + + // probe and initialise the sensor + bool init(void) override; + + // this reads airspeed directly + bool has_airspeed() override {return true;} + + // read the from the sensor + bool get_airspeed(float &airspeed) override; + + // return the current temperature in degrees C + bool get_temperature(float &temperature) override; + + +private: + // pointer to serial uart + AP_HAL::UARTDriver *_uart = nullptr; + + // add a single character to the buffer and attempt to decode + // returns true if a complete sentence was successfully decoded + // distance should be pulled directly from _distance_m member + bool decode(char c); + + // decode the just-completed term + // returns true if new sentence has just passed checksum test and is validated + bool decode_latest_term(); + + // enum for handled messages + enum sentence_types : uint8_t { + TPYE_MTW = 0, + TYPE_VHW, + }; + + + // message decoding related members + char _term[15]; // buffer for the current term within the current sentence + uint8_t _term_offset; // offset within the _term buffer where the next character should be placed + uint8_t _term_number; // term index within the current sentence + float _speed; // speed in m/s + float _temp; // temp in deg c + uint8_t _checksum; // checksum accumulator + bool _term_is_checksum; // current term is the checksum + bool _sentence_done; // has the current term already been decoded + bool _sentence_valid; // is the decodeing valid so far + sentence_types _sentence_type; // the sentence type currently being processed + + // Store the temp ready for a temp request + float _temp_sum; + uint16_t _temp_count; + + // store last sent speed and temp as update rate is slow + float _last_temp; + float _last_speed; + + // time last message was received + uint32_t _last_update_ms; +};