Skip to content

Commit

Permalink
AP_Beacon: Added Marvelmind Beacon class
Browse files Browse the repository at this point in the history 
Code cleanup, adhering to Ardupilot code style
Marvelmind code fixes
formatting fixes including replacing camel case with underscore
replaced puts with ::printf
removed unused debug and counter

Removed useless library includes.
Corrected spellings.
Update the last_update_ms so healthy() doesn't break.
  • Loading branch information
@karthikdesai @rmackay9
karthikdesai authored and rmackay9 committed Apr 26, 2017
1 parent 47ff9dd commit 44d84fc
Show file tree
Hide file tree
Showing 4 changed files with 447 additions and 1 deletion.
5 changes: 4 additions & 1 deletion libraries/AP_Beacon/AP_Beacon.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include "AP_Beacon.h"
#include "AP_Beacon_Backend.h"
#include "AP_Beacon_Pozyx.h"
#include "AP_Beacon_Marvelmind.h"
#include "AP_Beacon_SITL.h"

extern const AP_HAL::HAL &hal;
Expand All @@ -26,7 +27,7 @@ const AP_Param::GroupInfo AP_Beacon::var_info[] = {
// @Param: _TYPE
// @DisplayName: Beacon based position estimation device type
// @Description: What type of beacon based position estimation device is connected
// @Values: 0:None,1:Pozyx
// @Values: 0:None,1:Pozyx,2:Marvelmind
// @User: Advanced
AP_GROUPINFO("_TYPE", 0, AP_Beacon, _type, 0),

Expand Down Expand Up @@ -86,6 +87,8 @@ void AP_Beacon::init(void)
// create backend
if (_type == AP_BeaconType_Pozyx) {
_driver = new AP_Beacon_Pozyx(*this, serial_manager);
} else if (_type == AP_BeaconType_Marvelmind) {
_driver = new AP_Beacon_Marvelmind(*this, serial_manager);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (_type == AP_BeaconType_SITL) {
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_Beacon/AP_Beacon.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ class AP_Beacon
enum AP_BeaconType {
AP_BeaconType_None = 0,
AP_BeaconType_Pozyx = 1,
AP_BeaconType_Marvelmind = 2,
AP_BeaconType_SITL = 10
};

Expand Down
338 changes: 338 additions & 0 deletions libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,338 @@
/*
* AP_Beacon_Marvelmind.cpp
*
* Created on: 21.03.2017
*/

#include <AP_HAL/AP_HAL.h>

#include "AP_Beacon_Marvelmind.h"

extern const AP_HAL::HAL& hal;

AP_Beacon_Marvelmind::AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager &serial_manager) :
AP_Beacon_Backend(frontend)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Beacon, 0);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Beacon, 0));
hedge = new MarvelmindHedge();
if (hedge) {
create_marvelmind_hedge();
parse_state = RECV_HDR; // current state of receive data
num_bytes_in_block_received = 0; // bytes received
data_id = 0;
start_marvelmind_hedge();
} else {
// initialising beacon failed
}
}
}

bool AP_Beacon_Marvelmind::get_or_alloc_beacon(struct StationaryBeaconPosition &b, uint8_t address)
{
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;
}
}
}
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;
}
}
}

uint8_t AP_Beacon_Marvelmind::mark_position_ready()
{
uint8_t ind = hedge->_last_values_next;
const uint8_t ind_cur = ind;
hedge->position_buffer[ind].ready = true;
hedge->position_buffer[ind].processed = false;
ind++;
if (ind >= hedge->max_buffered_positions) {
ind = 0;
}
if (hedge->_last_values_count < hedge->max_buffered_positions) {
hedge->_last_values_count++;
}
hedge->_have_new_values = true;
hedge->_last_values_next = ind;
return ind_cur;
}

void AP_Beacon_Marvelmind::process_position_datagram(struct PositionValue &p)
{
uint8_t ind = hedge->_last_values_next;
hedge->position_buffer[ind].address = input_buffer[16];
hedge->position_buffer[ind].timestamp = input_buffer[5]
| (((uint32_t) input_buffer[6]) << 8)
| (((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
const int16_t vy = input_buffer[11] | (((uint16_t) input_buffer[12]) << 8);
hedge->position_buffer[ind].y = vy * 10; // 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].high_resolution = false;
ind = mark_position_ready();
p = hedge->position_buffer[ind];
}

void AP_Beacon_Marvelmind::process_position_highres_datagram(struct PositionValue &p)
{
uint8_t ind = hedge->_last_values_next;
hedge->position_buffer[ind].address = input_buffer[22];
hedge->position_buffer[ind].timestamp = input_buffer[5]
| (((uint32_t) input_buffer[6]) << 8)
| (((uint32_t) input_buffer[7]) << 16)
| (((uint32_t) input_buffer[8]) << 24);
const int32_t vx = input_buffer[9] | (((uint32_t) input_buffer[10]) << 8)
| (((uint32_t) input_buffer[11]) << 16)
| (((uint32_t) input_buffer[12]) << 24);
hedge->position_buffer[ind].x = vx;
const int32_t vy = input_buffer[13] | (((uint32_t) input_buffer[14]) << 8)
| (((uint32_t) input_buffer[15]) << 16)
| (((uint32_t) input_buffer[16]) << 24);
hedge->position_buffer[ind].y = vy;
const int32_t vz = input_buffer[17] | (((uint32_t) input_buffer[18]) << 8)
| (((uint32_t) input_buffer[19]) << 16)
| (((uint32_t) input_buffer[20]) << 24);
hedge->position_buffer[ind].z = vz;
hedge->position_buffer[ind].high_resolution = true;
ind = mark_position_ready();
p = hedge->position_buffer[ind];
}

uint16_t AP_Beacon_Marvelmind::calc_crc_modbus(uint8_t *buf, uint16_t len)
{
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
}
}
}
return crc;
}

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;
}

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");
}
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)
{
if (uart == nullptr) {
return;
}
// read any available characters
int32_t num_bytes_read = uart->available();
uint8_t received_char = 0;
if (num_bytes_read < 0) {
return;
}
while (num_bytes_read-- > 0) {
bool good_byte = false;
received_char = uart->read();
input_buffer[num_bytes_in_block_received] = received_char;
switch (parse_state) {
case RECV_HDR:
switch (num_bytes_in_block_received) {
case 0:
good_byte = (received_char == 0xff);
break;
case 1:
good_byte = (received_char == 0x47);
break;
case 2:
good_byte = true;
break;
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);
break;
case 4: {
switch (data_id) {
case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID: {
good_byte = (received_char == 0x10);
break;
}
case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID:
case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID:
good_byte = true;
break;
case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID: {
good_byte = (received_char == 0x16);
break;
}
}
if (good_byte) {
parse_state = RECV_DGRAM;
}
break;
}
}
if (good_byte) {
// correct header byte
num_bytes_in_block_received++;
} else {
// ...or incorrect
parse_state = RECV_HDR;
num_bytes_in_block_received = 0;
}
break;

case RECV_DGRAM:
num_bytes_in_block_received++;
if (num_bytes_in_block_received >= 7 + input_buffer[4]) {
// parse dgram
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();
}
}
}
// and repeat
parse_state = RECV_HDR;
num_bytes_in_block_received = 0;
}
break;
}
}
}

Loading

0 comments on commit 44d84fc

Please sign in to comment.