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

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
This commit is contained in:
Karthik Desai 2017-05-08 18:10:01 +02:00 committed by Randy Mackay
parent 43f22cf502
commit 677aa88e33
2 changed files with 266 additions and 187 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

368
libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
View File

@ -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> #include <AP_HAL/AP_HAL.h>
@ -17,6 +31,7 @@ AP_Beacon_Marvelmind::AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager
if (uart != nullptr) { if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Beacon, 0)); uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Beacon, 0));
hedge = new MarvelmindHedge(); hedge = new MarvelmindHedge();
last_update_ms = 0;
if (hedge) { if (hedge) {
create_marvelmind_hedge(); create_marvelmind_hedge();
parse_state = RECV_HDR; // current state of receive data 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() uint8_t AP_Beacon_Marvelmind::mark_position_ready()
@ -124,7 +86,7 @@ uint8_t AP_Beacon_Marvelmind::mark_position_ready()
return ind_cur; 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; uint8_t ind = hedge->_last_values_next;
hedge->position_buffer[ind].address = input_buffer[16]; 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[7]) << 16)
| (((uint32_t) input_buffer[8]) << 24); | (((uint32_t) input_buffer[8]) << 24);
const int16_t vx = input_buffer[9] | (((uint16_t) input_buffer[10]) << 8); 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); 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); 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; hedge->position_buffer[ind].high_resolution = false;
ind = mark_position_ready(); ind = mark_position_ready();
p = hedge->position_buffer[ind]; 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; uint8_t ind = hedge->_last_values_next;
hedge->position_buffer[ind].address = input_buffer[22]; 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]; 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);
bool AP_Beacon_Marvelmind::healthy() const int16_t y = input_buffer[ofs + 3]
{ | (((uint16_t) input_buffer[ofs + 4]) << 8);
// healthy if we have parsed a message within the past 300ms const int16_t z = input_buffer[ofs + 5]
return ((AP_HAL::millis() - last_update_ms) < AP_BEACON_TIMEOUT_MS); | (((uint16_t) input_buffer[ofs + 6]) << 8);
} stationary_beacon = get_or_alloc_beacon(address);
if (stationary_beacon != nullptr) {
void AP_Beacon_Marvelmind::start_marvelmind_hedge() stationary_beacon->address = address; //The instance and the address are the same
{ stationary_beacon->x = x * 10; // centimeters -> millimeters
hedge->position_buffer = (PositionValue*) malloc(sizeof(struct PositionValue) * hedge->max_buffered_positions); stationary_beacon->y = y * 10; // centimeters -> millimeters
if (hedge->position_buffer == nullptr) { stationary_beacon->z = z * 10; // centimeters -> millimeters
if (hedge->verbose) { stationary_beacon->high_resolution = false;
hal.console->printf("MarvelMind: Not enough memory"); 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) void AP_Beacon_Marvelmind::update(void)
@ -252,9 +239,9 @@ void AP_Beacon_Marvelmind::update(void)
case 3: case 3:
data_id = (((uint16_t)received_char) << 8) + input_buffer[2]; data_id = (((uint16_t)received_char) << 8) + input_buffer[2];
good_byte = (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID) good_byte = (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID)
|| (data_id == AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID) || (data_id == AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID)
|| (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_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_HIGHRES_ID);
break; break;
case 4: { case 4: {
switch (data_id) { 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); uint16_t block_crc = calc_crc_modbus(input_buffer, num_bytes_in_block_received);
if (block_crc == 0) { if (block_crc == 0) {
switch (data_id) { switch (data_id) {
case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID: case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID:
// add to position_buffer {
process_position_datagram(cur_position); // add to position_buffer
break; process_position_datagram(cur_position);
case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID: vehicle_position_initialized = true;
{ set_stationary_beacons_positions_and_distances();
process_beacons_positions_datagram(cur_beacon); break;
Vector3f pos(cur_beacon.x / 1000.0f, }
cur_beacon.y / 1000.0f, cur_beacon.z / 1000.0f);
set_beacon_position(cur_beacon.address, pos); case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID:
set_beacon_distance(cur_beacon.address, pos.length()); {
break; process_beacons_positions_datagram();
} beacon_position_initialized = true;
case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID: set_stationary_beacons_positions_and_distances();
// add to position_buffer break;
process_position_highres_datagram(cur_position); }
break;
case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID: case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID:
process_beacons_positions_highres_datagram(cur_beacon); {
break; process_position_highres_datagram(cur_position);
} vehicle_position_initialized = true;
// callback set_stationary_beacons_positions_and_distances();
if (hedge->receive_data_callback) { break;
if (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID) { }
hedge->receive_data_callback(cur_position);
Vector3f pos(cur_position.x / 1000.0f, case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID:
cur_position.y / 1000.0f, {
cur_position.z / 1000.0f); process_beacons_positions_highres_datagram();
set_vehicle_position(pos, 0.0f); //TODO: Calculate Accuracy of the received signal beacon_position_initialized = true;
last_update_ms = AP_HAL::millis(); 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;
}
}

85
libraries/AP_Beacon/AP_Beacon_Marvelmind.h
View File

@ -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_ #ifndef AP_BEACON_MARVELMIND_H_
@ -9,7 +23,6 @@
#pragma once #pragma once
#define AP_BEACON_MARVELMIND_MAX_STATIONARY_BEACONS 30
#define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID 0x0001 #define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID 0x0001
#define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID 0x0002 #define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID 0x0002
#define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID 0x0011 #define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID 0x0011
@ -22,11 +35,22 @@ class AP_Beacon_Marvelmind : public AP_Beacon_Backend
{ {
public: 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 struct PositionValue
{ {
uint8_t address; uint8_t address;
uint32_t timestamp; uint32_t timestamp;
int32_t x, y, z;// coordinates in millimeters int32_t x, y, z; // coordinates in millimeters
bool high_resolution; bool high_resolution;
bool ready; bool ready;
bool processed; bool processed;
@ -42,63 +66,58 @@ public:
struct StationaryBeaconsPositions struct StationaryBeaconsPositions
{ {
uint8_t num_beacons; uint8_t num_beacons;
struct StationaryBeaconPosition beacons[AP_BEACON_MARVELMIND_MAX_STATIONARY_BEACONS]; StationaryBeaconPosition beacons[AP_BEACON_MAX_BEACONS];
bool updated; bool updated;
}; };
struct MarvelmindHedge struct MarvelmindHedge
{ {
uint8_t max_buffered_positions; // maximum count of measurements of coordinates stored in buffer, default: 3 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 verbose; // verbose flag which activate console output, default: False
bool pause; // pause flag. If True, class would not read serial data bool pause; // pause flag. If True, class would not read serial data
bool terminationRequired; // If True, thread would exit from main loop and stop 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_count;
uint8_t _last_values_next; uint8_t _last_values_next;
bool _have_new_values; 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 { enum {
RECV_HDR, RECV_HDR,
RECV_DGRAM RECV_DGRAM
} parse_state; // current state of receive data } 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]; uint8_t input_buffer[AP_BEACON_MARVELMIND_BUF_SIZE];
uint16_t num_bytes_in_block_received; uint16_t num_bytes_in_block_received;
uint16_t data_id; uint16_t data_id;
struct MarvelmindHedge* m_MarvelmindHedge;
uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len); 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(); 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 create_marvelmind_hedge();
void start_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_ */ #endif /* AP_BEACON_MARVELMIND_H_ */