From 039b295c66c00f90789328a9067d7274477571cd Mon Sep 17 00:00:00 2001 From: davidaroyer Date: Wed, 6 Sep 2017 12:43:33 -0500 Subject: [PATCH] AP_RangeFinder: update uLanding driver for new firmware data format --- .../AP_RangeFinder_uLanding.cpp | 140 ++++++++++++++++-- .../AP_RangeFinder/AP_RangeFinder_uLanding.h | 8 +- 2 files changed, 134 insertions(+), 14 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp index d4c6b566c0..074fece147 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp @@ -18,6 +18,9 @@ #include #include +#define ULANDING_HDR 254 // Header Byte from uLanding (0xFE) +#define ULANDING_HDR_V0 72 // Header Byte for beta V0 of uLanding (0x48) + extern const AP_HAL::HAL& hal; /* @@ -46,6 +49,87 @@ bool AP_RangeFinder_uLanding::detect(RangeFinder &_ranger, uint8_t instance, AP_ return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Aerotenna_uLanding, 0) != nullptr; } +/* + detect uLanding Firmware Version +*/ +bool AP_RangeFinder_uLanding::detect_version(void) +{ + if (_version_known) { + // return true if we've already detected the uLanding version + return true; + } else if (uart == nullptr) { + return false; + } + + bool hdr_found = false; + uint8_t byte1 = 0; + uint8_t count = 0; + + // read any available data from uLanding + int16_t nbytes = uart->available(); + + while (nbytes-- > 0) { + uint8_t c = uart->read(); + + if (((c == ULANDING_HDR_V0) || (c == ULANDING_HDR)) && !hdr_found) { + byte1 = c; + hdr_found = true; + count++; + } else if (hdr_found) { + if (byte1 == ULANDING_HDR_V0) { + if (++count < 4) { + /* need to collect 4 bytes to check for recurring + * header byte in the old 3-byte data format + */ + continue; + } else { + if (c == byte1) { + // if header byte is recurring, set uLanding Version + _version = 0; + _header = ULANDING_HDR_V0; + _version_known = true; + return true; + } else { + /* if V0 header byte didn't occur again on 4th byte, + * start the search again for a header byte + */ + count = 0; + byte1 = 0; + hdr_found = false; + } + } + } else { + if ((c & 0x80) || (c == ULANDING_HDR_V0)) { + /* Though unlikely, it is possible we could find ULANDING_HDR + * in a data byte from the old 3-byte format. In this case, + * either the next byte is another data byte (which by default + * is of the form 0x1xxxxxxx), or the next byte is the old + * header byte (ULANDING_HDR_V0). In this case, start the search + * again for a header byte. + */ + count = 0; + byte1 = 0; + hdr_found = false; + } else { + /* if this second byte passes the above if statement, this byte + * is the version number + */ + _version = c; + _header = ULANDING_HDR; + _version_known = true; + return true; + } + } + } + } + + /* return false if we've gone through all available data + * and haven't detected a uLanding firmware version + */ + return false; +} + + // read - return last value measured by sensor bool AP_RangeFinder_uLanding::get_reading(uint16_t &reading_cm) { @@ -53,28 +137,53 @@ bool AP_RangeFinder_uLanding::get_reading(uint16_t &reading_cm) return false; } + + if (!detect_version()) { + // return false if uLanding version check failed + return false; + } + // read any available lines from the uLanding float sum = 0; uint16_t count = 0; - uint8_t index = 0; + bool hdr_found = false; int16_t nbytes = uart->available(); + while (nbytes-- > 0) { uint8_t c = uart->read(); - // ok, we have located start byte - if (c == 72 && index == 0) { + + if ((c == _header) && !hdr_found) { + // located header byte linebuf_len = 0; - index = 1; + hdr_found = true; } - // now it is ready to decode index information - if (index == 1) { - linebuf[linebuf_len] = c; - linebuf_len ++; - if (linebuf_len == 3) { - index = 0; - sum += (linebuf[2]&0x7F) *128 + (linebuf[1]&0x7F); + // decode index information + if (hdr_found) { + linebuf[linebuf_len++] = c; + + /* don't process linebuf until we've collected six bytes of data + * (or 3 bytes for Version 0 firmware) + */ + if ((linebuf_len < (sizeof(linebuf)/sizeof(linebuf[0]))) || + (_version == 0 && linebuf_len < 3)) { + continue; + } else { + if (_version == 0) { + // parse data from Firmware Version #0 + sum += (linebuf[2]&0x7F)*128 + (linebuf[1]&0x7F); + count++; + } else { + // evaluate checksum + if (((linebuf[1] + linebuf[2] + linebuf[3] + linebuf[4]) & 0xFF) == linebuf[5]) { + // if checksum passed, parse data from Firmware Version #1 + sum += linebuf[3]*256 + linebuf[2]; + count++; + } + } + + hdr_found = false; linebuf_len = 0; - count ++; } } } @@ -83,7 +192,12 @@ bool AP_RangeFinder_uLanding::get_reading(uint16_t &reading_cm) return false; } - reading_cm = 2.5f * sum / count; + reading_cm = sum / count; + + if (_version == 0) { + reading_cm *= 2.5f; + } + return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h index 3e0208909e..c3571aabf1 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_uLanding.h @@ -18,11 +18,17 @@ public: void update(void); private: + // detect uLanding Firmware Version + bool detect_version(void); + // get a reading bool get_reading(uint16_t &reading_cm); AP_HAL::UARTDriver *uart = nullptr; uint32_t last_reading_ms = 0; - uint8_t linebuf[10]; + uint8_t linebuf[6]; uint8_t linebuf_len = 0; + bool _version_known; + uint8_t _header; + uint8_t _version; };