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_Marvelmind: Record the timestamp once, on success.

Browse Source
This commit is contained in:
Karthik Desai 2018-05-18 15:46:46 +02:00 committed by Francisco Ferreira
parent 372fba2152
commit 4f26bc7cad
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
View File

@ -366,6 +366,7 @@ bool AP_Beacon_Marvelmind::healthy()
void AP_Beacon_Marvelmind::set_stationary_beacons_positions() void AP_Beacon_Marvelmind::set_stationary_beacons_positions()
{ {
bool set = false;
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(hedge.cur_position.y__mm * 0.001f, vehicle_position_NED__m = Vector3f(hedge.cur_position.y__mm * 0.001f,
@ -373,6 +374,7 @@ void AP_Beacon_Marvelmind::set_stationary_beacons_positions()
-hedge.cur_position.z__mm * 0.001f); //Transform Marvelmind ENU to Ardupilot NED -hedge.cur_position.z__mm * 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);
set = true;
Debug(2, Debug(2,
"Hedge is at N%.2f, E%.2f, D%.2f", "Hedge is at N%.2f, E%.2f, D%.2f",
vehicle_position_NED__m[0], vehicle_position_NED__m[0],
@ -386,6 +388,7 @@ void AP_Beacon_Marvelmind::set_stationary_beacons_positions()
hedge.positions_beacons.beacons[i].x__mm * 0.001f, hedge.positions_beacons.beacons[i].x__mm * 0.001f,
-hedge.positions_beacons.beacons[i].z__mm * 0.001f); //Transform Marvelmind ENU to Ardupilot NED -hedge.positions_beacons.beacons[i].z__mm * 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]);
set = true;
Debug(2, Debug(2,
"Beacon %d is at N%.2f, E%.2f, D%.2f", "Beacon %d is at N%.2f, E%.2f, D%.2f",
i, i,
@ -397,6 +400,8 @@ void AP_Beacon_Marvelmind::set_stationary_beacons_positions()
hedge.positions_beacons.updated = false; hedge.positions_beacons.updated = false;
} }
if (set) {
last_update_ms = AP_HAL::millis();
} }
} }