diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 372bd1b7dab978..8b5428be9936be 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -7582,6 +7582,16 @@ def ProximitySensors(self): "OA_TYPE": 2, }) sensors = [ # tuples of name, prx_type + ('ld06', 16, { + mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 273, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1130, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 696, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 625, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 967, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 760, + mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 771, + }), ('sf45b', 8, { mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 270, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 258, diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index 47a625d4d77670..4763ea8292e191 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -184,6 +184,14 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const crsf = NEW_NOTHROW SITL::CRSF(); return crsf; #endif +#if AP_SIM_PS_LD06_ENABLED + } else if (streq(name, "ld06")) { + if (ld06 != nullptr) { + AP_HAL::panic("Only one ld06 at a time"); + } + ld06 = NEW_NOTHROW SITL::PS_LD06(); + return ld06; +#endif // AP_SIM_PS_LD06_ENABLED #if HAL_SIM_PS_RPLIDARA2_ENABLED } else if (streq(name, "rplidara2")) { if (rplidara2 != nullptr) { @@ -431,6 +439,12 @@ void SITL_State_Common::sim_update(void) } #endif +#if AP_SIM_PS_LD06_ENABLED + if (ld06 != nullptr) { + ld06->update(sitl_model->get_location()); + } +#endif // AP_SIM_PS_LD06_ENABLED + #if HAL_SIM_PS_RPLIDARA2_ENABLED if (rplidara2 != nullptr) { rplidara2->update(sitl_model->get_location()); diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index 472f7dfefa6601..e102d84ef95884 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -44,6 +44,7 @@ #include // #include // #include +#include #include #include #include @@ -172,6 +173,11 @@ class HALSITL::SITL_State_Common { // SITL::Frsky_SPort *frsky_sport; // SITL::Frsky_SPortPassthrough *frsky_sportpassthrough; +#if AP_SIM_PS_LD06_ENABLED + // simulated LD06: + SITL::PS_LD06 *ld06; +#endif // AP_SIM_PS_LD06_ENABLED + #if HAL_SIM_PS_RPLIDARA2_ENABLED // simulated RPLidarA2: SITL::PS_RPLidarA2 *rplidara2; diff --git a/libraries/AP_Proximity/AP_Proximity/examples/LD06/sample.dump b/libraries/AP_Proximity/AP_Proximity/examples/LD06/sample.dump new file mode 100644 index 00000000000000..b791334259f0c5 Binary files /dev/null and b/libraries/AP_Proximity/AP_Proximity/examples/LD06/sample.dump differ diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.cpp b/libraries/AP_Proximity/AP_Proximity_LD06.cpp index 88d5f5b4f3e44f..69781e21ee8f1b 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LD06.cpp @@ -106,8 +106,10 @@ void AP_Proximity_LD06::get_readings() continue; } - const uint32_t total_packet_length = _response[START_DATA_LENGTH] + 3; - if (total_packet_length > ARRAY_SIZE(_response)) { + // total_packet_length = sizeof(header) + datalength + sizeof(footer): + const uint32_t total_packet_length = 6 + 3*_response[START_DATA_LENGTH] + 5; + if (_response[START_DATA_LENGTH] != PAYLOAD_COUNT || + total_packet_length > ARRAY_SIZE(_response)) { // invalid packet received; throw away all data and // start again. _byte_count = 0; @@ -121,15 +123,10 @@ void AP_Proximity_LD06::get_readings() 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; - } + _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; @@ -148,12 +145,6 @@ void AP_Proximity_LD06::parse_response_data() // 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]; @@ -161,49 +152,68 @@ void AP_Proximity_LD06::parse_response_data() 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; + // 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; - // 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); + float angle_step; + if (start_angle < end_angle) { + angle_step = (end_angle - start_angle) / (PAYLOAD_COUNT - 1); + } else { + angle_step = (end_angle + 360 - start_angle) / (PAYLOAD_COUNT - 1); } - - // 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; + const float angle_deg = correct_angle_for_orientation(start_angle + angle_step * (i / MEASUREMENT_PAYLOAD_LENGTH)); + const float distance_m = 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; + if (distance_m < distance_min() || distance_m > distance_max() || _response[i + 2] < 20) { // XXX 20 good? + continue; + } + if (ignore_reading(angle_deg, distance_m)) { + continue; + } + + uint16_t a2d = (int)(angle_deg / 2.0) * 2; + if (_angle_2deg == a2d) { + if (distance_m < _dist_2deg_m) { + _dist_2deg_m = distance_m; } + } else { + // New 2deg angle, record the old one + + const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face((float)_angle_2deg); + + if (face != _last_face) { + // distance is for a new face, the previous one can be updated now + if (_last_distance_valid) { + frontend.boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m, state.instance); + } else { + // reset distance from last face + frontend.boundary.reset_face(face, state.instance); + } - sampled_counts ++; - distance_avg += distance_meas; - } - } + // initialize the new face + _last_face = face; + _last_distance_valid = false; + } - // 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); + // update shortest distance + if (!_last_distance_valid || (_dist_2deg_m < _last_distance_m)) { + _last_distance_m = _dist_2deg_m; + _last_distance_valid = true; + _last_angle_deg = (float)_angle_2deg; + } + // update OA database + database_push(_last_angle_deg, _last_distance_m); + + _angle_2deg = a2d; + _dist_2deg_m = distance_m; + } } } #endif // AP_PROXIMITY_LD06_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.h b/libraries/AP_Proximity/AP_Proximity_LD06.h index af8bd37edd7df6..949e56240437b0 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.h +++ b/libraries/AP_Proximity/AP_Proximity_LD06.h @@ -62,7 +62,13 @@ class AP_Proximity_LD06 : public AP_Proximity_Backend_Serial // Store for error-tracking purposes uint32_t _last_distance_received_ms; - // Boundary to store the measurements - AP_Proximity_Temp_Boundary _temp_boundary; + // face related variables + AP_Proximity_Boundary_3D::Face _last_face;///< last face requested + float _last_angle_deg; ///< yaw angle (in degrees) of _last_distance_m + float _last_distance_m; ///< shortest distance for _last_face + bool _last_distance_valid; ///< true if _last_distance_m is valid + + uint16_t _angle_2deg; + float _dist_2deg_m; }; #endif // AP_PROXIMITY_LD06_ENABLED diff --git a/libraries/SITL/SIM_PS_LD06.cpp b/libraries/SITL/SIM_PS_LD06.cpp new file mode 100644 index 00000000000000..416694a2e30d5c --- /dev/null +++ b/libraries/SITL/SIM_PS_LD06.cpp @@ -0,0 +1,162 @@ +/* + 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 . + */ +/* + Simulator for the LD06 proximity sensors +*/ + +#include "SIM_config.h" + +#if AP_SIM_PS_LD06_ENABLED + +#include "SIM_PS_LD06.h" + +#include +#include +#include +#include +#include + +using namespace SITL; + +uint32_t PS_LD06::packet_for_location(const Location &location, + uint8_t *data, + uint8_t buflen) +{ + return 0; +} + +void PS_LD06::update_input() +{ + // just discard any input + char buffer[256]; + uint8_t buflen = 0; + read_from_autopilot(&buffer[buflen], ARRAY_SIZE(buffer) - buflen - 1); +} + +void PS_LD06::update_output(const Location &location) +{ + const uint32_t now_ms = AP_HAL::millis(); + if (last_scan_output_time_ms == 0) { + last_scan_output_time_ms = now_ms; + return; + } + const uint32_t time_delta_ms = (now_ms - last_scan_output_time_ms); + + // the driver and device only send/handle 12 readings at a time at + // the moment + const uint32_t sample_count = 12; + + const uint32_t samples_per_second = 4500; + const float samples_per_ms = samples_per_second / 1000.0f; + + const uint32_t required_time_delta_ms = sample_count * samples_per_ms; + + if (time_delta_ms < required_time_delta_ms) { + return; + } + // sanity check we're being called often enough: + if (time_delta_ms / samples_per_ms > sample_count) { + AP_HAL::panic("Not being called often enough"); + } + + const float degrees_per_s = 2152; // see example datasheet page 8 + const float degrees_per_ms = degrees_per_s / 1000.0f; + const float degrees_per_sample = degrees_per_ms / samples_per_ms; + + // ::fprintf(stderr, "Packing %u samples in for %ums interval (%f degrees/sample)\n", sample_count, time_delta_ms, degrees_per_sample); + + last_scan_output_time_ms += sample_count/samples_per_ms; + + const uint32_t required_send_buffer_size = 11 + 3*sample_count; + if (required_send_buffer_size > send_buffer_size) { + if (send_buffer != nullptr) { + free(send_buffer); + } + send_buffer = (uint8_t*)malloc(required_send_buffer_size); + if (send_buffer == nullptr) { + AP_BoardConfig::allocation_error("LD06 send buffer"); + } + send_buffer_size = required_send_buffer_size; + } + + struct PACKED PacketStart { + uint8_t preamble; + uint8_t length : 5; + uint8_t reserved : 3; + uint16_t angular_rate; // degrees/second + uint16_t start_angle; // centidegrees + }; + struct PACKED Measurement { + uint16_t distance; + uint8_t confidence; + }; + struct PACKED PacketEnd { + uint16_t end_angle; + uint16_t timestamp; + uint8_t crc; + }; + + PacketStart &packet_start = *((PacketStart*)send_buffer); + + packet_start.preamble = 0x54; + packet_start.length = sample_count; + packet_start.angular_rate = htole16(degrees_per_s); + packet_start.start_angle = htole16(last_degrees_bf * 100); + + uint16_t offset = sizeof(PacketStart); + + for (uint32_t i=0; i 12.0) { // 12 metre max range + // return 0 for out-of-range; is this correct? + distance = 0.0f; + confidence = 0; + } + + Measurement &measurement = *((Measurement*)&send_buffer[offset]); + + measurement.distance = htole16(distance * 1000); // m -> mm + measurement.confidence = confidence; + + offset += sizeof(Measurement); + } + + PacketEnd &packet_end = *((PacketEnd*)&send_buffer[offset]); + + packet_end.end_angle = htole16(last_degrees_bf * 100); // centidegrees + packet_end.timestamp = htole16(0); // milliseconds + offset += sizeof(PacketEnd); + + packet_end.crc = crc8_generic(&send_buffer[0], offset-1, 0x4D); + + const ssize_t ret = write_to_autopilot((const char*)send_buffer, offset); + if (ret != offset) { + // abort(); // there are startup issues with this, we fill things up before the driver is ready + } +} + +void PS_LD06::update(const Location &location) +{ + update_input(); + update_output(location); +} + +#endif // AP_SIM_PS_LD06_ENABLED diff --git a/libraries/SITL/SIM_PS_LD06.h b/libraries/SITL/SIM_PS_LD06.h new file mode 100644 index 00000000000000..17f49959203f6e --- /dev/null +++ b/libraries/SITL/SIM_PS_LD06.h @@ -0,0 +1,86 @@ +/* + 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 . + + Simulator for the LD06 proximity sensor + + Datasheet: https://storage.googleapis.com/mauser-public-images/prod_description_document/2021/315/8fcea7f5d479f4f4b71316d80b77ff45_096-6212_a.pdf + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:ld06 --speedup=1 -l 51.8752066,14.6487840,54.15,0 --map + +param set SERIAL5_PROTOCOL 11 +param set PRX1_TYPE 16 +reboot + +arm throttle +rc 3 1600 + +# for avoidance: +param set DISARM_DELAY 0 +param set AVOID_ENABLE 2 # use proximity sensor +param set AVOID_MARGIN 2.00 # 2m +param set AVOID_BEHAVE 0 # slide +param set OA_TYPE 2 +reboot + +param ftp +param set OA_DB_OUTPUT 3 + +mode loiter +script /tmp/post-locations.scr +arm throttle +rc 3 1600 +rc 3 1500 +rc 2 1450 + +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_PS_LD06_ENABLED + +#include "SIM_SerialProximitySensor.h" + +#include + +namespace SITL { + +class PS_LD06 : public SerialProximitySensor { +public: + + using SerialProximitySensor::SerialProximitySensor; + + uint32_t packet_for_location(const Location &location, + uint8_t *data, + uint8_t buflen) override; + + void update(const Location &location) override; + +private: + + void update_input(); // in the form of PWM (uninplemented) + void update_output(const Location &location); + + uint32_t last_scan_output_time_ms; + + float last_degrees_bf; + + uint8_t * send_buffer; + uint16_t send_buffer_size; +}; + +}; + +#endif // AP_SIM_PS_LD06_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 5d6850bd39c01e..782b5b8b253473 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -21,6 +21,10 @@ #define HAL_SIM_PS_RPLIDARA2_ENABLED HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED #endif +#ifndef AP_SIM_PS_LD06_ENABLED +#define AP_SIM_PS_LD06_ENABLED HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED +#endif + #ifndef AP_SIM_IS31FL3195_ENABLED #define AP_SIM_IS31FL3195_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif