AP_RangeFinder: add Ainstein radar driver

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -59,6 +59,7 @@
 #include "AP_RangeFinder_NRA24_CAN.h"
 #include "AP_RangeFinder_TOFSenseF_I2C.h"
 #include "AP_RangeFinder_JRE_Serial.h"
+#include "AP_RangeFinder_Ainstein_LR_D1.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -552,6 +553,12 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
         break;
 #endif
 
+#if AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
+    case Type::Ainstein_LR_D1:
+        serial_create_fn = AP_RangeFinder_Ainstein_LR_D1::create;
+        break;
+#endif
+
 #if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED
     case Type::TOFSenseP_CAN:
         _add_backend(new AP_RangeFinder_TOFSenseP_CAN(state[instance], params[instance]), instance);

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -174,6 +174,9 @@ public:
 #if AP_RANGEFINDER_JRE_SERIAL_ENABLED
         JRE_Serial = 41,
 #endif
+#if AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
+        Ainstein_LR_D1 = 42,
+#endif
 #if AP_RANGEFINDER_SIM_ENABLED
         SIM = 100,
 #endif

libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.cpp

@@ -0,0 +1,119 @@
+/*
+   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_config.h"
+
+#if AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
+
+#include "AP_RangeFinder_Ainstein_LR_D1.h"
+#include <GCS_MAVLink/GCS.h>
+
+// get_reading - read a value from the sensor
+bool AP_RangeFinder_Ainstein_LR_D1::get_reading(float &reading_m)
+{
+    if (uart == nullptr || uart->available() == 0) {
+        return false;
+    }
+
+    bool has_data = false;
+
+    uint32_t available = MAX(uart->available(), static_cast<unsigned int>(PACKET_SIZE*4));
+    while (available >= PACKET_SIZE) {
+        // ---------------
+        // Sync up with the header
+        const uint8_t header[] = {
+            0xEB,   // Header MSB
+            0x90,   // Header LSB
+            0x00   // Device ID
+        };
+        for (uint8_t i = 0; i<ARRAY_SIZE(header); i++) {
+            available--;
+            if (uart->read() != header[i]) {
+                continue;
+            }
+        }
+
+        const uint8_t rest_of_packet_size = (PACKET_SIZE - ARRAY_SIZE(header));
+        if (available < rest_of_packet_size) {  
+            return false;
+        }
+
+        // ---------------
+        // header is aligned!
+        // ---------------
+
+        uint8_t buffer[rest_of_packet_size];
+        available -= uart->read(buffer, ARRAY_SIZE(buffer));
+
+        const uint8_t checksum = buffer[ARRAY_SIZE(buffer)-1]; // last byte is a checksum
+        if (crc_sum_of_bytes(buffer, ARRAY_SIZE(buffer)-1) != checksum) {
+            // bad Checksum
+            continue;
+        }
+
+        const uint8_t malfunction_alert = buffer[1];
+        reading_m = UINT16_VALUE(buffer[3], buffer[4]) * 0.01;
+        const uint8_t snr = buffer[5];
+
+        has_data = true;
+
+        if (malfunction_alert_prev != malfunction_alert) {
+            malfunction_alert_prev = malfunction_alert;
+            report_malfunction(malfunction_alert);
+        }
+
+        /* From datasheet:
+            Altitude measurements associated with a SNR value 
+            of 13dB or lower are considered erroneous. 
+
+            SNR values of 0 are considered out of maximum range (655 metres)
+
+            The altitude measurements should not in any circumstances be used as true
+            measurements independently of the corresponding SNR values. 
+        */
+        signal_quality_pct = (snr <= 13 || malfunction_alert != 0) ? RangeFinder::SIGNAL_QUALITY_MIN : RangeFinder::SIGNAL_QUALITY_MAX;
+
+        if (snr <= 13) {            
+            has_data = false;           
+            if (snr == 0) {
+                state.status = RangeFinder::Status::OutOfRangeHigh;
+                reading_m = MAX(656, max_distance_cm() * 0.01 + 1);
+            } else {
+                state.status = RangeFinder::Status::NoData;
+            }
+        } else {
+            state.status = RangeFinder::Status::Good;
+        }
+    }
+
+    return has_data;
+}
+
+void AP_RangeFinder_Ainstein_LR_D1::report_malfunction(const uint8_t _malfunction_alert_) {
+    if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::Temperature)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Temperature alert");
+    }
+    if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::Voltage)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Voltage alert");
+    }    
+    if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::IFSignalSaturation)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: IF signal saturation alert");
+    }
+    if (_malfunction_alert_ & static_cast<uint8_t>(MalfunctionAlert::AltitudeReading)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RangeFinder: Altitude reading overflow alert");
+    }
+}
+
+#endif

libraries/AP_RangeFinder/AP_RangeFinder_Ainstein_LR_D1.h

@@ -0,0 +1,62 @@
+#pragma once
+
+#include "AP_RangeFinder_config.h"
+
+/*
+ questions:
+ - bug on page 22 on malfunction codes
+ - fixed length seems strange at 28 bytes
+ - definition of snr field is missing in documentation
+ - roll/pitch limits are in conflict, 3.2 vs 
+*/
+#if AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
+
+#include "AP_RangeFinder.h"
+#include "AP_RangeFinder_Backend_Serial.h"
+
+class AP_RangeFinder_Ainstein_LR_D1 : public AP_RangeFinder_Backend_Serial
+{
+
+public:
+
+    static AP_RangeFinder_Backend_Serial *create(
+        RangeFinder::RangeFinder_State &_state,
+        AP_RangeFinder_Params &_params) {
+        return new AP_RangeFinder_Ainstein_LR_D1(_state, _params);
+    }
+
+protected:
+
+    MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
+        return MAV_DISTANCE_SENSOR_RADAR;
+    }
+
+    uint32_t initial_baudrate(uint8_t serial_instance) const override {
+        return 115200;
+    }
+
+private:
+
+    using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
+
+    // get a reading
+    bool get_reading(float &reading_m) override;
+
+    // 0 is no return value, 100 is perfect.  false means signal
+    // quality is not available
+    int8_t get_signal_quality_pct() const override { return signal_quality_pct; };
+
+    static void report_malfunction(const uint8_t _malfunction_alert_);
+
+    enum class MalfunctionAlert : uint8_t {
+        Temperature       = (1U << 0),  // 0x01
+        Voltage           = (1U << 1),  // 0x02
+        IFSignalSaturation= (1U << 6),  // 0x40
+        AltitudeReading   = (1U << 7),  // 0x80
+    };
+
+    static constexpr uint8_t PACKET_SIZE = 32;
+    uint8_t malfunction_alert_prev;
+    int8_t signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN;    
+};
+#endif

libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp

@@ -10,7 +10,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,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,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,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,42:Ainstein_LR_D1,100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),
 

libraries/AP_RangeFinder/AP_RangeFinder_config.h

@@ -176,3 +176,7 @@
 #ifndef AP_RANGEFINDER_JRE_SERIAL_ENABLED
 #define AP_RANGEFINDER_JRE_SERIAL_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024
 #endif
+
+#ifndef AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
+#define AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024
+#endif