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

AP_Beacon: Apply correct conversion from Pozyx beacon earth frame 

The Pozyx beacon system uses a North, West, Up axis system and the APM software uses a North, East, Down convention.
This commit is contained in:
priseborough 2016-12-17 21:02:40 +11:00 committed by Randy Mackay
parent a02a84560f
commit ed0a90ae5f
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;