AP_Beacon_Marvelmind: Add function to find beacon's instance for the given address.

This function returns the instance number of a particular beacon's address.
It is needed because the MM sends the distances between the hedge and beacon in a random order so they have to be sorted by address before setting it.
The address of a beacon can be between 0 and 99.

libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp

@@ -234,6 +234,16 @@ void AP_Beacon_Marvelmind::process_beacons_distances_datagram()
     }
 }
 
+int8_t AP_Beacon_Marvelmind::find_beacon_instance(uint8_t address) const
+{
+    for (uint8_t i = 0; i < hedge.positions_beacons.num_beacons; i++) {
+        if (hedge.positions_beacons.beacons[i].address == address) {
+            return i;
+        }
+    }
+    return -1;
+}
+
 void AP_Beacon_Marvelmind::update(void)
 {
     if (uart == nullptr || hedge == nullptr || hedge->position_buffer == nullptr) {

libraries/AP_Beacon/AP_Beacon_Marvelmind.h

@@ -114,6 +114,7 @@ private:
     void process_beacons_distances_datagram();
     void set_stationary_beacons_positions();
     void order_stationary_beacons();
+    int8_t find_beacon_instance(uint8_t address) const;
 
     // Variables for Ardupilot
     AP_HAL::UARTDriver *uart;