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

AP_Beacon: order the stationary beacons in ascending order of address

This commit is contained in:
Karthik Desai 2017-05-12 09:54:02 +02:00 committed by Randy Mackay
parent 8a5fcc82bd
commit 5060ebe626
2 changed files with 25 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
View File

@ -173,6 +173,7 @@ void AP_Beacon_Marvelmind::process_beacons_positions_datagram()
hedge->positions_beacons.updated = true;
}
}
order_stationary_beacons();
}
void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram()
@ -207,6 +208,7 @@ void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram()
hedge->positions_beacons.updated = true;
}
}
order_stationary_beacons();
}
void AP_Beacon_Marvelmind::update(void)
@ -396,3 +398,25 @@ void AP_Beacon_Marvelmind::set_stationary_beacons_positions_and_distances()
hedge->_have_new_values = false;
}
}
void AP_Beacon_Marvelmind::order_stationary_beacons()
{
if(hedge->positions_beacons.updated) {
bool swapped = false;
uint8_t j = hedge->positions_beacons.num_beacons;
do
{
swapped = false;
StationaryBeaconPosition beacon_to_swap;
for(uint8_t i = 1; i < j; i++) {
if(hedge->positions_beacons.beacons[i-1].address > hedge->positions_beacons.beacons[i].address) {
beacon_to_swap = hedge->positions_beacons.beacons[i];
hedge->positions_beacons.beacons[i] = hedge->positions_beacons.beacons[i-1];
hedge->positions_beacons.beacons[i-1] = beacon_to_swap;
swapped = true;
}
}
j--;
} while(swapped);
}
}

1
libraries/AP_Beacon/AP_Beacon_Marvelmind.h
View File

@ -106,6 +106,7 @@ private:
void create_marvelmind_hedge();
void start_marvelmind_hedge();
void set_stationary_beacons_positions_and_distances();
void order_stationary_beacons();
// Variables for Ardupilot
AP_HAL::UARTDriver *uart;