feat(rangefinder): 添加对 Benewake TFA1500 激光测距仪的支持

libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc

@@ -144,3 +144,6 @@ define AP_SERVO_TELEM_ENABLED 0
 
 # don't send extra flight information on small boards
 define AP_MAVLINK_MSG_FLIGHT_INFORMATION_ENABLED 0
+
+# Enable Benewake TFA1500
+define AP_RANGEFINDER_BENEWAKE_TFA1500_ENABLED 1

libraries/AP_HAL_ChibiOS/hwdef/include/minimize_fpv_osd.inc

@@ -38,4 +38,4 @@ define AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED 0
 define AP_RANGEFINDER_BENEWAKE_TF02_ENABLED 1
 define AP_RANGEFINDER_BENEWAKE_TF03_ENABLED 1
 define AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED 1
-define AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED 1
+define AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED 1

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -40,6 +40,7 @@
 #include "AP_RangeFinder_Wasp.h"
 #include "AP_RangeFinder_Benewake_TF02.h"
 #include "AP_RangeFinder_Benewake_TF03.h"
+#include "AP_RangeFinder_Benewake_TFA1500.h"
 #include "AP_RangeFinder_Benewake_TFMini.h"
 #include "AP_RangeFinder_Benewake_TFMiniPlus.h"
 #include "AP_RangeFinder_Benewake_TFS20L.h"
@@ -532,6 +533,11 @@ __INITFUNC__ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial
         serial_create_fn = AP_RangeFinder_TeraRanger_Serial::create;
         break;
 #endif
