Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Beacon: Added Marvelmind Beacon class

Browse Source 
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.
This commit is contained in:
karthik.desai 2017-04-03 22:53:50 +02:00 committed by Randy Mackay
parent 47ff9ddeec
commit 44d84fca15
4 changed files with 447 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_Beacon/AP_Beacon.cpp
View File

@ -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;
@ -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),
@ -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) {

1
libraries/AP_Beacon/AP_Beacon.h
View File

@ -36,6 +36,7 @@ public:
enum AP_BeaconType {
AP_BeaconType_None = 0,
AP_BeaconType_Pozyx = 1,
AP_BeaconType_Marvelmind = 2,
AP_BeaconType_SITL = 10
};

338
libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp Normal file
View File

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

104
libraries/AP_Beacon/AP_Beacon_Marvelmind.h Normal file
View File

@ -0,0 +1,104 @@
/*
* AP_Beacon_Marvelmind.h
*
* Created on: 21.03.2017
*/
#ifndef AP_BEACON_MARVELMIND_H_
#define AP_BEACON_MARVELMIND_H_
#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
#define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID 0x0012
#define AP_BEACON_MARVELMIND_BUF_SIZE 255
#include "AP_Beacon_Backend.h"
class AP_Beacon_Marvelmind : public AP_Beacon_Backend
{
public:
struct PositionValue
{
uint8_t address;
uint32_t timestamp;
int32_t x, y, z;// coordinates in millimeters
bool high_resolution;
bool ready;
bool processed;
};
struct StationaryBeaconPosition
{
uint8_t address;
int32_t x, y, z;// coordinates in millimeters
bool high_resolution;
};
struct StationaryBeaconsPositions
{
uint8_t num_beacons;
struct StationaryBeaconPosition beacons[AP_BEACON_MARVELMIND_MAX_STATIONARY_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;
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
// 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;
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);
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 create_marvelmind_hedge();
void start_marvelmind_hedge();
bool get_position_from_marvelmind_hedge(struct PositionValue *position);
void stop_marvelmind_hedge();
AP_HAL::UARTDriver *uart = nullptr;
uint32_t last_update_ms = 0;
};
#endif /* AP_BEACON_MARVELMIND_H_ */