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

AP_Proximity: Add orientation param to Cygbot

Browse Source
This commit is contained in:
rishabsingh3003 2022-08-22 23:49:20 +05:30 committed by Randy Mackay
parent c0ae7fa5f6
commit 5af59abb8f
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp
View File

@ -139,20 +139,21 @@ void AP_Proximity_Cygbot_D1::parse_payload()
// start from second byte as first byte is part of the header
for (uint16_t i = 2; i < _msg.payload_len; i += 2) {
const float corrected_angle = correct_angle_for_orientation(sampled_angle);
const uint16_t distance_mm = UINT16_VALUE(_msg.payload[i], _msg.payload[i+1]);
float distance_m = distance_mm * 0.001f;
if (distance_m > distance_min() && distance_m < distance_max()) {
if (ignore_reading(sampled_angle, distance_m)) {
if (ignore_reading(corrected_angle, distance_m)) {
// ignore this angle
sampled_angle += CYGBOT_2D_ANGLE_STEP;
continue;
}
// convert angle to face
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(sampled_angle);
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(corrected_angle);
// push face to temp boundary
_temp_boundary.add_distance(face, sampled_angle, distance_m);
_temp_boundary.add_distance(face, corrected_angle, distance_m);
// push to OA_DB
database_push(sampled_angle, distance_m);
database_push(corrected_angle, distance_m);
}
// increment sampled angle
sampled_angle += CYGBOT_2D_ANGLE_STEP;