diff --git a/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp b/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp index 32f9aba99d..8a7e0f28b3 100644 --- a/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp +++ b/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp @@ -28,8 +28,6 @@ #define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID 0x0011 #define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID 0x0012 -#define MAX_BUFFERED_POSITIONS 3 - extern const AP_HAL::HAL& hal; 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); if (uart != nullptr) { uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Beacon, 0)); - hedge = new MarvelmindHedge(); last_update_ms = 0; - if (hedge && hedge->position_buffer != nullptr) { - parse_state = RECV_HDR; // current state of receive data - num_bytes_in_block_received = 0; // bytes received - data_id = 0; - } else { - hal.console->printf("MarvelMind: MarvelmindHedge failed\n"); - } + parse_state = RECV_HDR; // current state of receive data + num_bytes_in_block_received = 0; // bytes received + data_id = 0; + hedge._have_new_values = false; + hedge.positions_beacons.num_beacons = 0; + 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; } -uint8_t AP_Beacon_Marvelmind::mark_position_ready() +void AP_Beacon_Marvelmind::process_position_datagram() { - 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 >= 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); + hedge.cur_position.address = input_buffer[16]; + hedge.cur_position.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; // centimeters -> millimeters + hedge.cur_position.x__mm = vx * 10; // centimeters -> millimeters 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); - 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]; + hedge.cur_position.z__mm = vz * 10; // centimeters -> millimeters + hedge.cur_position.high_resolution = false; + hedge._have_new_values = true; } -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->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]; + hedge.cur_position.address = input_buffer[22]; + hedge.cur_position.timestamp = input_buffer[5] + | (((uint32_t) input_buffer[6]) << 8) + | (((uint32_t) input_buffer[7]) << 16) + | (((uint32_t) input_buffer[8]) << 24); + hedge.cur_position.x__mm = input_buffer[9] | (((uint32_t) input_buffer[10]) << 8) + | (((uint32_t) input_buffer[11]) << 16) + | (((uint32_t) input_buffer[12]) << 24); + hedge.cur_position.y__mm = input_buffer[13] | (((uint32_t) input_buffer[14]) << 8) + | (((uint32_t) input_buffer[15]) << 16) + | (((uint32_t) input_buffer[16]) << 24); + hedge.cur_position.z__mm = input_buffer[17] | (((uint32_t) input_buffer[18]) << 8) + | (((uint32_t) input_buffer[19]) << 16) + | (((uint32_t) input_buffer[20]) << 24); + hedge.cur_position.high_resolution = true; + hedge._have_new_values = true; } AP_Beacon_Marvelmind::StationaryBeaconPosition* AP_Beacon_Marvelmind::get_or_alloc_beacon(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) { - return &hedge->positions_beacons.beacons[i]; - } + const uint8_t n_used = hedge.positions_beacons.num_beacons; + for (uint8_t i = 0; i < n_used; i++) { + if (hedge.positions_beacons.beacons[i].address == address) { + return &hedge.positions_beacons.beacons[i]; } } if (n_used >= AP_BEACON_MAX_BEACONS) { return nullptr; } - hedge->positions_beacons.num_beacons = (n_used + 1); - return &hedge->positions_beacons.beacons[n_used]; + hedge.positions_beacons.num_beacons = (n_used + 1); + return &hedge.positions_beacons.beacons[n_used]; } 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->z = z * 10; // centimeters -> millimeters stationary_beacon->high_resolution = false; - hedge->positions_beacons.updated = true; + hedge.positions_beacons.updated = true; } } order_stationary_beacons(); @@ -211,7 +181,7 @@ void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram() stationary_beacon->y = y; // millimeters stationary_beacon->z = z; // millimeters stationary_beacon->high_resolution = true; - hedge->positions_beacons.updated = true; + hedge.positions_beacons.updated = true; } } 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) { - if (uart == nullptr || hedge == nullptr || hedge->position_buffer == nullptr) { + if (uart == nullptr) { return; } // read any available characters @@ -331,7 +301,7 @@ void AP_Beacon_Marvelmind::update(void) case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID: { // add to position_buffer - process_position_datagram(cur_position); + process_position_datagram(); vehicle_position_initialized = true; set_stationary_beacons_positions(); break; @@ -353,7 +323,7 @@ void AP_Beacon_Marvelmind::update(void) case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID: { - process_position_highres_datagram(cur_position); + process_position_highres_datagram(); vehicle_position_initialized = true; set_stationary_beacons_positions(); 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() { // 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() { if (vehicle_position_initialized && beacon_position_initialized) { - if (hedge->_have_new_values) { - vehicle_position_NED__m = Vector3f(cur_position.y * 0.001f, - cur_position.x * 0.001f, - -cur_position.z * 0.001f); //Transform Marvelmind ENU to Ardupilot NED + if (hedge._have_new_values) { + vehicle_position_NED__m = Vector3f(hedge.cur_position.y__mm * 0.001f, + hedge.cur_position.x__mm * 0.001f, + -hedge.cur_position.z__mm * 0.001f); //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(); + 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) { - if (hedge->positions_beacons.updated) { - beacon_position_NED__m[i] = Vector3f(hedge->positions_beacons.beacons[i].y * 0.001f, - hedge->positions_beacons.beacons[i].x * 0.001f, - -hedge->positions_beacons.beacons[i].z * 0.001f); //Transform Marvelmind ENU to Ardupilot NED + hedge._have_new_values = false; + 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__mm * 0.001f, + 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]); } } - hedge->positions_beacons.updated = false; - hedge->_have_new_values = false; + hedge.positions_beacons.updated = false; + + } } } void AP_Beacon_Marvelmind::order_stationary_beacons() { - if (hedge->positions_beacons.updated) { + if (hedge.positions_beacons.updated) { bool swapped = false; - uint8_t j = hedge->positions_beacons.num_beacons; + uint8_t j = hedge.positions_beacons.num_beacons; do { swapped = false; StationaryBeaconPosition beacon_to_swap; - for(uint8_t i = 1; i < j; i++) { - if (hedge->positions_beacons.beacons[i-1].address > hedge->positions_beacons.beacons[i].address) { - beacon_to_swap = hedge->positions_beacons.beacons[i]; - hedge->positions_beacons.beacons[i] = hedge->positions_beacons.beacons[i-1]; - hedge->positions_beacons.beacons[i-1] = beacon_to_swap; + for (uint8_t i = 1; i < j; i++) { + if (hedge.positions_beacons.beacons[i-1].address > hedge.positions_beacons.beacons[i].address) { + beacon_to_swap = hedge.positions_beacons.beacons[i]; + hedge.positions_beacons.beacons[i] = hedge.positions_beacons.beacons[i-1]; + hedge.positions_beacons.beacons[i-1] = beacon_to_swap; swapped = true; } } diff --git a/libraries/AP_Beacon/AP_Beacon_Marvelmind.h b/libraries/AP_Beacon/AP_Beacon_Marvelmind.h index 0d67435a2c..6d7534b8b6 100644 --- a/libraries/AP_Beacon/AP_Beacon_Marvelmind.h +++ b/libraries/AP_Beacon/AP_Beacon_Marvelmind.h @@ -62,17 +62,10 @@ private: bool updated; }; - 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; - - uint8_t _last_values_count; - uint8_t _last_values_next; + PositionValue cur_position; bool _have_new_values; }; @@ -81,19 +74,17 @@ private: RECV_DGRAM } parse_state; // current state of receive data - MarvelmindHedge *hedge; - PositionValue cur_position; + MarvelmindHedge hedge; uint8_t input_buffer[AP_BEACON_MARVELMIND_BUF_SIZE]; uint16_t num_bytes_in_block_received; uint16_t data_id; uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len); StationaryBeaconPosition* get_or_alloc_beacon(uint8_t address); - uint8_t mark_position_ready(); 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 process_position_highres_datagram(); + void process_position_datagram(); void process_beacons_distances_datagram(); void set_stationary_beacons_positions(); void order_stationary_beacons();