From 4957fc35a40234454d1371a63f6cab5555cee667 Mon Sep 17 00:00:00 2001 From: R-Nikhil726 <73047134+R-Nikhil726@users.noreply.github.com> Date: Fri, 20 Oct 2023 10:47:35 +0530 Subject: [PATCH 1/2] Create cmake-single-platform.yml --- .github/workflows/cmake-single-platform.yml | 39 +++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 .github/workflows/cmake-single-platform.yml diff --git a/.github/workflows/cmake-single-platform.yml b/.github/workflows/cmake-single-platform.yml new file mode 100644 index 0000000000000..f522b05df5580 --- /dev/null +++ b/.github/workflows/cmake-single-platform.yml @@ -0,0 +1,39 @@ +# This starter workflow is for a CMake project running on a single platform. There is a different starter workflow if you need cross-platform coverage. +# See: https://github.com/actions/starter-workflows/blob/main/ci/cmake-multi-platform.yml +name: CMake on a single platform + +on: + push: + branches: [ "master" ] + pull_request: + branches: [ "master" ] + +env: + # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) + BUILD_TYPE: Release + +jobs: + build: + # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. + # You can convert this to a matrix build if you need cross-platform coverage. + # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + + - name: Configure CMake + # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. + # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + + - name: Build + # Build your program with the given configuration + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + + - name: Test + working-directory: ${{github.workspace}}/build + # Execute tests defined by the CMake configuration. + # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail + run: ctest -C ${{env.BUILD_TYPE}} + From d6341551102394732ac56ff92a6c478ccda1049a Mon Sep 17 00:00:00 2001 From: Chowss Date: Fri, 20 Oct 2023 11:09:32 +0530 Subject: [PATCH 2/2] Added Ld19 Sensor Driver Interface --- libraries/AP_Proximity/AP_Proximity.cpp | 12 ++ libraries/AP_Proximity/AP_Proximity.h | 3 + libraries/AP_Proximity/AP_Proximity_LD19.cpp | 174 +++++++++++++++++++ libraries/AP_Proximity/AP_Proximity_LD19.h | 44 +++++ libraries/AP_Proximity/AP_Proximity_config.h | 4 + 5 files changed, 237 insertions(+) create mode 100644 libraries/AP_Proximity/AP_Proximity_LD19.cpp create mode 100644 libraries/AP_Proximity/AP_Proximity_LD19.h diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index ac1d6786e6c6b..7f9f74f196a30 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -30,6 +30,7 @@ #include "AP_Proximity_DroneCAN.h" #include "AP_Proximity_Scripting.h" #include "AP_Proximity_LD06.h" +#include "AP_Proximity_LD19.h" #include @@ -177,6 +178,17 @@ void AP_Proximity::init() } break; #endif + +#if AP_PROXIMITY_LD19_ENABLED + case Type::LD19: + if (AP_Proximity_LD19::detect(serial_instance)) { + state[instance].instance = instance; + drivers[instance] = new AP_Proximity_LD19(*this, state[instance], params[instance], serial_instance); + serial_instance++; + } + break; +#endif + #if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED case Type::SF45B: if (AP_Proximity_LightWareSF45B::detect(serial_instance)) { diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 0363fea8f4da9..2f3e51e4ef633 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -86,6 +86,9 @@ class AP_Proximity #endif #if AP_PROXIMITY_LD06_ENABLED LD06 = 16, +#endif +#if AP_PROXIMITY_LD19_ENABLED + LD19 = 17, #endif }; diff --git a/libraries/AP_Proximity/AP_Proximity_LD19.cpp b/libraries/AP_Proximity/AP_Proximity_LD19.cpp new file mode 100644 index 0000000000000..3e74ac3f8a66d --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_LD19.cpp @@ -0,0 +1,174 @@ + +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_LD19_ENABLED +#include "AP_Proximity_LD19.h" + +#define LD_START_CHAR 0x54 +#define PROXIMITY_LD19_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_LD19::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_LD19_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_LD19::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) { + + 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; + _response_data = false; + } + } + } +} + +// Parses the data packet received from the LiDAR +void AP_Proximity_LD19::parse_response_data() +{ + + // Data interpretation based on: + // http://wiki.inno-maker.com/display/HOMEPAGE/LD19?preview=/6949506/6949511/LDROBOT_LD19_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 // AP_PROXIMITY_LD19_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_LD19.h b/libraries/AP_Proximity/AP_Proximity_LD19.h new file mode 100644 index 0000000000000..ec9d4517b4f35 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_LD19.h @@ -0,0 +1,44 @@ + +#pragma once +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_LD19_ENABLED +#include "AP_Proximity_Backend_Serial.h" + +#define MESSAGE_LENGTH_LD19 47 + +// Minimum and maximum distance that the sensor can read in meters +#define MAX_READ_DISTANCE_LD19 12.0f +#define MIN_READ_DISTANCE_LD19 0.02f + +class AP_Proximity_LD19 : 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_LD19; } + float distance_min() const override { return MIN_READ_DISTANCE_LD19; } + +private: + + // Get and parse the sensor data + void parse_response_data(); + void get_readings(); + + // Store and keep track of the bytes being read from the sensor + uint8_t _response[MESSAGE_LENGTH_LD19]; + 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 // AP_PROXIMITY_LD19_ENABLED \ No newline at end of file diff --git a/libraries/AP_Proximity/AP_Proximity_config.h b/libraries/AP_Proximity/AP_Proximity_config.h index a2130b0b5c4f2..d13274a2699fc 100644 --- a/libraries/AP_Proximity/AP_Proximity_config.h +++ b/libraries/AP_Proximity/AP_Proximity_config.h @@ -68,3 +68,7 @@ #ifndef AP_PROXIMITY_LD06_ENABLED #define AP_PROXIMITY_LD06_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED #endif + +#ifndef AP_PROXIMITY_LD19_ENABLED +#define AP_PROXIMITY_LD19_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED +#endif \ No newline at end of file