From a91d4f71c1051c84ec5fe449a36d7b07b0c2fa9d Mon Sep 17 00:00:00 2001 From: Joshua Henderson Date: Tue, 26 Apr 2022 21:09:20 -0400 Subject: [PATCH 1/3] AP_Math: add CRC8_generic method --- libraries/AP_Math/crc.cpp | 10 ++++++++++ libraries/AP_Math/crc.h | 1 + 2 files changed, 11 insertions(+) diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp index 9835f49595ad0..bc059535daefc 100644 --- a/libraries/AP_Math/crc.cpp +++ b/libraries/AP_Math/crc.cpp @@ -92,6 +92,16 @@ uint8_t crc_crc8(const uint8_t *p, uint8_t len) return crc & 0xFF; } +// CRC8 that does not use a lookup table: for generic polynomials +uint8_t crc8_generic(const uint8_t *buf, const uint16_t buf_len, const uint8_t polynomial) +{ + uint8_t crc = 0; + for (uint16_t i = 0; i < buf_len; i++) { + crc = crc8_dvb(buf[i], crc, polynomial); + } + return crc; +} + // crc8 from betaflight uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a) { diff --git a/libraries/AP_Math/crc.h b/libraries/AP_Math/crc.h index 7cd9d380e4ba6..6850445b66bc6 100644 --- a/libraries/AP_Math/crc.h +++ b/libraries/AP_Math/crc.h @@ -21,6 +21,7 @@ uint16_t crc_crc4(uint16_t *data); uint8_t crc_crc8(const uint8_t *p, uint8_t len); +uint8_t crc8_generic(const uint8_t *buf, const uint16_t buf_len, const uint8_t polynomial); // CRC8 that does not use a lookup table for generic polynomials uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a); uint8_t crc8_dvb(uint8_t crc, uint8_t a, uint8_t seed); uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length); From 52f6f6e16091831002a7e7c2911ccb63a1d16377 Mon Sep 17 00:00:00 2001 From: Adithya Patil Date: Thu, 16 Jun 2022 23:28:26 -0400 Subject: [PATCH 2/3] AP_Proximity: add driver for LD06 --- libraries/AP_Proximity/AP_Proximity.cpp | 10 ++ libraries/AP_Proximity/AP_Proximity.h | 3 + libraries/AP_Proximity/AP_Proximity_LD06.cpp | 169 ++++++++++++++++++ libraries/AP_Proximity/AP_Proximity_LD06.h | 104 +++++++++++ .../AP_Proximity/AP_Proximity_Params.cpp | 2 +- libraries/AP_Proximity/AP_Proximity_config.h | 4 + 6 files changed, 291 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_Proximity/AP_Proximity_LD06.cpp create mode 100644 libraries/AP_Proximity/AP_Proximity_LD06.h diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index a74ba8f995834..ac1d6786e6c6b 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -29,6 +29,7 @@ #include "AP_Proximity_Cygbot_D1.h" #include "AP_Proximity_DroneCAN.h" #include "AP_Proximity_Scripting.h" +#include "AP_Proximity_LD06.h" #include @@ -216,6 +217,15 @@ void AP_Proximity::init() state[instance].instance = instance; drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance], params[instance]); break; +#endif +#if AP_PROXIMITY_LD06_ENABLED + case Type::LD06: + if (AP_Proximity_LD06::detect(serial_instance)) { + state[instance].instance = instance; + drivers[instance] = new AP_Proximity_LD06(*this, state[instance], params[instance], serial_instance); + serial_instance++; + } + break; #endif } diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index a07f5f3abe4e1..0363fea8f4da9 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -83,6 +83,9 @@ class AP_Proximity #endif #if AP_PROXIMITY_SCRIPTING_ENABLED Scripting = 15, +#endif +#if AP_PROXIMITY_LD06_ENABLED + LD06 = 16, #endif }; diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.cpp b/libraries/AP_Proximity/AP_Proximity_LD06.cpp new file mode 100644 index 0000000000000..386de5f4a86b1 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_LD06.cpp @@ -0,0 +1,169 @@ +/* + 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 . + */ + +/* + * ArduPilot device driver for Inno-Maker LD06 LiDAR + * + * ALL INFORMATION REGARDING PROTOCOL WAS DERIVED FROM InnoMaker DATASHEET: + * + * http://wiki.inno-maker.com/display/HOMEPAGE/LD06?preview=/6949506/6949511/LDROBOT_LD06_Development%20manual_v1.0_en.pdf + * + * Author: Adithya Patil, Georgia Institute of Technology + * Based on the SLAMTEC RPLiDAR code written by Steven Josefs, IAV GmbH and CYGBOT D1 LiDAR code + * + */ + +#include "AP_Proximity_LD06.h" + +#if HAL_PROXIMITY_ENABLED && AP_PROXIMITY_LD06_ENABLED + +// Update the sensor readings +void AP_Proximity_LD06::update(void) +{ + // Escape if no connection detected/supported while running + if (_uart == nullptr) { + return; + } + + // Begin getting sensor readings + // Calls method that repeatedly reads through UART channel + get_readings(); + + // Check if the data is being received correctly and sets Proximity Status + if (_last_distance_received_ms == 0 || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_LD06_TIMEOUT_MS)) { + set_status(AP_Proximity::Status::NoData); + } else { + set_status(AP_Proximity::Status::Good); + } +} + +// Called repeatedly to get the readings at the current instant +void AP_Proximity_LD06::get_readings() +{ + if (_uart == nullptr) { + return; + } + + // Store the number of bytes available on the UART input + uint32_t nbytes = MIN((uint16_t) 4000, _uart->available()); + + // Loops through all bytes that were received + while (nbytes-- > 0) { + + // Gets and logs the current byte being read + const uint8_t c = _uart->read(); + + // Stores the byte in an array if the byte is a start byte or we have already read a start byte + if (c == LD_START_CHAR || _response_data) { + + // Sets to true if a start byte has been read, default false otherwise + _response_data = true; + + // Stores the next byte in an array + _response[_byte_count] = c; + _byte_count++; + + if (_byte_count == _response[START_DATA_LENGTH] + 3) { + + set_status(AP_Proximity::Status::Good); + + uint32_t current_ms = AP_HAL::millis(); + + // Stores the last distance taken, used to reduce number of readings taken + if (_last_distance_received_ms != current_ms) { + _last_distance_received_ms = current_ms; + } + + // Updates the temporary boundary and passes off the completed data + parse_response_data(); + _temp_boundary.update_3D_boundary(state.instance, frontend.boundary); + _temp_boundary.reset(); + + + // Resets the bytes read and whether or not we are reading data to accept a new payload + _byte_count = 0; + _response_data = false; + } + } + } +} + +// Parses the data packet received from the LiDAR +void AP_Proximity_LD06::parse_response_data() +{ + + // Data interpretation based on: + // http://wiki.inno-maker.com/display/HOMEPAGE/LD06?preview=/6949506/6949511/LDROBOT_LD06_Development%20manual_v1.0_en.pdf + + // Second byte in array stores length of data - not used but stored for debugging + // const uint8_t data_length = _response[START_DATA_LENGTH]; + + // Respective bits store the radar speed, start/end angles + // Use bitwise operations to correctly obtain correct angles + // Divide angles by 100 as per manual + const float start_angle = float(UINT16_VALUE(_response[START_BEGIN_ANGLE + 1], _response[START_BEGIN_ANGLE])) * 0.01; + const float end_angle = float(UINT16_VALUE(_response[START_END_ANGLE + 1], _response[START_END_ANGLE])) * 0.01; + + // Verify the checksum that is stored in the last element of the response array + // Return if checksum is incorrect - i.e. bad data, bad readings, etc. + const uint8_t check_sum = _response[START_CHECK_SUM]; + if (check_sum != crc8_generic(&_response[0], sizeof(_response) / sizeof(_response[0]) - 1, 0x4D)) { + return; + } + + // Calculates the angle that this point was sampled at + float sampled_counts = 0; + const float angle_step = (end_angle - start_angle) / (PAYLOAD_COUNT - 1); + float uncorrected_angle = start_angle + (end_angle - start_angle) * 0.5; + + // Handles the case that the angles read went from 360 to 0 (jumped) + if (angle_step < 0) { + uncorrected_angle = wrap_360(start_angle + (end_angle + 360 - start_angle) * 0.5); + } + + // Takes the angle in the middle of the readings to be pushed to the database + const float push_angle = correct_angle_for_orientation(uncorrected_angle); + + float distance_avg = 0.0; + + // Each recording point is three bytes long, goes through all of that and updates database + for (uint16_t i = START_PAYLOAD; i < START_PAYLOAD + MEASUREMENT_PAYLOAD_LENGTH * PAYLOAD_COUNT; i += MEASUREMENT_PAYLOAD_LENGTH) { + + // Gets the distance recorded and converts to meters + const float distance_meas = UINT16_VALUE(_response[i + 1], _response[i]) * 0.001; + + // Validates data and checks if it should be included + if (distance_meas > distance_min() && distance_meas < distance_max()) { + if (ignore_reading(push_angle, distance_meas)) { + continue; + } + + sampled_counts ++; + distance_avg += distance_meas; + } + } + + // Convert angle to appropriate face and adds to database + // Since angle increments are only about 3 degrees, ignore readings if there were only 1 or 2 measurements + // (likely outliers) recorded in the range + if (sampled_counts > 2) { + // Gets the average distance read + distance_avg /= sampled_counts; + + // Pushes the average distance and angle to the obstacle avoidance database + const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(push_angle); + _temp_boundary.add_distance(face, push_angle, distance_avg); + database_push(push_angle, distance_avg); + } +} +#endif // HAL_PROXIMITY_ENABLED && AP_PROXIMITY_LD06_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.h b/libraries/AP_Proximity/AP_Proximity_LD06.h new file mode 100644 index 0000000000000..1ea6b278850be --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_LD06.h @@ -0,0 +1,104 @@ +/* + 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 . + */ + +/* + * ArduPilot device driver for Inno-Maker LD06 LiDAR + * + * ALL INFORMATION REGARDING PROTOCOL WAS DERIVED FROM LD06 DATASHEET: + * + * http://wiki.inno-maker.com/display/HOMEPAGE/LD06?preview=/6949506/6949511/LDROBOT_LD06_Development%20manual_v1.0_en.pdf + * + * Author: Adithya Patil, Georgia Institute of Technology + * Based on the SLAMTEC RPLiDAR code written by Steven Josefs, IAV GmbH + * + */ + + +#pragma once + +#include "AP_Proximity.h" + +#ifndef AP_PROXIMITY_LD06_ENABLED +#define AP_PROXIMITY_LD06_ENABLED HAL_PROXIMITY_ENABLED +#endif + +#if HAL_PROXIMITY_ENABLED && AP_PROXIMITY_LD06_ENABLED +#include "AP_Proximity_Backend_Serial.h" + +#define MESSAGE_LENGTH 47 +#define LD_START_CHAR 0x54 +#define PROXIMITY_LD06_TIMEOUT_MS 50 + +// Indices in data array where each value starts being recorded +// See comment below about data payload for more info about formatting +#define START_BEGIN_CHARACTER 0 +#define START_DATA_LENGTH 1 +#define START_RADAR_SPEED 2 +#define START_BEGIN_ANGLE 4 +#define START_PAYLOAD 6 +#define START_END_ANGLE 42 +#define START_CHECK_SUM 46 +#define MEASUREMENT_PAYLOAD_LENGTH 3 +#define PAYLOAD_COUNT 12 + +// Minimum and maximum distance that the sensor can read in meters +#define MAX_READ_DISTANCE 12.0f +#define MIN_READ_DISTANCE 0.02f + +class AP_Proximity_LD06 : public AP_Proximity_Backend_Serial +{ +public: + + using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial; + + // Update the state of the sensor + void update(void) override; + + // Get the max and min distances for the sensor being used + float distance_max() const override { return MAX_READ_DISTANCE; } + float distance_min() const override { return MIN_READ_DISTANCE; } + +private: + + // Get and parse the sensor data + void parse_response_data(); + void get_readings(); + + /* ------------------------------------------ + Data Packet Structure: + Start Character : 1 Byte + Data Length : 1 Byte + Radar Speed : 2 Bytes + Start Angle : 2 Bytes + Data Measurements : 36 Bytes + Contains 12 measurements of 3 Bytes each + Each measurement has 2 Bytes for distance to closest object + Each measuremnt has the 3rd Byte as measurement Confidence + End Angle : 2 Bytes + Timestamp : 2 Bytes + Checksum : 1 Byte + ------------------------------------------ */ + // ----> 47 data bytes in total for one packet + + // Store and keep track of the bytes being read from the sensor + uint8_t _response[MESSAGE_LENGTH]; + bool _response_data; + uint16_t _byte_count; + + // Store for error-tracking purposes + uint32_t _last_distance_received_ms; + + // Boundary to store the measurements + AP_Proximity_Temp_Boundary _temp_boundary; +}; +#endif // HAL_PROXIMITY_ENABLED && AP_PROXIMITY_LD06_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_Params.cpp b/libraries/AP_Proximity/AP_Proximity_Params.cpp index d9f9f78211fb7..046a29c3b7181 100644 --- a/libraries/AP_Proximity/AP_Proximity_Params.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Proximity_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Proximity type // @Description: What type of proximity sensor is connected - // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1, 14:DroneCAN, 15:Scripting + // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1, 14:DroneCAN, 15:Scripting, 16:LD06 // @RebootRequired: True // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Proximity_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Proximity/AP_Proximity_config.h b/libraries/AP_Proximity/AP_Proximity_config.h index 403634f773bbc..adf0dd8de5cbf 100644 --- a/libraries/AP_Proximity/AP_Proximity_config.h +++ b/libraries/AP_Proximity/AP_Proximity_config.h @@ -59,3 +59,7 @@ #ifndef AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED #define AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED #endif + +#ifndef AP_PROXIMITY_LD06_ENABLED +#define AP_PROXIMITY_LD06_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED +#endif From 096276be51941d9e5c994f3d5c2d65f33593ab1a Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Sun, 13 Aug 2023 22:57:48 -0400 Subject: [PATCH 3/3] AP_Proximity: Minor fixes to LD06 driver --- libraries/AP_Proximity/AP_Proximity_LD06.cpp | 49 ++++++++++++++---- libraries/AP_Proximity/AP_Proximity_LD06.h | 53 ++++---------------- libraries/AP_Proximity/AP_Proximity_config.h | 5 ++ 3 files changed, 53 insertions(+), 54 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.cpp b/libraries/AP_Proximity/AP_Proximity_LD06.cpp index 386de5f4a86b1..409a3352e3a2c 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LD06.cpp @@ -23,9 +23,41 @@ * */ +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_LD06_ENABLED #include "AP_Proximity_LD06.h" -#if HAL_PROXIMITY_ENABLED && AP_PROXIMITY_LD06_ENABLED +#define LD_START_CHAR 0x54 +#define PROXIMITY_LD06_TIMEOUT_MS 50 + +// Indices in data array where each value starts being recorded +// See comment below about data payload for more info about formatting +#define START_BEGIN_CHARACTER 0 +#define START_DATA_LENGTH 1 +#define START_RADAR_SPEED 2 +#define START_BEGIN_ANGLE 4 +#define START_PAYLOAD 6 +#define START_END_ANGLE 42 +#define START_CHECK_SUM 46 +#define MEASUREMENT_PAYLOAD_LENGTH 3 +#define PAYLOAD_COUNT 12 + + /* ------------------------------------------ + Data Packet Structure: + Start Character : 1 Byte + Data Length : 1 Byte + Radar Speed : 2 Bytes + Start Angle : 2 Bytes + Data Measurements : 36 Bytes + Contains 12 measurements of 3 Bytes each + Each measurement has 2 Bytes for distance to closest object + Each measurement has the 3rd Byte as measurement Confidence + End Angle : 2 Bytes + Timestamp : 2 Bytes + Checksum : 1 Byte + ------------------------------------------ */ +// ----> 47 data bytes in total for one packet // Update the sensor readings void AP_Proximity_LD06::update(void) @@ -74,21 +106,18 @@ void AP_Proximity_LD06::get_readings() _byte_count++; if (_byte_count == _response[START_DATA_LENGTH] + 3) { - - set_status(AP_Proximity::Status::Good); - - uint32_t current_ms = AP_HAL::millis(); + + const uint32_t current_ms = AP_HAL::millis(); // Stores the last distance taken, used to reduce number of readings taken if (_last_distance_received_ms != current_ms) { _last_distance_received_ms = current_ms; } - + // Updates the temporary boundary and passes off the completed data parse_response_data(); _temp_boundary.update_3D_boundary(state.instance, frontend.boundary); _temp_boundary.reset(); - // Resets the bytes read and whether or not we are reading data to accept a new payload _byte_count = 0; @@ -100,7 +129,7 @@ void AP_Proximity_LD06::get_readings() // Parses the data packet received from the LiDAR void AP_Proximity_LD06::parse_response_data() -{ +{ // Data interpretation based on: // http://wiki.inno-maker.com/display/HOMEPAGE/LD06?preview=/6949506/6949511/LDROBOT_LD06_Development%20manual_v1.0_en.pdf @@ -154,7 +183,7 @@ void AP_Proximity_LD06::parse_response_data() } // Convert angle to appropriate face and adds to database - // Since angle increments are only about 3 degrees, ignore readings if there were only 1 or 2 measurements + // Since angle increments are only about 3 degrees, ignore readings if there were only 1 or 2 measurements // (likely outliers) recorded in the range if (sampled_counts > 2) { // Gets the average distance read @@ -166,4 +195,4 @@ void AP_Proximity_LD06::parse_response_data() database_push(push_angle, distance_avg); } } -#endif // HAL_PROXIMITY_ENABLED && AP_PROXIMITY_LD06_ENABLED +#endif // AP_PROXIMITY_LD06_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.h b/libraries/AP_Proximity/AP_Proximity_LD06.h index 1ea6b278850be..c51adbb102e82 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.h +++ b/libraries/AP_Proximity/AP_Proximity_LD06.h @@ -23,37 +23,18 @@ * */ - #pragma once +#include "AP_Proximity_config.h" -#include "AP_Proximity.h" - -#ifndef AP_PROXIMITY_LD06_ENABLED -#define AP_PROXIMITY_LD06_ENABLED HAL_PROXIMITY_ENABLED -#endif +#if AP_PROXIMITY_LD06_ENABLED -#if HAL_PROXIMITY_ENABLED && AP_PROXIMITY_LD06_ENABLED #include "AP_Proximity_Backend_Serial.h" -#define MESSAGE_LENGTH 47 -#define LD_START_CHAR 0x54 -#define PROXIMITY_LD06_TIMEOUT_MS 50 - -// Indices in data array where each value starts being recorded -// See comment below about data payload for more info about formatting -#define START_BEGIN_CHARACTER 0 -#define START_DATA_LENGTH 1 -#define START_RADAR_SPEED 2 -#define START_BEGIN_ANGLE 4 -#define START_PAYLOAD 6 -#define START_END_ANGLE 42 -#define START_CHECK_SUM 46 -#define MEASUREMENT_PAYLOAD_LENGTH 3 -#define PAYLOAD_COUNT 12 +#define MESSAGE_LENGTH_LD06 47 // Minimum and maximum distance that the sensor can read in meters -#define MAX_READ_DISTANCE 12.0f -#define MIN_READ_DISTANCE 0.02f +#define MAX_READ_DISTANCE_LD06 12.0f +#define MIN_READ_DISTANCE_LD06 0.02f class AP_Proximity_LD06 : public AP_Proximity_Backend_Serial { @@ -65,8 +46,8 @@ class AP_Proximity_LD06 : public AP_Proximity_Backend_Serial void update(void) override; // Get the max and min distances for the sensor being used - float distance_max() const override { return MAX_READ_DISTANCE; } - float distance_min() const override { return MIN_READ_DISTANCE; } + float distance_max() const override { return MAX_READ_DISTANCE_LD06; } + float distance_min() const override { return MIN_READ_DISTANCE_LD06; } private: @@ -74,24 +55,8 @@ class AP_Proximity_LD06 : public AP_Proximity_Backend_Serial void parse_response_data(); void get_readings(); - /* ------------------------------------------ - Data Packet Structure: - Start Character : 1 Byte - Data Length : 1 Byte - Radar Speed : 2 Bytes - Start Angle : 2 Bytes - Data Measurements : 36 Bytes - Contains 12 measurements of 3 Bytes each - Each measurement has 2 Bytes for distance to closest object - Each measuremnt has the 3rd Byte as measurement Confidence - End Angle : 2 Bytes - Timestamp : 2 Bytes - Checksum : 1 Byte - ------------------------------------------ */ - // ----> 47 data bytes in total for one packet - // Store and keep track of the bytes being read from the sensor - uint8_t _response[MESSAGE_LENGTH]; + uint8_t _response[MESSAGE_LENGTH_LD06]; bool _response_data; uint16_t _byte_count; @@ -101,4 +66,4 @@ class AP_Proximity_LD06 : public AP_Proximity_Backend_Serial // Boundary to store the measurements AP_Proximity_Temp_Boundary _temp_boundary; }; -#endif // HAL_PROXIMITY_ENABLED && AP_PROXIMITY_LD06_ENABLED +#endif // AP_PROXIMITY_LD06_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_config.h b/libraries/AP_Proximity/AP_Proximity_config.h index adf0dd8de5cbf..a2130b0b5c4f2 100644 --- a/libraries/AP_Proximity/AP_Proximity_config.h +++ b/libraries/AP_Proximity/AP_Proximity_config.h @@ -24,6 +24,11 @@ #define AP_PROXIMITY_DRONECAN_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS #endif + +#ifndef AP_PROXIMITY_LD06_ENABLED +#define AP_PROXIMITY_LD06_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED +#endif + #ifndef AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED #define AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED #endif