Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Dr.-Ing. Amilcar Do Carmo Lucas 2017-11-01 12:27:20 +01:00 committed by Randy Mackay
parent 29078824fc
commit 54b3936a46
2 changed files with 49 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
View File

@ -24,6 +24,7 @@
#define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID 0x0001 #define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID 0x0001
#define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID 0x0002 #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_POSITION_DATAGRAM_HIGHRES_ID 0x0011
#define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID 0x0012 #define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID 0x0012
@ -216,6 +217,23 @@ void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram()
order_stationary_beacons(); 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) void AP_Beacon_Marvelmind::update(void)
{ {
if (uart == nullptr || hedge == nullptr || hedge->position_buffer == nullptr) { if (uart == nullptr || hedge == nullptr || hedge->position_buffer == nullptr) {
@ -247,6 +265,7 @@ void AP_Beacon_Marvelmind::update(void)
data_id = (((uint16_t)received_char) << 8) + input_buffer[2]; data_id = (((uint16_t)received_char) << 8) + input_buffer[2];
good_byte = (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID) good_byte = (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID)
|| (data_id == AP_BEACON_MARVELMIND_POSITIONS_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_POSITION_DATAGRAM_HIGHRES_ID)
|| (data_id == AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID); || (data_id == AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID);
break; break;
@ -256,6 +275,7 @@ void AP_Beacon_Marvelmind::update(void)
good_byte = (received_char == 0x10); good_byte = (received_char == 0x10);
break; break;
} }
case AP_BEACON_MARVELMIND_DISTANCES_DATAGRAM_ID:
case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID: case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID:
case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID: case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID:
good_byte = true; good_byte = true;
@ -293,7 +313,7 @@ void AP_Beacon_Marvelmind::update(void)
// add to position_buffer // add to position_buffer
process_position_datagram(cur_position); process_position_datagram(cur_position);
vehicle_position_initialized = true; vehicle_position_initialized = true;
set_stationary_beacons_positions_and_distances(); set_stationary_beacons_positions();
break; break;
} }
@ -301,15 +321,20 @@ void AP_Beacon_Marvelmind::update(void)
{ {
process_beacons_positions_datagram(); process_beacons_positions_datagram();
beacon_position_initialized = true; beacon_position_initialized = true;
set_stationary_beacons_positions_and_distances(); set_stationary_beacons_positions();
break; break;
} }
case AP_BEACON_MARVELMIND_DISTANCES_DATAGRAM_ID:
{
process_beacons_distances_datagram();
}
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(cur_position);
vehicle_position_initialized = true; vehicle_position_initialized = true;
set_stationary_beacons_positions_and_distances(); set_stationary_beacons_positions();
break; break;
} }
@ -317,7 +342,7 @@ void AP_Beacon_Marvelmind::update(void)
{ {
process_beacons_positions_highres_datagram(); process_beacons_positions_highres_datagram();
beacon_position_initialized = true; beacon_position_initialized = true;
set_stationary_beacons_positions_and_distances(); set_stationary_beacons_positions();
break; break;
} }
} }
@ -367,7 +392,7 @@ bool AP_Beacon_Marvelmind::healthy()
return ((AP_HAL::millis() - last_update_ms) < AP_BEACON_TIMEOUT_MS); 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 (vehicle_position_initialized && beacon_position_initialized) {
if (hedge->_have_new_values) { if (hedge->_have_new_values) {
@ -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 -hedge->positions_beacons.beacons[i].z / 1000.0f); //Transform Marvelmind ENU to Ardupilot NED
set_beacon_position(i, beacon_position_NED__m[i]); 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->positions_beacons.updated = false;
hedge->_have_new_values = false; hedge->_have_new_values = false;

26
libraries/AP_Beacon/AP_Beacon_Marvelmind.h
View File

@ -18,15 +18,12 @@
April 2017 April 2017
*/ */
#ifndef AP_BEACON_MARVELMIND_H_
#define AP_BEACON_MARVELMIND_H_
#pragma once #pragma once
#define AP_BEACON_MARVELMIND_BUF_SIZE 255
#include "AP_Beacon_Backend.h" #include "AP_Beacon_Backend.h"
#define AP_BEACON_MARVELMIND_BUF_SIZE 255
class AP_Beacon_Marvelmind : public AP_Beacon_Backend class AP_Beacon_Marvelmind : public AP_Beacon_Backend
{ {
public: public:
@ -66,6 +63,20 @@ private:
bool updated; 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 struct MarvelmindHedge
{ {
MarvelmindHedge(); MarvelmindHedge();
@ -87,6 +98,7 @@ private:
} parse_state; // current state of receive data } parse_state; // current state of receive data
MarvelmindHedge *hedge; MarvelmindHedge *hedge;
RawBeaconDistances raw_beacon_distances;
PositionValue cur_position; 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;
@ -99,7 +111,8 @@ private:
void process_beacons_positions_highres_datagram(); void process_beacons_positions_highres_datagram();
void process_position_highres_datagram(PositionValue &p); void process_position_highres_datagram(PositionValue &p);
void process_position_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(); void order_stationary_beacons();
// Variables for Ardupilot // Variables for Ardupilot
@ -115,4 +128,3 @@ private:
bool beacon_position_initialized; bool beacon_position_initialized;
}; };
#endif /* AP_BEACON_MARVELMIND_H_ */