AP_RangeFinder: Add UART driver for NoopLoop rangefinder

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -52,6 +52,7 @@
 #include "AP_RangeFinder_USD1_CAN.h"
 #include "AP_RangeFinder_Benewake_CAN.h"
 #include "AP_RangeFinder_Lua.h"
+#include "AP_RangeFinder_NoopLoop.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -532,6 +533,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
     case Type::Lua_Scripting:
 #if AP_SCRIPTING_ENABLED
         _add_backend(new AP_RangeFinder_Lua(state[instance], params[instance]), instance);
+#endif
+        break;
+    case Type::NoopLoop_P:
+#if AP_RANGEFINDER_NOOPLOOP_ENABLED
+        serial_create_fn = AP_RangeFinder_NoopLoop::create;
 #endif
         break;
 

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -97,6 +97,7 @@ public:
         Benewake_CAN = 34,
         TeraRanger_Serial = 35,
         Lua_Scripting = 36,
+        NoopLoop_P = 37,
         SIM = 100,
     };
 

libraries/AP_RangeFinder/AP_RangeFinder_NoopLoop.cpp

@@ -0,0 +1,107 @@
+/*
+   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_NoopLoop.h"
+
+#if AP_RANGEFINDER_NOOPLOOP_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/utility/sparse-endian.h>
+
+#include <ctype.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define NOOPLOOP_FRAME_HEADER 0x57
+#define NOOPLOOP_FRAME_HEADER_1 0x00
+#define NOOPLOOP_FRAME_LENGTH 16
+#define NOOPLOOP_DIST_MAX_MM 8000000
+
+// format of serial packets received from NoopLoop TOF Sense P and F lidar
+//
+// Data Bit             Definition             Description
+// -----------------------------------------------------------------------
+// byte 0               Frame header           0x57
+// byte 1               Function Mark          0x00
+// byte 2               (Reserved)
+// byte 3               Id                     id
+// byte 4-7             System time            System time in ms
+// byte 8-10            Distance*1000
+// byte 11              Status                 Strength high 8 bits
+// byte 12-13           Signal Strength
+// byte 14              (Reserved)
+// byte 15              Checksum
+
+// distance returned in reading_m
+bool AP_RangeFinder_NoopLoop::get_reading(float &reading_m)
+{
+    if (uart == nullptr) {
+        return false;
+    }
+
+    // read any available lines from the lidar
+    uint32_t nbytes = uart->available();
+    while (nbytes-- > 0) {
+        uint8_t c;
+        if (!uart->read(c)) {
+            break;
+        }
+        // if buffer is empty and this byte is 0x57, add to buffer
+        if (linebuf_len == 0) {
+            if (c == NOOPLOOP_FRAME_HEADER) {
+                linebuf[linebuf_len++] = c;
+            }
+        } else if (linebuf_len == 1) {
+            // if buffer has 1 element and this byte is 0x00, add it to buffer
+            // if not clear the buffer
+            if (c == NOOPLOOP_FRAME_HEADER_1) {
+                linebuf[linebuf_len++] = c;
+            } else {
+                linebuf_len = 0;
+            }
+        } else {
+            // add character to buffer
+            linebuf[linebuf_len++] = c;
+            // if buffer now has 16 items try to decode it
+            if (linebuf_len == NOOPLOOP_FRAME_LENGTH) {
+                // calculate checksum
+                uint8_t checksum = 0;
+                for (uint8_t i=0; i<NOOPLOOP_FRAME_LENGTH-1; i++) {
+                    checksum += linebuf[i];
+                }
+                // if checksum matches extract contents
+                if (checksum == linebuf[NOOPLOOP_FRAME_LENGTH-1]) {
+                    // calculate distance
+                    const int32_t dist = (int32_t)(linebuf[8] << 8 | linebuf[9] << 16 | linebuf[10] << 24) / 256;
+                    const uint8_t valid = (linebuf[11]);
+                    if (dist > 0 && reading_m < NOOPLOOP_DIST_MAX_MM && valid < 255) {
+                        reading_m = dist * 0.001f;
+                        linebuf_len = 0;
+                        uart->discard_input();
+                        return true;
+                    }
+                }
+                // clear buffer
+                linebuf_len = 0;
+                uart->discard_input();
+            }
+        }
+    }
+
+    // no readings so return false
+    return false;
+}
+
+#endif  // AP_RANGEFINDER_NOOPLOOP_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_NoopLoop.h

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

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,35:TeraRangerSerial,36:Lua_Scripting,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,36:Lua_Scripting,37:NoopLoop_TOFSense, 100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),