aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Proximity: increase angular resoluion to mavlink packet OBSTACLE_DISTANCE

Browse Source
This commit is contained in:
Tom Pittenger 2019-05-12 12:28:33 -06:00 committed by Tom Pittenger
parent eb37565a9f
commit c67a585b68
1 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Proximity/AP_Proximity_MAV.cpp
View File

@ -88,13 +88,20 @@ void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg)
mavlink_msg_obstacle_distance_decode(msg, &packet);
// check increment (message's sector width)
uint8_t increment = packet.increment;
if (packet.increment == 0) {
float increment;
if (packet.increment_f > 0) {
// use increment float
increment = packet.increment_f;
} else if (packet.increment > 0) {
// use increment uint8_t
increment = packet.increment;
} else {
// invalid increment
return;
}
const float MAX_DISTANCE = 9999.0f;
const uint8_t total_distances = MIN(360.0f / increment, 72);
const float total_distances = MIN(360.0f / increment, MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN); // usually 72
// set distance min and max
_distance_min = packet.min_distance / 100.0f;
@ -102,12 +109,10 @@ void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg)
_last_update_ms = AP_HAL::millis();
// get user configured yaw correction from front end
const float yaw_correction = constrain_float(frontend.get_yaw_correction(state.instance), -360.0f, +360.0f);
float dir_correction;
if (frontend.get_orientation(state.instance) == 0) {
dir_correction = 1.0f;
} else {
dir_correction = -1.0f;
const float param_yaw_offset = constrain_float(frontend.get_yaw_correction(state.instance), -360.0f, +360.0f);
const float yaw_correction = wrap_360(param_yaw_offset + packet.angle_offset);
if (frontend.get_orientation(state.instance) != 0) {
increment *= -1;
}
// initialise updated array and proximity sector angles (to closest object) and distances
@ -123,7 +128,7 @@ void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg)
// iterate over message's sectors
for (uint8_t j = 0; j < total_distances; j++) {
const float packet_distance_m = packet.distances[j] * 0.01f;
const float mid_angle = wrap_360(j * increment * dir_correction + yaw_correction);
const float mid_angle = wrap_360(j * increment + yaw_correction);
// iterate over proximity sectors
for (uint8_t i = 0; i < _num_sectors; i++) {