Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

LD06: Record only 2deg slices, I was running into drouble on my Pixhawk #27045

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+72 −50
Open

LD06: Record only 2deg slices, I was running into drouble on my Pixhawk #27045

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
112 changes: 64 additions & 48 deletions libraries/AP_Proximity/AP_Proximity_LD06.cpp
View file
Open in desktop
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
Open in desktop
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
Loading