Skip to content

Commit

Permalink
LD06: Record only 2deg slices, I was running into drouble on my Pixhawk
Browse files Browse the repository at this point in the history
  • Loading branch information
@mw46d
mw46d committed May 12, 2024
1 parent 40fc2eb commit 53b22a5
Show file tree
Hide file tree
Showing 2 changed files with 72 additions and 50 deletions.
112 changes: 64 additions & 48 deletions libraries/AP_Proximity/AP_Proximity_LD06.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,22 +102,23 @@ void AP_Proximity_LD06::get_readings()
_response_data = true;

// Stores the next byte in an array
_response[_byte_count] = c;
_byte_count++;
_response[_byte_count++] = c;

// Hard check that we got a valid length. Currently, all packagea are 47 bytes long. Not sure,
// why the recorded length is 44, but it is.
if (_byte_count == START_DATA_LENGTH + 1 && _response[START_DATA_LENGTH] != 44) {
// Resets the bytes read and whether or not we are reading data to accept a new payload
_byte_count = 0;
_response_data = false;
}

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;
}
_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;
Expand All @@ -137,62 +138,77 @@ 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];
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;
// 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;
}

sampled_counts ++;
distance_avg += distance_meas;
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);
}

// initialize the new face
_last_face = face;
_last_distance_valid = false;
}

// 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);

// 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);
_angle_2deg = a2d;
_dist_2deg_m = distance_m;
}
}
}
#endif // AP_PROXIMITY_LD06_ENABLED
10 changes: 8 additions & 2 deletions libraries/AP_Proximity/AP_Proximity_LD06.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,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 = 0;
float _dist_2deg_m = 0.0;
};
#endif // AP_PROXIMITY_LD06_ENABLED

0 comments on commit 53b22a5

Please sign in to comment.