zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Beacon: Use marvelmind beacon raw distances on the EKF 

Add support for Marvelmind RAW distance measurements this
requires Marvelmind FW >= 5.77 and "Raw distances data" enabled
in the marvelmind GUI dashboard.
This is better than the previous workaround we had.
master
Dr.-Ing. Amilcar Do Carmo Lucas 7 years ago committed by Randy Mackay
parent
29078824fc
commit
54b3936a46
2 changed files with 49 additions and 19 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 42
      libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
  2. 26
      libraries/AP_Beacon/AP_Beacon_Marvelmind.h

42
libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
Unescape View File

@ -24,6 +24,7 @@ @@ -24,6 +24,7 @@
#define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID 0x0001
#define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID 0x0002
#define AP_BEACON_MARVELMIND_DISTANCES_DATAGRAM_ID 0x0004
#define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID 0x0011
#define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID 0x0012
@ -216,6 +217,23 @@ void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram() @@ -216,6 +217,23 @@ void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram()
order_stationary_beacons();
}
void AP_Beacon_Marvelmind::process_beacons_distances_datagram()
{
if (32 != input_buffer[4]) {
return; // incorrect size
}
raw_beacon_distances.address = input_buffer[5]; // Address of hedgehog
for (uint8_t i = 0; i < 4; i++) {
const uint8_t ofs = 6 + i * 6;
raw_beacon_distances.beacon[i].address = input_buffer[ofs];
raw_beacon_distances.beacon[i].distance = 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);
set_beacon_distance(i, raw_beacon_distances.beacon[i].distance / 1000.0f); // millimeters -> meters
}
}
void AP_Beacon_Marvelmind::update(void)
{
if (uart == nullptr || hedge == nullptr || hedge->position_buffer == nullptr) {
@ -247,6 +265,7 @@ void AP_Beacon_Marvelmind::update(void) @@ -247,6 +265,7 @@ void AP_Beacon_Marvelmind::update(void)
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_DISTANCES_DATAGRAM_ID)
|| (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID)
|| (data_id == AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID);
break;
@ -256,6 +275,7 @@ void AP_Beacon_Marvelmind::update(void) @@ -256,6 +275,7 @@ void AP_Beacon_Marvelmind::update(void)
good_byte = (received_char == 0x10);
break;
}
case AP_BEACON_MARVELMIND_DISTANCES_DATAGRAM_ID:
case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID:
case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID:
good_byte = true;
@ -293,7 +313,7 @@ void AP_Beacon_Marvelmind::update(void) @@ -293,7 +313,7 @@ void AP_Beacon_Marvelmind::update(void)
// add to position_buffer
process_position_datagram(cur_position);
vehicle_position_initialized = true;
set_stationary_beacons_positions_and_distances();
set_stationary_beacons_positions();
break;
}
@ -301,15 +321,20 @@ void AP_Beacon_Marvelmind::update(void) @@ -301,15 +321,20 @@ void AP_Beacon_Marvelmind::update(void)
{
process_beacons_positions_datagram();
beacon_position_initialized = true;
set_stationary_beacons_positions_and_distances();
set_stationary_beacons_positions();
break;
}
case AP_BEACON_MARVELMIND_DISTANCES_DATAGRAM_ID:
{
process_beacons_distances_datagram();
}
case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID:
{
process_position_highres_datagram(cur_position);
vehicle_position_initialized = true;
set_stationary_beacons_positions_and_distances();
set_stationary_beacons_positions();
break;
}
@ -317,7 +342,7 @@ void AP_Beacon_Marvelmind::update(void) @@ -317,7 +342,7 @@ void AP_Beacon_Marvelmind::update(void)
{
process_beacons_positions_highres_datagram();
beacon_position_initialized = true;
set_stationary_beacons_positions_and_distances();
set_stationary_beacons_positions();
break;
}
}
@ -367,7 +392,7 @@ bool AP_Beacon_Marvelmind::healthy() @@ -367,7 +392,7 @@ bool AP_Beacon_Marvelmind::healthy()
return ((AP_HAL::millis() - last_update_ms) < AP_BEACON_TIMEOUT_MS);
}
void AP_Beacon_Marvelmind::set_stationary_beacons_positions_and_distances()
void AP_Beacon_Marvelmind::set_stationary_beacons_positions()
{
if (vehicle_position_initialized && beacon_position_initialized) {
if (hedge->_have_new_values) {
@ -385,13 +410,6 @@ void AP_Beacon_Marvelmind::set_stationary_beacons_positions_and_distances() @@ -385,13 +410,6 @@ void AP_Beacon_Marvelmind::set_stationary_beacons_positions_and_distances()
-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;

26
libraries/AP_Beacon/AP_Beacon_Marvelmind.h
Unescape View File

@ -18,15 +18,12 @@ @@ -18,15 +18,12 @@
April 2017
*/
#ifndef AP_BEACON_MARVELMIND_H_
#define AP_BEACON_MARVELMIND_H_
#pragma once
#define AP_BEACON_MARVELMIND_BUF_SIZE 255
#include "AP_Beacon_Backend.h"
#define AP_BEACON_MARVELMIND_BUF_SIZE 255
class AP_Beacon_Marvelmind : public AP_Beacon_Backend
{
public:
@ -66,6 +63,20 @@ private: @@ -66,6 +63,20 @@ private:
bool updated;
};
struct DistanceToBeacon
{
uint8_t address; ///< Address of beacon (0 if item not filled)
uint32_t distance; ///< Distance to the beacon in millimeters
uint8_t reserved; ///< reserved (0)
};
struct RawBeaconDistances
{
uint8_t address; ///< Address of hedgehog
DistanceToBeacon beacon[4]; ///< Distance to beacon
uint8_t reserved[7]; ///< reserved
};
struct MarvelmindHedge
{
MarvelmindHedge();
@ -87,6 +98,7 @@ private: @@ -87,6 +98,7 @@ private:
} parse_state; // current state of receive data
MarvelmindHedge *hedge;
RawBeaconDistances raw_beacon_distances;
PositionValue cur_position;
uint8_t input_buffer[AP_BEACON_MARVELMIND_BUF_SIZE];
uint16_t num_bytes_in_block_received;
@ -99,7 +111,8 @@ private: @@ -99,7 +111,8 @@ private:
void process_beacons_positions_highres_datagram();
void process_position_highres_datagram(PositionValue &p);
void process_position_datagram(PositionValue &p);
void set_stationary_beacons_positions_and_distances();
void process_beacons_distances_datagram();
void set_stationary_beacons_positions();
void order_stationary_beacons();
// Variables for Ardupilot
@ -115,4 +128,3 @@ private: @@ -115,4 +128,3 @@ private:
bool beacon_position_initialized;
};
#endif /* AP_BEACON_MARVELMIND_H_ */

Write Preview
Loading…
Cancel
Save