diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp
index 7169c40231..2d731ff335 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp
+++ b/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:
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h
index 9a85327fe6..dfb01b5674 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder.h
@@ -102,6 +102,7 @@ public:
MSP = 32,
USD1_CAN = 33,
Benewake_CAN = 34,
+ TeraRanger_Serial = 35,
SIM = 100,
};
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp
index cf55d446d6..197305ebac 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp
+++ b/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),
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp
new file mode 100644
index 0000000000..e9a502de79
--- /dev/null
+++ b/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 .
+ */
+
+#include "AP_RangeFinder_TeraRanger_Serial.h"
+
+#if AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED
+
+
+#include
+#include
+#include
+#include
+
+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
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.h
new file mode 100644
index 0000000000..6840849e07
--- /dev/null
+++ b/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