From 3bcc410048dad57c987b33871aa20d2980cb49b6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 14 Jun 2017 13:32:40 +0900 Subject: [PATCH] AP_Beacon: fix pozyx west-east processing --- libraries/AP_Beacon/AP_Beacon_Pozyx.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Beacon/AP_Beacon_Pozyx.cpp b/libraries/AP_Beacon/AP_Beacon_Pozyx.cpp index 886a6495f1..4b3932e064 100644 --- a/libraries/AP_Beacon/AP_Beacon_Pozyx.cpp +++ b/libraries/AP_Beacon/AP_Beacon_Pozyx.cpp @@ -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;