Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: RPLidarA2 supports yaw-correction and orientation params

This commit is contained in:
Randy Mackay 2019-05-18 12:16:20 +09:00
parent c0e8a443af
commit ff3d3d256c
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
View File

@ -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;