diff --git a/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp b/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp index dc34b49e4f..dadb8551a6 100644 --- a/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp +++ b/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp @@ -366,6 +366,7 @@ bool AP_Beacon_Marvelmind::healthy() void AP_Beacon_Marvelmind::set_stationary_beacons_positions() { + bool set = false; if (vehicle_position_initialized && beacon_position_initialized) { if (hedge._have_new_values) { 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 //TODO: Calculate Accuracy of the received signal. Marvelmind *advertises* +/- 2cms set_vehicle_position(vehicle_position_NED__m, 0.02f); + set = true; Debug(2, "Hedge is at N%.2f, E%.2f, D%.2f", 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].z__mm * 0.001f); //Transform Marvelmind ENU to Ardupilot NED set_beacon_position(i, beacon_position_NED__m[i]); + set = true; Debug(2, "Beacon %d is at N%.2f, E%.2f, D%.2f", i, @@ -397,6 +400,8 @@ void AP_Beacon_Marvelmind::set_stationary_beacons_positions() hedge.positions_beacons.updated = false; } + if (set) { + last_update_ms = AP_HAL::millis(); } }