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_ */