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

AP_Beacon: Change from division to multiplication

This commit is contained in:
murata 2022-03-12 02:32:15 +09:00 committed by Andrew Tridgell
parent 9d321c5c31
commit 12b3b22dbc
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

@ -113,7 +113,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 * 0.001f, beacon_y * 0.001f, -beacon_z * 0.001f);
if (beacon_pos.length() <= AP_BEACON_DISTANCE_MAX) {
set_beacon_position(beacon_id, beacon_pos);
parsed = true;
@ -139,7 +139,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 * 0.001f, vehicle_y * 0.001f, -vehicle_z * 0.001f));
if (veh_pos.length() <= AP_BEACON_DISTANCE_MAX) {
set_vehicle_position(veh_pos, position_error);
parsed = true;