From ff3d3d256c99aae7ea926e117994744e3d59754c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 18 May 2019 12:16:20 +0900 Subject: [PATCH] AP_Proximity: RPLidarA2 supports yaw-correction and orientation params --- libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp index 257883c6f6..82f598ddb3 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp +++ b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp @@ -394,7 +394,8 @@ void AP_Proximity_RPLidarA2::parse_response_data() Debug(2, "UART %02x %02x%02x %02x%02x", payload[0], payload[2], payload[1], payload[4], payload[3]); //show HEX values // check if valid SCAN packet: a valid packet starts with startbits which are complementary plus a checkbit in byte+1 if ((payload.sensor_scan.startbit == !payload.sensor_scan.not_startbit) && payload.sensor_scan.checkbit) { - const float angle_deg = payload.sensor_scan.angle_q6/64.0f; + const float angle_sign = (frontend.get_orientation(state.instance) == 1) ? -1.0f : 1.0f; + const float angle_deg = wrap_360(payload.sensor_scan.angle_q6/64.0f * angle_sign + frontend.get_yaw_correction(state.instance)); const float distance_m = (payload.sensor_scan.distance_q2/4000.0f); #if RP_DEBUG_LEVEL >= 2 const float quality = payload.sensor_scan.quality;