Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Beacon: Use multiplications instead of divisions in Marvelmind

Browse Source
This commit is contained in:
Dr.-Ing. Amilcar do Carmo Lucas 2018-03-19 11:40:16 +01:00 committed by Tom Pittenger
parent 914f3385a4
commit 2f59572d42
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
View File

@ -230,7 +230,7 @@ void AP_Beacon_Marvelmind::process_beacons_distances_datagram()
| (((uint32_t) input_buffer[ofs + 2]) << 8) | (((uint32_t) input_buffer[ofs + 2]) << 8)
| (((uint32_t) input_buffer[ofs + 3]) << 16) | (((uint32_t) input_buffer[ofs + 3]) << 16)
| (((uint32_t) input_buffer[ofs + 4]) << 24); | (((uint32_t) input_buffer[ofs + 4]) << 24);
set_beacon_distance(i, raw_beacon_distances.beacon[i].distance / 1000.0f); // millimeters -> meters set_beacon_distance(i, raw_beacon_distances.beacon[i].distance * 0.001f); // millimeters -> meters
} }
} }
@ -397,18 +397,18 @@ void AP_Beacon_Marvelmind::set_stationary_beacons_positions()
{ {
if (vehicle_position_initialized && beacon_position_initialized) { if (vehicle_position_initialized && beacon_position_initialized) {
if (hedge->_have_new_values) { if (hedge->_have_new_values) {
vehicle_position_NED__m = Vector3f(cur_position.y / 1000.0f, vehicle_position_NED__m = Vector3f(cur_position.y * 0.001f,
cur_position.x / 1000.0f, cur_position.x * 0.001f,
-cur_position.z / 1000.0f); //Transform Marvelmind ENU to Ardupilot NED -cur_position.z * 0.001f); //Transform Marvelmind ENU to Ardupilot NED
//TODO: Calculate Accuracy of the received signal. Marvelmind *advertises* +/- 2cms //TODO: Calculate Accuracy of the received signal. Marvelmind *advertises* +/- 2cms
set_vehicle_position(vehicle_position_NED__m, 0.02f); set_vehicle_position(vehicle_position_NED__m, 0.02f);
last_update_ms = AP_HAL::millis(); last_update_ms = AP_HAL::millis();
} }
for (uint8_t i=0; i < hedge->positions_beacons.num_beacons; ++i) { for (uint8_t i=0; i < hedge->positions_beacons.num_beacons; ++i) {
if (hedge->positions_beacons.updated) { if (hedge->positions_beacons.updated) {
beacon_position_NED__m[i] = Vector3f(hedge->positions_beacons.beacons[i].y / 1000.0f, beacon_position_NED__m[i] = Vector3f(hedge->positions_beacons.beacons[i].y * 0.001f,
hedge->positions_beacons.beacons[i].x / 1000.0f, hedge->positions_beacons.beacons[i].x * 0.001f,
-hedge->positions_beacons.beacons[i].z / 1000.0f); //Transform Marvelmind ENU to Ardupilot NED -hedge->positions_beacons.beacons[i].z * 0.001f); //Transform Marvelmind ENU to Ardupilot NED
set_beacon_position(i, beacon_position_NED__m[i]); set_beacon_position(i, beacon_position_NED__m[i]);
} }
} }