AP_RangeFinder: add TeraRange Neo Rangefinder

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -30,6 +30,7 @@
 #include "AP_RangeFinder_LeddarOne.h"
 #include "AP_RangeFinder_USD1_Serial.h"
 #include "AP_RangeFinder_TeraRangerI2C.h"
+#include "AP_RangeFinder_TeraRanger_Serial.h"
 #include "AP_RangeFinder_VL53L0X.h"
 #include "AP_RangeFinder_VL53L1X.h"
 #include "AP_RangeFinder_NMEA.h"
@@ -537,6 +538,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
     case Type::BenewakeTF03:
 #if AP_RANGEFINDER_BENEWAKE_TF03_ENABLED
         serial_create_fn = AP_RangeFinder_Benewake_TF03::create;
+#endif
+        break;
+    case Type::TeraRanger_Serial:
+#if AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED
+        serial_create_fn = AP_RangeFinder_TeraRanger_Serial::create;
 #endif
         break;
     case Type::PWM:

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -102,6 +102,7 @@ public:
         MSP = 32,
         USD1_CAN = 33,
         Benewake_CAN = 34,
+        TeraRanger_Serial = 35,
         SIM = 100,
     };
 

libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp

@@ -15,7 +15,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Rangefinder type
     // @Description: Type of connected rangefinder
-    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,100:SITL
+    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),
 

libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp

@@ -0,0 +1,120 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_RangeFinder_TeraRanger_Serial.h"
+
+#if AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED
+
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/utility/sparse-endian.h>
+#include <AP_Math/AP_Math.h>
+#include <ctype.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define FRAME_HEADER 0x54
+#define FRAME_LENGTH 5
+#define DIST_MAX_CM 3000
+#define OUT_OF_RANGE_ADD_CM 1000
+#define STATUS_MASK 0x1F
+#define DISTANCE_ERROR 0x0001
+
+// format of serial packets received from rangefinder
+//
+// Data Bit             Definition      Description
+// ------------------------------------------------
+// byte 0               Frame header    0x54
+// byte 1               DIST_H          Distance (in mm) high 8 bits
+// byte 2               DIST_L          Distance (in mm) low 8 bits
+// byte 3               STATUS          Status,Strengh,OverTemp
+// byte 4               CRC8            packet CRC
+
+// distance returned in reading_m, set to true if sensor reports a good reading
+bool AP_RangeFinder_TeraRanger_Serial::get_reading(float &reading_m)
+{
+    if (uart == nullptr) {
+        return false;
+    }
+
+    float sum_mm = 0;
+    uint16_t count = 0;
+    uint16_t bad_read = 0;
+
+    // read any available lines from the lidar
+    int16_t nbytes = uart->available();
+    while (nbytes-- > 0) {
+        int16_t r = uart->read();
+        if (r < 0) {
+            continue;
+        }
+        uint8_t c = (uint8_t)r;
+        // if buffer is empty and this byte is 0x57, add to buffer
+        if (linebuf_len == 0) {
+            if (c == FRAME_HEADER) {
+                linebuf[linebuf_len++] = c;
+            }
+        // buffer is not empty, add byte to buffer
+        } else {
+            // add character to buffer
+            linebuf[linebuf_len++] = c;
+            // if buffer now has 5 items try to decode it
+            if (linebuf_len == FRAME_LENGTH) {
+                // calculate CRC8 (tbd)
+                uint8_t crc = 0;
+                crc =crc_crc8(linebuf,FRAME_LENGTH-1);
+                // if crc matches, extract contents
+                if (crc == linebuf[FRAME_LENGTH-1]) {
+                    // calculate distance
+                    uint16_t dist = ((uint16_t)linebuf[1] << 8) | linebuf[2];
+                    if (dist >= DIST_MAX_CM *10) {
+                        // this reading is out of range and a bad read
+                        bad_read++;
+                    } else {
+                        // check if reading is good, no errors, no overtemp, reading is not the special case of 1mm
+                        if ((STATUS_MASK & linebuf[3]) == 0 && (dist != DISTANCE_ERROR)) {
+                            // add distance to sum
+                            sum_mm += dist;
+                            count++;
+                        } else {
+                            // this reading is bad
+                            bad_read++;
+                        }
+                    }
+                }
+                // clear buffer
+                linebuf_len = 0;
+            }
+        }
+    }
+
+    if (count > 0) {
+        // return average distance of readings since last update
+        reading_m = (sum_mm * 0.001f) / count;
+        return true;
+    }
+
+    if (bad_read > 0) {
+        // if a bad read has occurred this update overwrite return with larger of
+        // driver defined maximum range for the model and user defined max range + 1m
+        reading_m = MAX(DIST_MAX_CM, max_distance_cm() + OUT_OF_RANGE_ADD_CM) * 0.01f;
+        return true;
+    }
+
+    // no readings so return false
+    return false;
+}
+
+#endif // AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.h

@@ -0,0 +1,40 @@
+#pragma once
+
+#include "AP_RangeFinder.h"
+#include "AP_RangeFinder_Backend_Serial.h"
+
+#ifndef AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED
+#define AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED
+#endif
+
+#if AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED
+
+class AP_RangeFinder_TeraRanger_Serial : public AP_RangeFinder_Backend_Serial
+{
+
+public:
+
+    static AP_RangeFinder_Backend_Serial *create(
+        RangeFinder::RangeFinder_State &_state,
+        AP_RangeFinder_Params &_params) {
+        return new AP_RangeFinder_TeraRanger_Serial(_state, _params);
+    }
+
+protected:
+
+    using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
+
+    MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
+        return MAV_DISTANCE_SENSOR_LASER;
+    }
+
+private:
+
+    // get a reading
+    // distance returned in reading_m
+    bool get_reading(float &reading_m) override;
+
+    uint8_t linebuf[10];
+    uint8_t linebuf_len;
+};
+#endif  // AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED