AP_Beacon: transform Marvelmind ENU to ArduPilot NED coordinates

Cache beacon positions to speed-up distance calculations
Only pass data to EKF after both hedgehog and beacon positions are known
Add license and credit Marvelmind
Re-order code around to minimize diff with upstream marvelmind code
Integrated review requests

libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp

@@ -1,7 +1,21 @@
 /*
- * AP_Beacon_Marvelmind.cpp
+   This program is free software: you can redistribute it and/or modify
- *
+   it under the terms of the GNU General Public License as published by
- *  Created on: 21.03.2017
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+ Original C Code by Marvelmind (https://bitbucket.org/marvelmind_robotics/)
+ Adapted into Ardupilot by Karthik Desai, Amilcar Lucas
+ April 2017
  */
 
 #include <AP_HAL/AP_HAL.h>
@@ -17,6 +31,7 @@ AP_Beacon_Marvelmind::AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager
     if (uart != nullptr) {
         uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Beacon, 0));
         hedge = new MarvelmindHedge();
+        last_update_ms = 0;
         if (hedge) {
             create_marvelmind_hedge();
             parse_state = RECV_HDR; // current state of receive data
@@ -29,81 +44,28 @@ AP_Beacon_Marvelmind::AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager
     }
 }
 
-bool AP_Beacon_Marvelmind::get_or_alloc_beacon(struct StationaryBeaconPosition &b, uint8_t address)
+//////////////////////////////////////////////////////////////////////////////
+// Calculate Modbus CRC16 for array of bytes
+// buf: input buffer
+// len: size of buffer
+// returncode: CRC value
+//////////////////////////////////////////////////////////////////////////////
+uint16_t AP_Beacon_Marvelmind::calc_crc_modbus(uint8_t *buf, uint16_t len)
 {
-    const uint8_t n_used = hedge->positions_beacons.num_beacons;
+    uint16_t crc = 0xFFFF;
-    if (n_used != 0) {
+    for (uint16_t pos = 0; pos < len; pos++) {
-        for (uint8_t i = 0; i < n_used; i++) {
+        crc ^= (uint16_t) buf[pos]; // XOR byte into least sig. byte of crc
-            if (hedge->positions_beacons.beacons[i].address == address) {
+        for (uint8_t i = 8; i != 0; i--) { // Loop over each bit
-                b = hedge->positions_beacons.beacons[i];
+            if ((crc & 0x0001) != 0) { // If the LSB is set
-                return true;
+                crc >>= 1; // Shift right and XOR 0xA001
+                crc ^= 0xA001;
+            } else {
+                // Else LSB is not set
+                crc >>= 1; // Just shift right
             }
         }
     }
-    if (n_used >= (AP_BEACON_MARVELMIND_MAX_STATIONARY_BEACONS - 1)) {
+    return crc;
-        return false;
-    }
-    hedge->positions_beacons.num_beacons = (n_used + 1);
-    b = hedge->positions_beacons.beacons[n_used];
-    return true;
-}
-
-void AP_Beacon_Marvelmind::process_beacons_positions_datagram(struct StationaryBeaconPosition &b)
-{
-    const uint8_t n = input_buffer[5]; // number of beacons in packet
-    if ((1 + n * 8) != input_buffer[4]) {
-        return; // incorrect size
-    }
-    for (uint8_t i = 0; i < n; i++) {
-        const uint8_t ofs = 6 + i * 8;
-        const uint8_t address = input_buffer[ofs];
-        const int16_t x = input_buffer[ofs + 1]
-                | (((uint16_t) input_buffer[ofs + 2]) << 8);
-        const int16_t y = input_buffer[ofs + 3]
-                | (((uint16_t) input_buffer[ofs + 4]) << 8);
-        const int16_t z = input_buffer[ofs + 5]
-                | (((uint16_t) input_buffer[ofs + 6]) << 8);
-        if (get_or_alloc_beacon(b, address)) {
-            b.address = address;
-            b.x = x * 10; // millimeters
-            b.y = y * 10; // millimeters
-            b.z = z * 10; // millimeters
-            b.high_resolution = false;
-            hedge->positions_beacons.updated = true;
-        }
-    }
-}
-
-void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram(struct StationaryBeaconPosition &b)
-{
-    const uint8_t n = input_buffer[5]; // number of beacons in packet
-    if ((1 + n * 14) != input_buffer[4]) {
-        return; // incorrect size
-    }
-    for (uint8_t i = 0; i < n; i++) {
-        const uint8_t ofs = 6 + i * 14;
-        const uint8_t address = input_buffer[ofs];
-        const int32_t x = input_buffer[ofs + 1]
-                | (((uint32_t) input_buffer[ofs + 2]) << 8)
-                | (((uint32_t) input_buffer[ofs + 3]) << 16)
-                | (((uint32_t) input_buffer[ofs + 4]) << 24);
-        const int32_t y = input_buffer[ofs + 5]
-                | (((uint32_t) input_buffer[ofs + 6]) << 8)
-                | (((uint32_t) input_buffer[ofs + 7]) << 16)
-                | (((uint32_t) input_buffer[ofs + 8]) << 24);
-        const int32_t z = input_buffer[ofs + 9]
-                | (((uint32_t) input_buffer[ofs + 10]) << 8)
-                | (((uint32_t) input_buffer[ofs + 11]) << 16)
-                | (((uint32_t) input_buffer[ofs + 12]) << 24);
-        if (get_or_alloc_beacon(b, address)) {
-            b.address = address;
-            b.x = x;
-            b.y = y;
-            b.z = z;
-            b.high_resolution = true;
-            hedge->positions_beacons.updated = true;
-        }
-    }
 }
 
 uint8_t AP_Beacon_Marvelmind::mark_position_ready()
@@ -124,7 +86,7 @@ uint8_t AP_Beacon_Marvelmind::mark_position_ready()
     return ind_cur;
 }
 
-void AP_Beacon_Marvelmind::process_position_datagram(struct PositionValue &p)
+void AP_Beacon_Marvelmind::process_position_datagram(AP_Beacon_Marvelmind::PositionValue &p)
 {
     uint8_t ind = hedge->_last_values_next;
     hedge->position_buffer[ind].address = input_buffer[16];
@@ -133,17 +95,17 @@ void AP_Beacon_Marvelmind::process_position_datagram(struct PositionValue &p)
             | (((uint32_t) input_buffer[7]) << 16)
             | (((uint32_t) input_buffer[8]) << 24);
     const int16_t vx = input_buffer[9] | (((uint16_t) input_buffer[10]) << 8);
-    hedge->position_buffer[ind].x = vx * 10; // millimeters
+    hedge->position_buffer[ind].x = vx * 10; // centimeters -> millimeters
     const int16_t vy = input_buffer[11] | (((uint16_t) input_buffer[12]) << 8);
-    hedge->position_buffer[ind].y = vy * 10; // millimeters
+    hedge->position_buffer[ind].y = vy * 10; // centimeters -> millimeters
     const int16_t vz = input_buffer[13] | (((uint16_t) input_buffer[14]) << 8);
-    hedge->position_buffer[ind].z = vz * 10; // millimeters
+    hedge->position_buffer[ind].z = vz * 10; // centimeters -> millimeters
     hedge->position_buffer[ind].high_resolution = false;
     ind = mark_position_ready();
     p = hedge->position_buffer[ind];
 }
 
-void AP_Beacon_Marvelmind::process_position_highres_datagram(struct PositionValue &p)
+void AP_Beacon_Marvelmind::process_position_highres_datagram(AP_Beacon_Marvelmind::PositionValue &p)
 {
     uint8_t ind = hedge->_last_values_next;
     hedge->position_buffer[ind].address = input_buffer[22];
@@ -168,58 +130,83 @@ void AP_Beacon_Marvelmind::process_position_highres_datagram(struct PositionValu
     p = hedge->position_buffer[ind];
 }
 
-uint16_t AP_Beacon_Marvelmind::calc_crc_modbus(uint8_t *buf, uint16_t len)
+AP_Beacon_Marvelmind::StationaryBeaconPosition* AP_Beacon_Marvelmind::get_or_alloc_beacon(uint8_t address)
 {
-    uint16_t crc = 0xFFFF;
+    const uint8_t n_used = hedge->positions_beacons.num_beacons;
-    for (uint16_t pos = 0; pos < len; pos++) {
+    if (n_used != 0) {
-        crc ^= (uint16_t) buf[pos]; // XOR byte into least sig. byte of crc
+        for (uint8_t i = 0; i < n_used; i++) {
-        for (uint8_t i = 8; i != 0; i--) { // Loop over each bit
+            if (hedge->positions_beacons.beacons[i].address == address) {
-            if ((crc & 0x0001) != 0) { // If the LSB is set
+                return &hedge->positions_beacons.beacons[i];
-                crc >>= 1; // Shift right and XOR 0xA001
-                crc ^= 0xA001;
-            } else {
-                // Else LSB is not set
-                crc >>= 1; // Just shift right
             }
         }
     }
-    return crc;
+    if (n_used >= AP_BEACON_MAX_BEACONS) {
+        return nullptr;
+    }
+    hedge->positions_beacons.num_beacons = (n_used + 1);
+    return &hedge->positions_beacons.beacons[n_used];
 }
 
-void AP_Beacon_Marvelmind::create_marvelmind_hedge()
+void AP_Beacon_Marvelmind::process_beacons_positions_datagram()
 {
-    hedge->max_buffered_positions = 3;
+    const uint8_t n = input_buffer[5]; // number of beacons in packet
-    hedge->position_buffer = nullptr;
+    StationaryBeaconPosition *stationary_beacon;
-    hedge->verbose = false;
+    if ((1 + n * 8) != input_buffer[4]) {
-    hedge->receive_data_callback = nullptr;
+        return; // incorrect size
-    hedge->_last_values_count = 0;
+    }
-    hedge->_last_values_next = 0;
+    for (uint8_t i = 0; i < n; i++) {
-    hedge->_have_new_values = false;
+        const uint8_t ofs = 6 + i * 8;
-    hedge->terminationRequired = false;
+        const uint8_t address = input_buffer[ofs];
+        const int16_t x = input_buffer[ofs + 1]
+                | (((uint16_t) input_buffer[ofs + 2]) << 8);
+        const int16_t y = input_buffer[ofs + 3]
+                | (((uint16_t) input_buffer[ofs + 4]) << 8);
+        const int16_t z = input_buffer[ofs + 5]
+                | (((uint16_t) input_buffer[ofs + 6]) << 8);
+        stationary_beacon = get_or_alloc_beacon(address);
+        if (stationary_beacon != nullptr) {
+            stationary_beacon->address = address; //The instance and the address are the same
+            stationary_beacon->x = x * 10; // centimeters -> millimeters
+            stationary_beacon->y = y * 10; // centimeters -> millimeters
+            stationary_beacon->z = z * 10; // centimeters -> millimeters
+            stationary_beacon->high_resolution = false;
+            hedge->positions_beacons.updated = true;
+        }
+    }
 }
 
-bool AP_Beacon_Marvelmind::healthy()
+void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram()
 {
-    // healthy if we have parsed a message within the past 300ms
+    const uint8_t n = input_buffer[5]; // number of beacons in packet
-    return ((AP_HAL::millis() - last_update_ms) < AP_BEACON_TIMEOUT_MS);
+    StationaryBeaconPosition *stationary_beacon;
+    if ((1 + n * 14) != input_buffer[4]) {
+        return; // incorrect size
     }
-
+    for (uint8_t i = 0; i < n; i++) {
-void AP_Beacon_Marvelmind::start_marvelmind_hedge()
+        const uint8_t ofs = 6 + i * 14;
-{
+        const uint8_t address = input_buffer[ofs];
-    hedge->position_buffer = (PositionValue*) malloc(sizeof(struct PositionValue) * hedge->max_buffered_positions);
+        const int32_t x = input_buffer[ofs + 1]
-    if (hedge->position_buffer == nullptr) {
+                | (((uint32_t) input_buffer[ofs + 2]) << 8)
-        if (hedge->verbose) {
+                | (((uint32_t) input_buffer[ofs + 3]) << 16)
-            hal.console->printf("MarvelMind: Not enough memory");
+                | (((uint32_t) input_buffer[ofs + 4]) << 24);
+        const int32_t y = input_buffer[ofs + 5]
+                | (((uint32_t) input_buffer[ofs + 6]) << 8)
+                | (((uint32_t) input_buffer[ofs + 7]) << 16)
+                | (((uint32_t) input_buffer[ofs + 8]) << 24);
+        const int32_t z = input_buffer[ofs + 9]
+                | (((uint32_t) input_buffer[ofs + 10]) << 8)
+                | (((uint32_t) input_buffer[ofs + 11]) << 16)
+                | (((uint32_t) input_buffer[ofs + 12]) << 24);
+        stationary_beacon = get_or_alloc_beacon(address);
+        if (stationary_beacon != nullptr) {
+            stationary_beacon->address = address; //The instance and the address are the same
+            stationary_beacon->x = x; // millimeters
+            stationary_beacon->y = y; // millimeters
+            stationary_beacon->z = z; // millimeters
+            stationary_beacon->high_resolution = true;
+            hedge->positions_beacons.updated = true;
         }
-        hedge->terminationRequired = true;
-        return;
     }
-    for (uint8_t i = 0; i < hedge->max_buffered_positions; i++) {
-        hedge->position_buffer[i].ready = false;
-        hedge->position_buffer[i].processed = false;
-    }
-    hedge->positions_beacons.num_beacons = 0;
-    hedge->positions_beacons.updated = false;
 }
 
 void AP_Beacon_Marvelmind::update(void)
@@ -295,35 +282,36 @@ void AP_Beacon_Marvelmind::update(void)
                 if (block_crc == 0) {
                     switch (data_id) {
                         case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID:
+                        {
                             // add to position_buffer
                             process_position_datagram(cur_position);
+                            vehicle_position_initialized = true;
+                            set_stationary_beacons_positions_and_distances();
                             break;
+                        }
+
                         case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID:
                         {
-                        process_beacons_positions_datagram(cur_beacon);
+                            process_beacons_positions_datagram();
-                        Vector3f pos(cur_beacon.x / 1000.0f,
+                            beacon_position_initialized = true;
-                                cur_beacon.y / 1000.0f, cur_beacon.z / 1000.0f);
+                            set_stationary_beacons_positions_and_distances();
-                        set_beacon_position(cur_beacon.address, pos);
-                        set_beacon_distance(cur_beacon.address, pos.length());
                             break;
                         }
+
                         case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID:
-                        // add to position_buffer
+                        {
                             process_position_highres_datagram(cur_position);
-                        break;
+                            vehicle_position_initialized = true;
-                    case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID:
+                            set_stationary_beacons_positions_and_distances();
-                        process_beacons_positions_highres_datagram(cur_beacon);
                             break;
                         }
-                    // callback
+
-                    if (hedge->receive_data_callback) {
+                        case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID:
-                        if (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID) {
+                        {
-                            hedge->receive_data_callback(cur_position);
+                            process_beacons_positions_highres_datagram();
-                            Vector3f pos(cur_position.x / 1000.0f,
+                            beacon_position_initialized = true;
-                                    cur_position.y / 1000.0f,
+                            set_stationary_beacons_positions_and_distances();
-                                    cur_position.z / 1000.0f);
+                            break;
-                            set_vehicle_position(pos, 0.0f); //TODO: Calculate Accuracy of the received signal
-                            last_update_ms = AP_HAL::millis();
                         }
                     }
                 }
@@ -336,3 +324,75 @@ void AP_Beacon_Marvelmind::update(void)
     }
 }
 
+//////////////////////////////////////////////////////////////////////////////
+// Create and initialize MarvelmindHedge structure
+//////////////////////////////////////////////////////////////////////////////
+void AP_Beacon_Marvelmind::create_marvelmind_hedge()
+{
+    hedge->max_buffered_positions = 3;
+    hedge->position_buffer = nullptr;
+    hedge->verbose = false;
+    hedge->receive_data_callback = nullptr;
+    hedge->_last_values_count = 0;
+    hedge->_last_values_next = 0;
+    hedge->_have_new_values = false;
+    hedge->terminationRequired = false;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Initialize and start work
+//////////////////////////////////////////////////////////////////////////////
+void AP_Beacon_Marvelmind::start_marvelmind_hedge()
+{
+    hedge->position_buffer = (PositionValue*) malloc(sizeof(struct PositionValue) * hedge->max_buffered_positions);
+    if (hedge->position_buffer == nullptr) {
+        if (hedge->verbose) {
+            hal.console->printf("MarvelMind: Not enough memory");
+        }
+        hedge->terminationRequired = true;
+        return;
+    }
+    for (uint8_t i = 0; i < hedge->max_buffered_positions; i++) {
+        hedge->position_buffer[i].ready = false;
+        hedge->position_buffer[i].processed = false;
+    }
+    hedge->positions_beacons.num_beacons = 0;
+    hedge->positions_beacons.updated = false;
+}
+
+bool AP_Beacon_Marvelmind::healthy()
+{
+    // healthy if we have parsed a message within the past 300ms
+    return ((AP_HAL::millis() - last_update_ms) < AP_BEACON_TIMEOUT_MS);
+}
+
+void AP_Beacon_Marvelmind::set_stationary_beacons_positions_and_distances()
+{
+    if (vehicle_position_initialized && beacon_position_initialized) {
+        if (hedge->_have_new_values) {
+            vehicle_position_NED__m = Vector3f(cur_position.y / 1000.0f,
+                                               cur_position.x / 1000.0f,
+                                              -cur_position.z / 1000.0f); //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);
+            last_update_ms = AP_HAL::millis();
+        }
+        for (uint8_t i=0; i < hedge->positions_beacons.num_beacons; ++i) {
+            if (hedge->positions_beacons.updated) {
+                beacon_position_NED__m[i] = Vector3f(hedge->positions_beacons.beacons[i].y / 1000.0f,
+                                                     hedge->positions_beacons.beacons[i].x / 1000.0f,
+                                                    -hedge->positions_beacons.beacons[i].z / 1000.0f); //Transform Marvelmind ENU to Ardupilot NED
+                set_beacon_position(i, beacon_position_NED__m[i]);
+            }
+            if (hedge->_have_new_values) {
+                // this is a big hack:
+                // The distances measured in the hedgehog to each beacon are not available in the Marvelmind serial protocol
+                // As a workaround we use the triangulated position calculated by the Marvelmind hardware and calculate the distances to the beacons
+                // as a result the EKF will not have to resolve ambiguities
+                set_beacon_distance(i, (beacon_position_NED__m[i] - vehicle_position_NED__m).length());
+            }
+        }
+        hedge->positions_beacons.updated = false;
+        hedge->_have_new_values = false;
+    }
+}

libraries/AP_Beacon/AP_Beacon_Marvelmind.h

@@ -1,7 +1,21 @@
 /*
- * AP_Beacon_Marvelmind.h
+   This program is free software: you can redistribute it and/or modify
- *
+   it under the terms of the GNU General Public License as published by
- *  Created on: 21.03.2017
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+ Original C Code by Marvelmind (https://bitbucket.org/marvelmind_robotics/)
+ Adapted into Ardupilot by Karthik Desai, Amilcar Lucas
+ April 2017
  */
 
 #ifndef AP_BEACON_MARVELMIND_H_
@@ -9,7 +23,6 @@
 
 #pragma once
 
-#define AP_BEACON_MARVELMIND_MAX_STATIONARY_BEACONS 30
 #define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID 0x0001
 #define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID 0x0002
 #define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID 0x0011
@@ -22,6 +35,17 @@ class AP_Beacon_Marvelmind : public AP_Beacon_Backend
 {
 public:
 
+    // constructor
+    AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager &serial_manager);
+
+    // return true if sensor is basically healthy (we are receiving data)
+    bool healthy();
+
+    // update
+    void update();
+
+private:
+    // Variables for Marvelmind
     struct PositionValue
     {
         uint8_t address;
@@ -42,63 +66,58 @@ public:
     struct StationaryBeaconsPositions
     {
         uint8_t num_beacons;
-        struct StationaryBeaconPosition beacons[AP_BEACON_MARVELMIND_MAX_STATIONARY_BEACONS];
+        StationaryBeaconPosition beacons[AP_BEACON_MAX_BEACONS];
         bool updated;
     };
 
     struct MarvelmindHedge
     {
         uint8_t max_buffered_positions;   // maximum count of measurements of coordinates stored in buffer, default: 3
-        struct PositionValue * position_buffer;  // buffer of measurements
+        PositionValue * position_buffer;  // buffer of measurements
-        struct StationaryBeaconsPositions positions_beacons;
+        StationaryBeaconsPositions positions_beacons;
         bool verbose;   // verbose flag which activate console output, default: False
         bool pause;     //  pause flag. If True, class would not read serial data
         bool terminationRequired;   //  If True, thread would exit from main loop and stop
-        void (*receive_data_callback)(struct PositionValue position); //  receive_data_callback is callback function to recieve data
+        void (*receive_data_callback)(PositionValue position); //  receive_data_callback is callback function to receive data
 
-        // private variables
         uint8_t _last_values_count;
         uint8_t _last_values_next;
         bool _have_new_values;
     };
 
-    // constructor
-    AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager &serial_manager);
-
-    // return true if sensor is basically healthy (we are receiving data)
-    bool healthy();
-
-    // update
-    void update();
-
-private:
     enum {
         RECV_HDR,
         RECV_DGRAM
     } parse_state; // current state of receive data
 
-    struct MarvelmindHedge *hedge;
+    MarvelmindHedge *hedge;
-    struct PositionValue cur_position;
+    PositionValue cur_position;
-    struct StationaryBeaconPosition cur_beacon;
     uint8_t input_buffer[AP_BEACON_MARVELMIND_BUF_SIZE];
     uint16_t num_bytes_in_block_received;
     uint16_t data_id;
 
-    struct MarvelmindHedge* m_MarvelmindHedge;
     uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len);
-    bool get_or_alloc_beacon(struct StationaryBeaconPosition &b, uint8_t address);
+    StationaryBeaconPosition* get_or_alloc_beacon(uint8_t address);
     uint8_t mark_position_ready();
-    void process_beacons_positions_datagram(struct StationaryBeaconPosition &b);
+    void process_beacons_positions_datagram();
-    void process_beacons_positions_highres_datagram(struct StationaryBeaconPosition &b);
+    void process_beacons_positions_highres_datagram();
-    void process_position_highres_datagram(struct PositionValue &p);
+    void process_position_highres_datagram(PositionValue &p);
-    void process_position_datagram(struct PositionValue &p);
+    void process_position_datagram(PositionValue &p);
     void create_marvelmind_hedge();
     void start_marvelmind_hedge();
-    bool get_position_from_marvelmind_hedge(struct PositionValue *position);
+    void set_stationary_beacons_positions_and_distances();
-    void stop_marvelmind_hedge();
 
-    AP_HAL::UARTDriver *uart = nullptr;
+    // Variables for Ardupilot
-    uint32_t last_update_ms = 0;
+    AP_HAL::UARTDriver *uart;
+    uint32_t last_update_ms;
+
+    // cache the vehicle position in NED coordinates [m]
+    Vector3f vehicle_position_NED__m;
+    bool vehicle_position_initialized;
+
+    // cache the beacon positions in NED coordinates [m]
+    Vector3f beacon_position_NED__m[AP_BEACON_MAX_BEACONS];
+    bool beacon_position_initialized;
 };
 
 #endif /* AP_BEACON_MARVELMIND_H_ */