Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Beacon_Marvelmind: Simplify the Marvelmind structure.

Browse Source 
This removes pointless pointer inits and removes the buffer.
The new incoming data is now injected as and when they are received by the driver.
This commit is contained in:
Karthik Desai 2018-05-18 15:25:52 +02:00 committed by Francisco Ferreira
parent 8957111f26
commit 358b3d222b
2 changed files with 75 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

196
libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
View File

@ -28,8 +28,6 @@
#define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID 0x0011 #define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID 0x0011
#define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID 0x0012 #define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID 0x0012
#define MAX_BUFFERED_POSITIONS 3
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_Beacon_Marvelmind::AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager &serial_manager) : AP_Beacon_Marvelmind::AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager &serial_manager) :
@ -38,15 +36,14 @@ AP_Beacon_Marvelmind::AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Beacon, 0); uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Beacon, 0);
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();
last_update_ms = 0; last_update_ms = 0;
if (hedge && hedge->position_buffer != nullptr) { parse_state = RECV_HDR; // current state of receive data
parse_state = RECV_HDR; // current state of receive data num_bytes_in_block_received = 0; // bytes received
num_bytes_in_block_received = 0; // bytes received data_id = 0;
data_id = 0; hedge._have_new_values = false;
} else { hedge.positions_beacons.num_beacons = 0;
hal.console->printf("MarvelMind: MarvelmindHedge failed\n"); hedge.positions_beacons.updated = false;
}
} }
} }
@ -74,83 +71,56 @@ uint16_t AP_Beacon_Marvelmind::calc_crc_modbus(uint8_t *buf, uint16_t len)
return crc; return crc;
} }
uint8_t AP_Beacon_Marvelmind::mark_position_ready() void AP_Beacon_Marvelmind::process_position_datagram()
{ {
uint8_t ind = hedge->_last_values_next; hedge.cur_position.address = input_buffer[16];
const uint8_t ind_cur = ind; hedge.cur_position.timestamp = input_buffer[5]
hedge->position_buffer[ind].ready = true; | (((uint32_t) input_buffer[6]) << 8)
hedge->position_buffer[ind].processed = false; | (((uint32_t) input_buffer[7]) << 16)
ind++; | (((uint32_t) input_buffer[8]) << 24);
if (ind >= MAX_BUFFERED_POSITIONS) {
ind = 0;
}
if (hedge->_last_values_count < 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(AP_Beacon_Marvelmind::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); const int16_t vx = input_buffer[9] | (((uint16_t) input_buffer[10]) << 8);
hedge->position_buffer[ind].x = vx * 10; // centimeters -> millimeters hedge.cur_position.x__mm = 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; // centimeters -> millimeters hedge.cur_position.y__mm = 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; // centimeters -> millimeters hedge.cur_position.z__mm = vz * 10; // centimeters -> millimeters
hedge->position_buffer[ind].high_resolution = false; hedge.cur_position.high_resolution = false;
ind = mark_position_ready(); hedge._have_new_values = true;
p = hedge->position_buffer[ind];
} }
void AP_Beacon_Marvelmind::process_position_highres_datagram(AP_Beacon_Marvelmind::PositionValue &p) void AP_Beacon_Marvelmind::process_position_highres_datagram()
{ {
uint8_t ind = hedge->_last_values_next; hedge.cur_position.address = input_buffer[22];
hedge->position_buffer[ind].address = input_buffer[22]; hedge.cur_position.timestamp = input_buffer[5]
hedge->position_buffer[ind].timestamp = input_buffer[5] | (((uint32_t) input_buffer[6]) << 8)
| (((uint32_t) input_buffer[6]) << 8) | (((uint32_t) input_buffer[7]) << 16)
| (((uint32_t) input_buffer[7]) << 16) | (((uint32_t) input_buffer[8]) << 24);
| (((uint32_t) input_buffer[8]) << 24); hedge.cur_position.x__mm = input_buffer[9] | (((uint32_t) input_buffer[10]) << 8)
const int32_t vx = input_buffer[9] | (((uint32_t) input_buffer[10]) << 8) | (((uint32_t) input_buffer[11]) << 16)
| (((uint32_t) input_buffer[11]) << 16) | (((uint32_t) input_buffer[12]) << 24);
| (((uint32_t) input_buffer[12]) << 24); hedge.cur_position.y__mm = input_buffer[13] | (((uint32_t) input_buffer[14]) << 8)
hedge->position_buffer[ind].x = vx; | (((uint32_t) input_buffer[15]) << 16)
const int32_t vy = input_buffer[13] | (((uint32_t) input_buffer[14]) << 8) | (((uint32_t) input_buffer[16]) << 24);
| (((uint32_t) input_buffer[15]) << 16) hedge.cur_position.z__mm = input_buffer[17] | (((uint32_t) input_buffer[18]) << 8)
| (((uint32_t) input_buffer[16]) << 24); | (((uint32_t) input_buffer[19]) << 16)
hedge->position_buffer[ind].y = vy; | (((uint32_t) input_buffer[20]) << 24);
const int32_t vz = input_buffer[17] | (((uint32_t) input_buffer[18]) << 8) hedge.cur_position.high_resolution = true;
| (((uint32_t) input_buffer[19]) << 16) hedge._have_new_values = true;
| (((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];
} }
AP_Beacon_Marvelmind::StationaryBeaconPosition* AP_Beacon_Marvelmind::get_or_alloc_beacon(uint8_t address) AP_Beacon_Marvelmind::StationaryBeaconPosition* AP_Beacon_Marvelmind::get_or_alloc_beacon(uint8_t address)
{ {
const uint8_t n_used = hedge->positions_beacons.num_beacons; const uint8_t n_used = hedge.positions_beacons.num_beacons;
if (n_used != 0) { for (uint8_t i = 0; i < n_used; i++) {
for (uint8_t i = 0; i < n_used; i++) { if (hedge.positions_beacons.beacons[i].address == address) {
if (hedge->positions_beacons.beacons[i].address == address) { return &hedge.positions_beacons.beacons[i];
return &hedge->positions_beacons.beacons[i];
}
} }
} }
if (n_used >= AP_BEACON_MAX_BEACONS) { if (n_used >= AP_BEACON_MAX_BEACONS) {
return nullptr; return nullptr;
} }
hedge->positions_beacons.num_beacons = (n_used + 1); hedge.positions_beacons.num_beacons = (n_used + 1);
return &hedge->positions_beacons.beacons[n_used]; return &hedge.positions_beacons.beacons[n_used];
} }
void AP_Beacon_Marvelmind::process_beacons_positions_datagram() void AP_Beacon_Marvelmind::process_beacons_positions_datagram()
@ -176,7 +146,7 @@ void AP_Beacon_Marvelmind::process_beacons_positions_datagram()
stationary_beacon->y = y * 10; // centimeters -> millimeters stationary_beacon->y = y * 10; // centimeters -> millimeters
stationary_beacon->z = z * 10; // centimeters -> millimeters stationary_beacon->z = z * 10; // centimeters -> millimeters
stationary_beacon->high_resolution = false; stationary_beacon->high_resolution = false;
hedge->positions_beacons.updated = true; hedge.positions_beacons.updated = true;
} }
} }
order_stationary_beacons(); order_stationary_beacons();
@ -211,7 +181,7 @@ void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram()
stationary_beacon->y = y; // millimeters stationary_beacon->y = y; // millimeters
stationary_beacon->z = z; // millimeters stationary_beacon->z = z; // millimeters
stationary_beacon->high_resolution = true; stationary_beacon->high_resolution = true;
hedge->positions_beacons.updated = true; hedge.positions_beacons.updated = true;
} }
} }
order_stationary_beacons(); order_stationary_beacons();
@ -256,7 +226,7 @@ int8_t AP_Beacon_Marvelmind::find_beacon_instance(uint8_t address) const
void AP_Beacon_Marvelmind::update(void) void AP_Beacon_Marvelmind::update(void)
{ {
if (uart == nullptr || hedge == nullptr || hedge->position_buffer == nullptr) { if (uart == nullptr) {
return; return;
} }
// read any available characters // read any available characters
@ -331,7 +301,7 @@ void AP_Beacon_Marvelmind::update(void)
case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID: case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID:
{ {
// add to position_buffer // add to position_buffer
process_position_datagram(cur_position); process_position_datagram();
vehicle_position_initialized = true; vehicle_position_initialized = true;
set_stationary_beacons_positions(); set_stationary_beacons_positions();
break; break;
@ -353,7 +323,7 @@ void AP_Beacon_Marvelmind::update(void)
case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID: case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID:
{ {
process_position_highres_datagram(cur_position); process_position_highres_datagram();
vehicle_position_initialized = true; vehicle_position_initialized = true;
set_stationary_beacons_positions(); set_stationary_beacons_positions();
break; break;
@ -377,36 +347,6 @@ void AP_Beacon_Marvelmind::update(void)
} }
} }
//////////////////////////////////////////////////////////////////////////////
// Create and initialize MarvelmindHedge structure
//////////////////////////////////////////////////////////////////////////////
AP_Beacon_Marvelmind::MarvelmindHedge::MarvelmindHedge() :
position_buffer{nullptr},
positions_beacons{},
pause{false},
receive_data_callback{nullptr},
_last_values_count{0},
_last_values_next{0},
_have_new_values{false}
{
position_buffer = new PositionValue[MAX_BUFFERED_POSITIONS];
if (position_buffer == nullptr) {
hal.console->printf("MarvelMind: Not enough memory\n");
return;
}
for (uint8_t i = 0; i < MAX_BUFFERED_POSITIONS; i++) {
position_buffer[i].ready = false;
position_buffer[i].processed = false;
}
positions_beacons.num_beacons = 0;
positions_beacons.updated = false;
}
AP_Beacon_Marvelmind::MarvelmindHedge::~MarvelmindHedge() {
if (position_buffer != nullptr)
delete[] position_buffer;
}
bool AP_Beacon_Marvelmind::healthy() bool AP_Beacon_Marvelmind::healthy()
{ {
// healthy if we have parsed a message within the past 300ms // healthy if we have parsed a message within the past 300ms
@ -416,41 +356,47 @@ bool AP_Beacon_Marvelmind::healthy()
void AP_Beacon_Marvelmind::set_stationary_beacons_positions() void AP_Beacon_Marvelmind::set_stationary_beacons_positions()
{ {
if (vehicle_position_initialized && beacon_position_initialized) { if (vehicle_position_initialized && beacon_position_initialized) {
if (hedge->_have_new_values) { if (hedge._have_new_values) {
vehicle_position_NED__m = Vector3f(cur_position.y * 0.001f, vehicle_position_NED__m = Vector3f(hedge.cur_position.y__mm * 0.001f,
cur_position.x * 0.001f, hedge.cur_position.x__mm * 0.001f,
-cur_position.z * 0.001f); //Transform Marvelmind ENU to Ardupilot NED -hedge.cur_position.z__mm * 0.001f); //Transform Marvelmind ENU to Ardupilot NED
//TODO: Calculate Accuracy of the received signal. Marvelmind *advertises* +/- 2cms //TODO: Calculate Accuracy of the received signal. Marvelmind *advertises* +/- 2cms
set_vehicle_position(vehicle_position_NED__m, 0.02f); set_vehicle_position(vehicle_position_NED__m, 0.02f);
last_update_ms = AP_HAL::millis(); Debug(2,
"Hedge is at N%f, E%f, D%f",
vehicle_position_NED__m[0],
vehicle_position_NED__m[1],
vehicle_position_NED__m[2]);
} }
for (uint8_t i=0; i < hedge->positions_beacons.num_beacons; ++i) { hedge._have_new_values = false;
if (hedge->positions_beacons.updated) { for (uint8_t i = 0; i < hedge.positions_beacons.num_beacons; ++i) {
beacon_position_NED__m[i] = Vector3f(hedge->positions_beacons.beacons[i].y * 0.001f, if (hedge.positions_beacons.updated) {
hedge->positions_beacons.beacons[i].x * 0.001f, beacon_position_NED__m[i] = Vector3f(hedge.positions_beacons.beacons[i].y__mm * 0.001f,
-hedge->positions_beacons.beacons[i].z * 0.001f); //Transform Marvelmind ENU to Ardupilot NED hedge.positions_beacons.beacons[i].x__mm * 0.001f,
-hedge.positions_beacons.beacons[i].z__mm * 0.001f); //Transform Marvelmind ENU to Ardupilot NED
set_beacon_position(i, beacon_position_NED__m[i]); set_beacon_position(i, beacon_position_NED__m[i]);
} }
} }
hedge->positions_beacons.updated = false; hedge.positions_beacons.updated = false;
hedge->_have_new_values = false;
}
} }
} }
void AP_Beacon_Marvelmind::order_stationary_beacons() void AP_Beacon_Marvelmind::order_stationary_beacons()
{ {
if (hedge->positions_beacons.updated) { if (hedge.positions_beacons.updated) {
bool swapped = false; bool swapped = false;
uint8_t j = hedge->positions_beacons.num_beacons; uint8_t j = hedge.positions_beacons.num_beacons;
do do
{ {
swapped = false; swapped = false;
StationaryBeaconPosition beacon_to_swap; StationaryBeaconPosition beacon_to_swap;
for(uint8_t i = 1; i < j; i++) { for (uint8_t i = 1; i < j; i++) {
if (hedge->positions_beacons.beacons[i-1].address > hedge->positions_beacons.beacons[i].address) { if (hedge.positions_beacons.beacons[i-1].address > hedge.positions_beacons.beacons[i].address) {
beacon_to_swap = hedge->positions_beacons.beacons[i]; beacon_to_swap = hedge.positions_beacons.beacons[i];
hedge->positions_beacons.beacons[i] = hedge->positions_beacons.beacons[i-1]; hedge.positions_beacons.beacons[i] = hedge.positions_beacons.beacons[i-1];
hedge->positions_beacons.beacons[i-1] = beacon_to_swap; hedge.positions_beacons.beacons[i-1] = beacon_to_swap;
swapped = true; swapped = true;
} }
} }

17
libraries/AP_Beacon/AP_Beacon_Marvelmind.h
View File

@ -62,17 +62,10 @@ private:
bool updated; bool updated;
}; };
struct MarvelmindHedge struct MarvelmindHedge
{ {
MarvelmindHedge();
~MarvelmindHedge();
uint8_t max_buffered_positions; // maximum count of measurements of coordinates stored in buffer, default: 3
PositionValue * position_buffer; // buffer of measurements
StationaryBeaconsPositions positions_beacons; StationaryBeaconsPositions positions_beacons;
PositionValue cur_position;
uint8_t _last_values_count;
uint8_t _last_values_next;
bool _have_new_values; bool _have_new_values;
}; };
@ -81,19 +74,17 @@ private:
RECV_DGRAM RECV_DGRAM
} parse_state; // current state of receive data } parse_state; // current state of receive data
MarvelmindHedge *hedge; MarvelmindHedge hedge;
PositionValue cur_position;
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;
uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len); uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len);
StationaryBeaconPosition* get_or_alloc_beacon(uint8_t address); StationaryBeaconPosition* get_or_alloc_beacon(uint8_t address);
uint8_t mark_position_ready();
void process_beacons_positions_datagram(); void process_beacons_positions_datagram();
void process_beacons_positions_highres_datagram(); void process_beacons_positions_highres_datagram();
void process_position_highres_datagram(PositionValue &p); void process_position_highres_datagram();
void process_position_datagram(PositionValue &p); void process_position_datagram();
void process_beacons_distances_datagram(); void process_beacons_distances_datagram();
void set_stationary_beacons_positions(); void set_stationary_beacons_positions();
void order_stationary_beacons(); void order_stationary_beacons();