+#if AP_RANGEFINDER_BENEWAKE_TFA1500_ENABLED
+    case Type::BenewakeTFA1500:
+        serial_create_fn = AP_RangeFinder_Benewake_TFA1500::create;
+        break;
+#endif
 #if AP_RANGEFINDER_PWM_ENABLED
     case Type::PWM:
         if (AP_RangeFinder_PWM::detect()) {

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -191,6 +191,9 @@ class RangeFinder
 #if AP_RANGEFINDER_BENEWAKE_TFS20L_ENABLED
         BenewakeTFS20L = 46,
 #endif // AP_RANGEFINDER_BENEWAKE_TFS20L_ENABLED
+#if AP_RANGEFINDER_BENEWAKE_TFA1500_ENABLED
+        BenewakeTFA1500 = 47,
+#endif // AP_RANGEFINDER_BENEWAKE_TFA1500_ENABLED
 #if AP_RANGEFINDER_SIM_ENABLED
         SIM = 100,
 #endif

libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFA1500.cpp

@@ -0,0 +1,131 @@
+#include "AP_RangeFinder_config.h"
+
+#if AP_RANGEFINDER_BENEWAKE_TFA1500_ENABLED
+
+#include "AP_RangeFinder_Benewake_TFA1500.h"
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/utility/sparse-endian.h>
+
+extern const AP_HAL::HAL &hal;
+
+static constexpr uint8_t  TFA1500_FRAME_HEADER     = 0x5CU;
+static constexpr uint8_t  TFA1500_FRAME_LENGTH     = 5U;
+static constexpr uint32_t TFA1500_DIST_MAX_CM      = 130000U;
+
+static const uint8_t TFA1500_CMD_START[] = {
+    0x55U, 0xAAU, 0xCBU, 0xCCU, 0xCCU, 0xCCU, 0xCCU, 0xFBU
+};
+
+static bool parse_frame(const uint8_t *buffer, uint32_t &dist_cm)
+{
+    union {
+        uint8_t bytes[TFA1500_FRAME_LENGTH];
+        struct PACKED {
+            uint8_t header;
+            uint8_t dist_low;
+            uint8_t dist_mid;
+            uint8_t dist_high;
+            uint8_t crc_sum_of_bytes;
+        } packet;
+    } frame;
+
+    memcpy(frame.bytes, buffer, sizeof(frame.bytes));
+
+    if (frame.packet.header != TFA1500_FRAME_HEADER) {
+        return false;
+    }
+
+    const uint8_t expected_crc_sum_of_bytes = ~(frame.packet.dist_low +
+                                        frame.packet.dist_mid +
+                                        frame.packet.dist_high);
+
+    if (expected_crc_sum_of_bytes != frame.packet.crc_sum_of_bytes) {
+        return false;
+    }
+
+    dist_cm = (frame.packet.dist_high << 16) |
+              (frame.packet.dist_mid << 8) |
+              frame.packet.dist_low;
+    return true;
+}
+
+void AP_RangeFinder_Benewake_TFA1500::init_serial(uint8_t serial_instance)
+{
+    AP_RangeFinder_Backend_Serial::init_serial(serial_instance);
+}
+
+bool AP_RangeFinder_Benewake_TFA1500::get_reading(float &reading_m)
+{
+    if (uart == nullptr) {
+        return false;
+    }
+
+    const uint32_t now = AP_HAL::millis();
+    if (last_init_ms == 0 ||
+        ((now - last_init_ms > 1000) && (now - state.last_reading_ms > 1000))) {
+        last_init_ms = now;
+        uart->write(TFA1500_CMD_START, sizeof(TFA1500_CMD_START));
+    }
+
+    float sum_cm = 0.0f;
+    uint16_t count = 0;
+    uint16_t count_out_of_range = 0;
+
+    // read limit to prevent consuming too much CPU
+    for (auto j = 0U; j < 8192; j++) {
+        uint8_t c;
+        if (!uart->read(c)) {
+            break;
+        }
+
+        tf_linebuf[tf_linebuf_len++] = c;
+
+        if (tf_linebuf_len < TFA1500_FRAME_LENGTH) {
+            continue;
+        }
+
+        uint32_t dist_cm = 0U;
+        if (parse_frame(tf_linebuf, dist_cm)) {
+
+            if ((dist_cm >= TFA1500_DIST_MAX_CM) || (dist_cm == (uint32_t)model_dist_max_cm())) {
+                count_out_of_range++;
+            } else {
+                sum_cm += dist_cm;
+                count++;
+            }
+
+            tf_linebuf_len = 0;
+
+        } else {
+
+            // shift buffer until a header byte is found or buffer is empty
+            uint8_t i;
+            for (i = 1; i < tf_linebuf_len; i++) {
+                if (tf_linebuf[i] == TFA1500_FRAME_HEADER) {
+                    break;
+                }
+            }
+            if (i < tf_linebuf_len) {
+                memmove(&tf_linebuf[0], &tf_linebuf[i], tf_linebuf_len - i);
+                tf_linebuf_len -= i;
+            } else {
+                tf_linebuf_len = 0;
+            }
+        }
+    }
+
+    if (count > 0) {
+        reading_m = (sum_cm * 0.01f) / count;
+        return true;
+    }
+
+    if (count_out_of_range > 0) {
+        reading_m = MAX(model_dist_max_cm() * 0.01f, max_distance());
+        return true;
+    }
+
+    return false;
+}
+
+#endif

libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFA1500.h

@@ -0,0 +1,31 @@
+// Datasheet: https://en.benewake.com/DataDownload/index_pid_20_lcid_104.html
+
+#pragma once
+
+#if AP_RANGEFINDER_BENEWAKE_TFA1500_ENABLED
+
+#include "AP_RangeFinder_Benewake.h"
+
+class AP_RangeFinder_Benewake_TFA1500 : public AP_RangeFinder_Benewake
+{
+public:
+    static AP_RangeFinder_Backend_Serial *create(
+        RangeFinder::RangeFinder_State &_state,
+        AP_RangeFinder_Params &_params)
+    {
+        return NEW_NOTHROW AP_RangeFinder_Benewake_TFA1500(_state, _params);
+    }
+    void init_serial(uint8_t serial_instance) override;
+
+protected:
+    float model_dist_max_cm() const override { return 0x3FFFFF; }
+
+private:
+    using AP_RangeFinder_Benewake::AP_RangeFinder_Benewake;
+    bool get_reading(float &reading_m) override;
+    uint8_t tf_linebuf[5];
+    uint8_t tf_linebuf_len;
+    uint32_t last_init_ms = 0;
+};
+
+#endif // AP_RANGEFINDER_BENEWAKE_TFA1500

libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp

@@ -61,6 +61,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
     // @Values: 44:HexsoonRadar
     // @Values: 45:LightWare-GRF
     // @Values: 46:BenewakeTFS20L
+    // @Values: 47:BenewakeTFA1500
     // @Values: 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

@@ -47,6 +47,10 @@
 #define AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED (AP_RANGEFINDER_BENEWAKE_ENABLED && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
 #endif
 
+#ifndef AP_RANGEFINDER_BENEWAKE_TFA1500_ENABLED
+#define AP_RANGEFINDER_BENEWAKE_TFA1500_ENABLED (AP_RANGEFINDER_BENEWAKE_ENABLED && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
+#endif
+
 #ifndef AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED
 #define AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED
 #endif