From 677aa88e3338a8c6e74da3a80773477af7fc4c79 Mon Sep 17 00:00:00 2001 From: Karthik Desai Date: Mon, 8 May 2017 18:10:01 +0200 Subject: [PATCH] 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 | 368 +++++++++++-------- libraries/AP_Beacon/AP_Beacon_Marvelmind.h | 85 +++-- 2 files changed, 266 insertions(+), 187 deletions(-) diff --git a/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp b/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp index 316c6f5d5e..0c0c3311b6 100644 --- a/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp +++ b/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp @@ -1,7 +1,21 @@ /* - * AP_Beacon_Marvelmind.cpp - * - * Created on: 21.03.2017 + 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 + 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 . + */ +/* + Original C Code by Marvelmind (https://bitbucket.org/marvelmind_robotics/) + Adapted into Ardupilot by Karthik Desai, Amilcar Lucas + April 2017 */ #include @@ -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; - if (n_used != 0) { - for (uint8_t i = 0; i < n_used; i++) { - if (hedge->positions_beacons.beacons[i].address == address) { - b = hedge->positions_beacons.beacons[i]; - return true; + uint16_t crc = 0xFFFF; + for (uint16_t pos = 0; pos < len; pos++) { + crc ^= (uint16_t) buf[pos]; // XOR byte into least sig. byte of crc + for (uint8_t i = 8; i != 0; i--) { // Loop over each bit + if ((crc & 0x0001) != 0) { // If the LSB is set + 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 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; - } - } + return crc; } 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; - for (uint16_t pos = 0; pos < len; pos++) { - crc ^= (uint16_t) buf[pos]; // XOR byte into least sig. byte of crc - for (uint8_t i = 8; i != 0; i--) { // Loop over each bit - if ((crc & 0x0001) != 0) { // If the LSB is set - crc >>= 1; // Shift right and XOR 0xA001 - crc ^= 0xA001; - } else { - // Else LSB is not set - crc >>= 1; // Just shift right + const uint8_t n_used = hedge->positions_beacons.num_beacons; + if (n_used != 0) { + for (uint8_t i = 0; i < n_used; i++) { + if (hedge->positions_beacons.beacons[i].address == address) { + return &hedge->positions_beacons.beacons[i]; } } } - 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; - 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; -} - -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::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"); + const uint8_t n = input_buffer[5]; // number of beacons in packet + StationaryBeaconPosition *stationary_beacon; + 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); + 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; } - 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; +} + +void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram() +{ + const uint8_t n = input_buffer[5]; // number of beacons in packet + StationaryBeaconPosition *stationary_beacon; + 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); + 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->positions_beacons.num_beacons = 0; - hedge->positions_beacons.updated = false; } void AP_Beacon_Marvelmind::update(void) @@ -252,9 +239,9 @@ void AP_Beacon_Marvelmind::update(void) case 3: data_id = (((uint16_t)received_char) << 8) + input_buffer[2]; good_byte = (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID) - || (data_id == AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID) - || (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID) - || (data_id == AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID); + || (data_id == AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID) + || (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID) + || (data_id == AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID); break; case 4: { switch (data_id) { @@ -294,36 +281,37 @@ void AP_Beacon_Marvelmind::update(void) uint16_t block_crc = calc_crc_modbus(input_buffer, num_bytes_in_block_received); if (block_crc == 0) { switch (data_id) { - case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID: - // add to position_buffer - process_position_datagram(cur_position); - break; - case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID: - { - process_beacons_positions_datagram(cur_beacon); - Vector3f pos(cur_beacon.x / 1000.0f, - cur_beacon.y / 1000.0f, cur_beacon.z / 1000.0f); - 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; - case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID: - process_beacons_positions_highres_datagram(cur_beacon); - break; - } - // callback - if (hedge->receive_data_callback) { - if (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID) { - hedge->receive_data_callback(cur_position); - Vector3f pos(cur_position.x / 1000.0f, - cur_position.y / 1000.0f, - cur_position.z / 1000.0f); - set_vehicle_position(pos, 0.0f); //TODO: Calculate Accuracy of the received signal - last_update_ms = AP_HAL::millis(); + 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(); + beacon_position_initialized = true; + set_stationary_beacons_positions_and_distances(); + break; + } + + case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID: + { + process_position_highres_datagram(cur_position); + vehicle_position_initialized = true; + set_stationary_beacons_positions_and_distances(); + break; + } + + case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID: + { + process_beacons_positions_highres_datagram(); + beacon_position_initialized = true; + set_stationary_beacons_positions_and_distances(); + break; } } } @@ -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; + } +} diff --git a/libraries/AP_Beacon/AP_Beacon_Marvelmind.h b/libraries/AP_Beacon/AP_Beacon_Marvelmind.h index 48e5f0c32e..6fe80c7346 100644 --- a/libraries/AP_Beacon/AP_Beacon_Marvelmind.h +++ b/libraries/AP_Beacon/AP_Beacon_Marvelmind.h @@ -1,7 +1,21 @@ /* - * AP_Beacon_Marvelmind.h - * - * Created on: 21.03.2017 + 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 + 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 . + */ +/* + 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,11 +35,22 @@ 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; uint32_t timestamp; - int32_t x, y, z;// coordinates in millimeters + int32_t x, y, z; // coordinates in millimeters bool high_resolution; bool ready; bool processed; @@ -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 - struct StationaryBeaconsPositions positions_beacons; + PositionValue * position_buffer; // buffer of measurements + 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; - struct PositionValue cur_position; - struct StationaryBeaconPosition cur_beacon; + MarvelmindHedge *hedge; + PositionValue cur_position; 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_highres_datagram(struct StationaryBeaconPosition &b); - void process_position_highres_datagram(struct PositionValue &p); - void process_position_datagram(struct PositionValue &p); + void process_beacons_positions_datagram(); + void process_beacons_positions_highres_datagram(); + void process_position_highres_datagram(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 stop_marvelmind_hedge(); + void set_stationary_beacons_positions_and_distances(); - AP_HAL::UARTDriver *uart = nullptr; - uint32_t last_update_ms = 0; + // Variables for Ardupilot + 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_ */