From 5af59abb8f2f29e3fa78082710d41c42d15f5014 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Mon, 22 Aug 2022 23:49:20 +0530 Subject: [PATCH] AP_Proximity: Add orientation param to Cygbot --- libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp b/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp index 654f511daf..3078f93cd9 100644 --- a/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp @@ -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;