diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc index b88283517b672b..892274b97625f2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc +++ b/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 \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_fpv_osd.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_fpv_osd.inc index 8fef4956a20305..028b77ed11bb8e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_fpv_osd.inc +++ b/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 \ No newline at end of file diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 74b621b5d5afe3..b487269936a308 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/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()) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index fbbdb1683a64c9..f9556ad20a900b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFA1500.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFA1500.cpp new file mode 100644 index 00000000000000..844974d905bb31 --- /dev/null +++ b/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 +#include + +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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFA1500.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFA1500.h new file mode 100644 index 00000000000000..4271d0029603ed --- /dev/null +++ b/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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index abeac3ccbbf89b..eb661e73d15ee3 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/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), diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_config.h b/libraries/AP_RangeFinder/AP_RangeFinder_config.h index 1011f0c1d304b2..6ef1e59e4dbb62 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_config.h +++ b/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