Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Beacon: fix pozyx west-east processing

Browse Source
This commit is contained in:
Randy Mackay 2017-06-14 13:32:40 +09:00
parent f8b5714ced
commit a0d1b3b19a
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Beacon/AP_Beacon_Pozyx.cpp
View File

@ -130,7 +130,7 @@ void AP_Beacon_Pozyx::parse_buffer()
int32_t beacon_x = (uint32_t)linebuf[5] << 24 | (uint32_t)linebuf[4] << 16 | (uint32_t)linebuf[3] << 8 | (uint32_t)linebuf[2];
int32_t beacon_y = (uint32_t)linebuf[9] << 24 | (uint32_t)linebuf[8] << 16 | (uint32_t)linebuf[7] << 8 | (uint32_t)linebuf[6];
int32_t beacon_z = (uint32_t)linebuf[13] << 24 | (uint32_t)linebuf[12] << 16 | (uint32_t)linebuf[11] << 8 | (uint32_t)linebuf[10];
Vector3f beacon_pos(beacon_x / 1000.0f, -beacon_y / 1000.0f, -beacon_z / 1000.0f);
Vector3f beacon_pos(beacon_x / 1000.0f, beacon_y / 1000.0f, -beacon_z / 1000.0f);
if (beacon_pos.length() <= AP_BEACON_DISTANCE_MAX) {
set_beacon_position(beacon_id, beacon_pos);
parsed = true;
@ -156,7 +156,7 @@ void AP_Beacon_Pozyx::parse_buffer()
int32_t vehicle_y = (uint32_t)linebuf[7] << 24 | (uint32_t)linebuf[6] << 16 | (uint32_t)linebuf[5] << 8 | (uint32_t)linebuf[4];
int32_t vehicle_z = (uint32_t)linebuf[11] << 24 | (uint32_t)linebuf[10] << 16 | (uint32_t)linebuf[9] << 8 | (uint32_t)linebuf[8];
int16_t position_error = (uint32_t)linebuf[13] << 8 | (uint32_t)linebuf[12];
Vector3f veh_pos(Vector3f(vehicle_x / 1000.0f, -vehicle_y / 1000.0f, -vehicle_z / 1000.0f));
Vector3f veh_pos(Vector3f(vehicle_x / 1000.0f, vehicle_y / 1000.0f, -vehicle_z / 1000.0f));
if (veh_pos.length() <= AP_BEACON_DISTANCE_MAX) {
set_vehicle_position(veh_pos, position_error);
parsed = true;