AP_ExternalAHRS: Create common lib for MicroStrain
* Move common parsing to MicroStrain_common * Add propogation of parsed packet type back to the driver * Rename the driver to add series-5 specifier * Unlocks adding 7-series driver with less code duplication * Add missing includes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman
Andrew Tridgell
parent
4d05eda6f0
commit
d8eee6f41a
@ -23,7 +23,7 @@
|
|||||||
#include "AP_ExternalAHRS.h"
|
#include "AP_ExternalAHRS.h"
|
||||||
#include "AP_ExternalAHRS_backend.h"
|
#include "AP_ExternalAHRS_backend.h"
|
||||||
#include "AP_ExternalAHRS_VectorNav.h"
|
#include "AP_ExternalAHRS_VectorNav.h"
|
||||||
#include "AP_ExternalAHRS_MicroStrain.h"
|
#include "AP_ExternalAHRS_MicroStrain5.h"
|
||||||
|
|
||||||
#include <GCS_MAVLink/GCS.h>
|
#include <GCS_MAVLink/GCS.h>
|
||||||
|
|
||||||
@ -98,9 +98,9 @@ void AP_ExternalAHRS::init(void)
|
|||||||
backend = new AP_ExternalAHRS_VectorNav(this, state);
|
backend = new AP_ExternalAHRS_VectorNav(this, state);
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
#if AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED
|
#if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
|
||||||
case DevType::MicroStrain:
|
case DevType::MicroStrain5:
|
||||||
backend = new AP_ExternalAHRS_MicroStrain(this, state);
|
backend = new AP_ExternalAHRS_MicroStrain5(this, state);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -46,8 +46,8 @@ public:
|
|||||||
#if AP_EXTERNAL_AHRS_VECTORNAV_ENABLED
|
#if AP_EXTERNAL_AHRS_VECTORNAV_ENABLED
|
||||||
VecNav = 1,
|
VecNav = 1,
|
||||||
#endif
|
#endif
|
||||||
#if AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED
|
#if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
|
||||||
MicroStrain = 2,
|
MicroStrain5 = 2,
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -1,578 +0,0 @@
|
|||||||
/*
|
|
||||||
This program is free software: you can redistribute it and/or modify
|
|
||||||
it under the terms of the GNU General Public License as published by
|
|
||||||
the Free Software Foundation, either version 3 of the License, or
|
|
||||||
(at your option) any later version.
|
|
||||||
This program is distributed in the hope that it will be useful,
|
|
||||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
GNU General Public License for more details.
|
|
||||||
You should have received a copy of the GNU General Public License
|
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
||||||
*/
|
|
||||||
/*
|
|
||||||
suppport for MicroStrain CX5/GX5-45 serially connected AHRS Systems
|
|
||||||
*/
|
|
||||||
|
|
||||||
#define ALLOW_DOUBLE_MATH_FUNCTIONS
|
|
||||||
|
|
||||||
#include "AP_ExternalAHRS_config.h"
|
|
||||||
|
|
||||||
#if AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED
|
|
||||||
|
|
||||||
#include "AP_ExternalAHRS_MicroStrain.h"
|
|
||||||
#include <AP_Baro/AP_Baro.h>
|
|
||||||
#include <AP_Compass/AP_Compass.h>
|
|
||||||
#include <AP_GPS/AP_GPS.h>
|
|
||||||
#include <AP_HAL/utility/sparse-endian.h>
|
|
||||||
#include <AP_InertialSensor/AP_InertialSensor.h>
|
|
||||||
#include <GCS_MAVLink/GCS.h>
|
|
||||||
#include <AP_Logger/AP_Logger.h>
|
|
||||||
#include <AP_HAL/utility/sparse-endian.h>
|
|
||||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
|
||||||
|
|
||||||
|
|
||||||
enum class DescriptorSet {
|
|
||||||
BaseCommand = 0x01,
|
|
||||||
DMCommand = 0x0C,
|
|
||||||
SystemCommand = 0x7F,
|
|
||||||
IMUData = 0x80,
|
|
||||||
GNSSData = 0x81,
|
|
||||||
EstimationData = 0x82
|
|
||||||
};
|
|
||||||
|
|
||||||
enum class INSPacketField {
|
|
||||||
ACCEL = 0x04,
|
|
||||||
GYRO = 0x05,
|
|
||||||
QUAT = 0x0A,
|
|
||||||
MAG = 0x06,
|
|
||||||
PRESSURE = 0x17
|
|
||||||
};
|
|
||||||
|
|
||||||
enum class GNSSPacketField {
|
|
||||||
LLH_POSITION = 0x03,
|
|
||||||
NED_VELOCITY = 0x05,
|
|
||||||
DOP_DATA = 0x07,
|
|
||||||
GPS_TIME = 0x09,
|
|
||||||
FIX_INFO = 0x0B
|
|
||||||
};
|
|
||||||
|
|
||||||
enum class GNSSFixType {
|
|
||||||
FIX_3D = 0x00,
|
|
||||||
FIX_2D = 0x01,
|
|
||||||
TIME_ONLY = 0x02,
|
|
||||||
NONE = 0x03,
|
|
||||||
INVALID = 0x04
|
|
||||||
};
|
|
||||||
|
|
||||||
enum class FilterPacketField {
|
|
||||||
FILTER_STATUS = 0x10,
|
|
||||||
GPS_TIME = 0x11,
|
|
||||||
LLH_POSITION = 0x01,
|
|
||||||
NED_VELOCITY = 0x02
|
|
||||||
};
|
|
||||||
|
|
||||||
extern const AP_HAL::HAL &hal;
|
|
||||||
|
|
||||||
AP_ExternalAHRS_MicroStrain::AP_ExternalAHRS_MicroStrain(AP_ExternalAHRS *_frontend,
|
|
||||||
AP_ExternalAHRS::state_t &_state): AP_ExternalAHRS_backend(_frontend, _state)
|
|
||||||
{
|
|
||||||
auto &sm = AP::serialmanager();
|
|
||||||
uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0);
|
|
||||||
|
|
||||||
baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
|
|
||||||
port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
|
|
||||||
|
|
||||||
if (!uart) {
|
|
||||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS no UART");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_MicroStrain::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
|
|
||||||
AP_BoardConfig::allocation_error("Failed to allocate ExternalAHRS update thread");
|
|
||||||
}
|
|
||||||
|
|
||||||
hal.scheduler->delay(5000);
|
|
||||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain ExternalAHRS initialised");
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_ExternalAHRS_MicroStrain::update_thread(void)
|
|
||||||
{
|
|
||||||
if (!port_open) {
|
|
||||||
port_open = true;
|
|
||||||
uart->begin(baudrate);
|
|
||||||
}
|
|
||||||
|
|
||||||
while (true) {
|
|
||||||
build_packet();
|
|
||||||
hal.scheduler->delay_microseconds(100);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Builds packets by looking at each individual byte, once a full packet has been read in it checks the checksum then handles the packet.
|
|
||||||
void AP_ExternalAHRS_MicroStrain::build_packet()
|
|
||||||
{
|
|
||||||
WITH_SEMAPHORE(sem);
|
|
||||||
uint32_t nbytes = MIN(uart->available(), 2048u);
|
|
||||||
while (nbytes--> 0) {
|
|
||||||
uint8_t b;
|
|
||||||
if (!uart->read(b)) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
switch (message_in.state) {
|
|
||||||
case ParseState::WaitingFor_SyncOne:
|
|
||||||
if (b == SYNC_ONE) {
|
|
||||||
message_in.packet.header[0] = b;
|
|
||||||
message_in.state = ParseState::WaitingFor_SyncTwo;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case ParseState::WaitingFor_SyncTwo:
|
|
||||||
if (b == SYNC_TWO) {
|
|
||||||
message_in.packet.header[1] = b;
|
|
||||||
message_in.state = ParseState::WaitingFor_Descriptor;
|
|
||||||
} else {
|
|
||||||
message_in.state = ParseState::WaitingFor_SyncOne;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case ParseState::WaitingFor_Descriptor:
|
|
||||||
message_in.packet.header[2] = b;
|
|
||||||
message_in.state = ParseState::WaitingFor_PayloadLength;
|
|
||||||
break;
|
|
||||||
case ParseState::WaitingFor_PayloadLength:
|
|
||||||
message_in.packet.header[3] = b;
|
|
||||||
message_in.state = ParseState::WaitingFor_Data;
|
|
||||||
message_in.index = 0;
|
|
||||||
break;
|
|
||||||
case ParseState::WaitingFor_Data:
|
|
||||||
message_in.packet.payload[message_in.index++] = b;
|
|
||||||
if (message_in.index >= message_in.packet.header[3]) {
|
|
||||||
message_in.state = ParseState::WaitingFor_Checksum;
|
|
||||||
message_in.index = 0;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case ParseState::WaitingFor_Checksum:
|
|
||||||
message_in.packet.checksum[message_in.index++] = b;
|
|
||||||
if (message_in.index >= 2) {
|
|
||||||
message_in.state = ParseState::WaitingFor_SyncOne;
|
|
||||||
message_in.index = 0;
|
|
||||||
|
|
||||||
if (valid_packet(message_in.packet)) {
|
|
||||||
handle_packet(message_in.packet);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// returns true if the fletcher checksum for the packet is valid, else false.
|
|
||||||
bool AP_ExternalAHRS_MicroStrain::valid_packet(const MicroStrain_Packet & packet) const
|
|
||||||
{
|
|
||||||
uint8_t checksum_one = 0;
|
|
||||||
uint8_t checksum_two = 0;
|
|
||||||
|
|
||||||
for (int i = 0; i < 4; i++) {
|
|
||||||
checksum_one += packet.header[i];
|
|
||||||
checksum_two += checksum_one;
|
|
||||||
}
|
|
||||||
|
|
||||||
for (int i = 0; i < packet.header[3]; i++) {
|
|
||||||
checksum_one += packet.payload[i];
|
|
||||||
checksum_two += checksum_one;
|
|
||||||
}
|
|
||||||
|
|
||||||
return packet.checksum[0] == checksum_one && packet.checksum[1] == checksum_two;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Calls the correct functions based on the packet descriptor of the packet
|
|
||||||
void AP_ExternalAHRS_MicroStrain::handle_packet(const MicroStrain_Packet& packet)
|
|
||||||
{
|
|
||||||
switch ((DescriptorSet) packet.header[2]) {
|
|
||||||
case DescriptorSet::IMUData:
|
|
||||||
handle_imu(packet);
|
|
||||||
post_imu();
|
|
||||||
break;
|
|
||||||
case DescriptorSet::GNSSData:
|
|
||||||
handle_gnss(packet);
|
|
||||||
break;
|
|
||||||
case DescriptorSet::EstimationData:
|
|
||||||
handle_filter(packet);
|
|
||||||
post_filter();
|
|
||||||
break;
|
|
||||||
case DescriptorSet::BaseCommand:
|
|
||||||
case DescriptorSet::DMCommand:
|
|
||||||
case DescriptorSet::SystemCommand:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Collects data from an imu packet into `imu_data`
|
|
||||||
void AP_ExternalAHRS_MicroStrain::handle_imu(const MicroStrain_Packet& packet)
|
|
||||||
{
|
|
||||||
last_ins_pkt = AP_HAL::millis();
|
|
||||||
|
|
||||||
// Iterate through fields of varying lengths in INS packet
|
|
||||||
for (uint8_t i = 0; i < packet.header[3]; i += packet.payload[i]) {
|
|
||||||
switch ((INSPacketField) packet.payload[i+1]) {
|
|
||||||
// Scaled Ambient Pressure
|
|
||||||
case INSPacketField::PRESSURE: {
|
|
||||||
imu_data.pressure = be32tofloat_ptr(packet.payload, i+2) * 100; // Convert millibar to pascals
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// Scaled Magnetometer Vector
|
|
||||||
case INSPacketField::MAG: {
|
|
||||||
imu_data.mag = populate_vector3f(packet.payload, i+2) * 1000; // Convert gauss to milligauss
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// Scaled Accelerometer Vector
|
|
||||||
case INSPacketField::ACCEL: {
|
|
||||||
imu_data.accel = populate_vector3f(packet.payload, i+2) * GRAVITY_MSS; // Convert g's to m/s^2
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// Scaled Gyro Vector
|
|
||||||
case INSPacketField::GYRO: {
|
|
||||||
imu_data.gyro = populate_vector3f(packet.payload, i+2);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// Quaternion
|
|
||||||
case INSPacketField::QUAT: {
|
|
||||||
imu_data.quat = populate_quaternion(packet.payload, i+2);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Posts data from an imu packet to `state` and `handle_external` methods
|
|
||||||
void AP_ExternalAHRS_MicroStrain::post_imu() const
|
|
||||||
{
|
|
||||||
{
|
|
||||||
WITH_SEMAPHORE(state.sem);
|
|
||||||
state.accel = imu_data.accel;
|
|
||||||
state.gyro = imu_data.gyro;
|
|
||||||
|
|
||||||
state.quat = imu_data.quat;
|
|
||||||
state.have_quaternion = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
{
|
|
||||||
AP_ExternalAHRS::ins_data_message_t ins {
|
|
||||||
accel: imu_data.accel,
|
|
||||||
gyro: imu_data.gyro,
|
|
||||||
temperature: -300
|
|
||||||
};
|
|
||||||
AP::ins().handle_external(ins);
|
|
||||||
}
|
|
||||||
|
|
||||||
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
|
||||||
{
|
|
||||||
AP_ExternalAHRS::mag_data_message_t mag {
|
|
||||||
field: imu_data.mag
|
|
||||||
};
|
|
||||||
AP::compass().handle_external(mag);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if AP_BARO_EXTERNALAHRS_ENABLED
|
|
||||||
{
|
|
||||||
const AP_ExternalAHRS::baro_data_message_t baro {
|
|
||||||
instance: 0,
|
|
||||||
pressure_pa: imu_data.pressure,
|
|
||||||
// setting temp to 25 effectively disables barometer temperature calibrations - these are already performed by MicroStrain
|
|
||||||
temperature: 25,
|
|
||||||
};
|
|
||||||
AP::baro().handle_external(baro);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
// Collects data from a gnss packet into `gnss_data`
|
|
||||||
void AP_ExternalAHRS_MicroStrain::handle_gnss(const MicroStrain_Packet &packet)
|
|
||||||
{
|
|
||||||
last_gps_pkt = AP_HAL::millis();
|
|
||||||
|
|
||||||
// Iterate through fields of varying lengths in GNSS packet
|
|
||||||
for (uint8_t i = 0; i < packet.header[3]; i += packet.payload[i]) {
|
|
||||||
switch ((GNSSPacketField) packet.payload[i+1]) {
|
|
||||||
// GPS Time
|
|
||||||
case GNSSPacketField::GPS_TIME: {
|
|
||||||
gnss_data.tow_ms = double_to_uint32(be64todouble_ptr(packet.payload, i+2) * 1000); // Convert seconds to ms
|
|
||||||
gnss_data.week = be16toh_ptr(&packet.payload[i+10]);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// GNSS Fix Information
|
|
||||||
case GNSSPacketField::FIX_INFO: {
|
|
||||||
switch ((GNSSFixType) packet.payload[i+2]) {
|
|
||||||
case (GNSSFixType::FIX_3D): {
|
|
||||||
gnss_data.fix_type = GPS_FIX_TYPE_3D_FIX;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (GNSSFixType::FIX_2D): {
|
|
||||||
gnss_data.fix_type = GPS_FIX_TYPE_2D_FIX;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (GNSSFixType::TIME_ONLY):
|
|
||||||
case (GNSSFixType::NONE): {
|
|
||||||
gnss_data.fix_type = GPS_FIX_TYPE_NO_FIX;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
default:
|
|
||||||
case (GNSSFixType::INVALID): {
|
|
||||||
gnss_data.fix_type = GPS_FIX_TYPE_NO_GPS;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
gnss_data.satellites = packet.payload[i+3];
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// LLH Position
|
|
||||||
case GNSSPacketField::LLH_POSITION: {
|
|
||||||
gnss_data.lat = be64todouble_ptr(packet.payload, i+2) * 1.0e7; // Decimal degrees to degrees
|
|
||||||
gnss_data.lon = be64todouble_ptr(packet.payload, i+10) * 1.0e7;
|
|
||||||
gnss_data.msl_altitude = be64todouble_ptr(packet.payload, i+26) * 1.0e2; // Meters to cm
|
|
||||||
gnss_data.horizontal_position_accuracy = be32tofloat_ptr(packet.payload, i+34);
|
|
||||||
gnss_data.vertical_position_accuracy = be32tofloat_ptr(packet.payload, i+38);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// DOP Data
|
|
||||||
case GNSSPacketField::DOP_DATA: {
|
|
||||||
gnss_data.hdop = be32tofloat_ptr(packet.payload, i+10);
|
|
||||||
gnss_data.vdop = be32tofloat_ptr(packet.payload, i+14);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// NED Velocity
|
|
||||||
case GNSSPacketField::NED_VELOCITY: {
|
|
||||||
gnss_data.ned_velocity_north = be32tofloat_ptr(packet.payload, i+2);
|
|
||||||
gnss_data.ned_velocity_east = be32tofloat_ptr(packet.payload, i+6);
|
|
||||||
gnss_data.ned_velocity_down = be32tofloat_ptr(packet.payload, i+10);
|
|
||||||
gnss_data.speed_accuracy = be32tofloat_ptr(packet.payload, i+26);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_ExternalAHRS_MicroStrain::handle_filter(const MicroStrain_Packet &packet)
|
|
||||||
{
|
|
||||||
last_filter_pkt = AP_HAL::millis();
|
|
||||||
|
|
||||||
// Iterate through fields of varying lengths in filter packet
|
|
||||||
for (uint8_t i = 0; i < packet.header[3]; i += packet.payload[i]) {
|
|
||||||
switch ((FilterPacketField) packet.payload[i+1]) {
|
|
||||||
// GPS Timestamp
|
|
||||||
case FilterPacketField::GPS_TIME: {
|
|
||||||
filter_data.tow_ms = be64todouble_ptr(packet.payload, i+2) * 1000; // Convert seconds to ms
|
|
||||||
filter_data.week = be16toh_ptr(&packet.payload[i+10]);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// LLH Position
|
|
||||||
case FilterPacketField::LLH_POSITION: {
|
|
||||||
filter_data.lat = be64todouble_ptr(packet.payload, i+2) * 1.0e7; // Decimal degrees to degrees
|
|
||||||
filter_data.lon = be64todouble_ptr(packet.payload, i+10) * 1.0e7;
|
|
||||||
filter_data.hae_altitude = be64todouble_ptr(packet.payload, i+26) * 1.0e2; // Meters to cm
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// NED Velocity
|
|
||||||
case FilterPacketField::NED_VELOCITY: {
|
|
||||||
filter_data.ned_velocity_north = be32tofloat_ptr(packet.payload, i+2);
|
|
||||||
filter_data.ned_velocity_east = be32tofloat_ptr(packet.payload, i+6);
|
|
||||||
filter_data.ned_velocity_down = be32tofloat_ptr(packet.payload, i+10);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
// Filter Status
|
|
||||||
case FilterPacketField::FILTER_STATUS: {
|
|
||||||
filter_status.state = be16toh_ptr(&packet.payload[i+2]);
|
|
||||||
filter_status.mode = be16toh_ptr(&packet.payload[i+4]);
|
|
||||||
filter_status.flags = be16toh_ptr(&packet.payload[i+6]);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_ExternalAHRS_MicroStrain::post_filter() const
|
|
||||||
{
|
|
||||||
{
|
|
||||||
WITH_SEMAPHORE(state.sem);
|
|
||||||
state.velocity = Vector3f{filter_data.ned_velocity_north, filter_data.ned_velocity_east, filter_data.ned_velocity_down};
|
|
||||||
state.have_velocity = true;
|
|
||||||
|
|
||||||
state.location = Location{filter_data.lat, filter_data.lon, gnss_data.msl_altitude, Location::AltFrame::ABSOLUTE};
|
|
||||||
state.have_location = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
AP_ExternalAHRS::gps_data_message_t gps {
|
|
||||||
gps_week: filter_data.week,
|
|
||||||
ms_tow: filter_data.tow_ms,
|
|
||||||
fix_type: (uint8_t) gnss_data.fix_type,
|
|
||||||
satellites_in_view: gnss_data.satellites,
|
|
||||||
|
|
||||||
horizontal_pos_accuracy: gnss_data.horizontal_position_accuracy,
|
|
||||||
vertical_pos_accuracy: gnss_data.vertical_position_accuracy,
|
|
||||||
horizontal_vel_accuracy: gnss_data.speed_accuracy,
|
|
||||||
|
|
||||||
hdop: gnss_data.hdop,
|
|
||||||
vdop: gnss_data.vdop,
|
|
||||||
|
|
||||||
longitude: filter_data.lon,
|
|
||||||
latitude: filter_data.lat,
|
|
||||||
msl_altitude: gnss_data.msl_altitude,
|
|
||||||
|
|
||||||
ned_vel_north: filter_data.ned_velocity_north,
|
|
||||||
ned_vel_east: filter_data.ned_velocity_east,
|
|
||||||
ned_vel_down: filter_data.ned_velocity_down,
|
|
||||||
};
|
|
||||||
|
|
||||||
if (gps.fix_type >= 3 && !state.have_origin) {
|
|
||||||
WITH_SEMAPHORE(state.sem);
|
|
||||||
state.origin = Location{int32_t(filter_data.lat),
|
|
||||||
int32_t(filter_data.lon),
|
|
||||||
int32_t(gnss_data.msl_altitude),
|
|
||||||
Location::AltFrame::ABSOLUTE};
|
|
||||||
state.have_origin = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t instance;
|
|
||||||
if (AP::gps().get_first_external_instance(instance)) {
|
|
||||||
AP::gps().handle_external(gps, instance);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t AP_ExternalAHRS_MicroStrain::get_port(void) const
|
|
||||||
{
|
|
||||||
if (!uart) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
return port_num;
|
|
||||||
};
|
|
||||||
|
|
||||||
// Get model/type name
|
|
||||||
const char* AP_ExternalAHRS_MicroStrain::get_name() const
|
|
||||||
{
|
|
||||||
return "MicroStrain";
|
|
||||||
}
|
|
||||||
|
|
||||||
bool AP_ExternalAHRS_MicroStrain::healthy(void) const
|
|
||||||
{
|
|
||||||
uint32_t now = AP_HAL::millis();
|
|
||||||
return (now - last_ins_pkt < 40 && now - last_gps_pkt < 500 && now - last_filter_pkt < 500);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool AP_ExternalAHRS_MicroStrain::initialised(void) const
|
|
||||||
{
|
|
||||||
return last_ins_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool AP_ExternalAHRS_MicroStrain::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
|
|
||||||
{
|
|
||||||
if (!healthy()) {
|
|
||||||
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain unhealthy");
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
if (gnss_data.fix_type < 3) {
|
|
||||||
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain no GPS lock");
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
if (filter_status.state != 0x02) {
|
|
||||||
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain filter not running");
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_ExternalAHRS_MicroStrain::get_filter_status(nav_filter_status &status) const
|
|
||||||
{
|
|
||||||
memset(&status, 0, sizeof(status));
|
|
||||||
if (last_ins_pkt != 0 && last_gps_pkt != 0) {
|
|
||||||
status.flags.initalized = 1;
|
|
||||||
}
|
|
||||||
if (healthy() && last_ins_pkt != 0) {
|
|
||||||
status.flags.attitude = 1;
|
|
||||||
status.flags.vert_vel = 1;
|
|
||||||
status.flags.vert_pos = 1;
|
|
||||||
|
|
||||||
if (gnss_data.fix_type >= 3) {
|
|
||||||
status.flags.horiz_vel = 1;
|
|
||||||
status.flags.horiz_pos_rel = 1;
|
|
||||||
status.flags.horiz_pos_abs = 1;
|
|
||||||
status.flags.pred_horiz_pos_rel = 1;
|
|
||||||
status.flags.pred_horiz_pos_abs = 1;
|
|
||||||
status.flags.using_gps = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_ExternalAHRS_MicroStrain::send_status_report(GCS_MAVLINK &link) const
|
|
||||||
{
|
|
||||||
// prepare flags
|
|
||||||
uint16_t flags = 0;
|
|
||||||
nav_filter_status filterStatus;
|
|
||||||
get_filter_status(filterStatus);
|
|
||||||
if (filterStatus.flags.attitude) {
|
|
||||||
flags |= EKF_ATTITUDE;
|
|
||||||
}
|
|
||||||
if (filterStatus.flags.horiz_vel) {
|
|
||||||
flags |= EKF_VELOCITY_HORIZ;
|
|
||||||
}
|
|
||||||
if (filterStatus.flags.vert_vel) {
|
|
||||||
flags |= EKF_VELOCITY_VERT;
|
|
||||||
}
|
|
||||||
if (filterStatus.flags.horiz_pos_rel) {
|
|
||||||
flags |= EKF_POS_HORIZ_REL;
|
|
||||||
}
|
|
||||||
if (filterStatus.flags.horiz_pos_abs) {
|
|
||||||
flags |= EKF_POS_HORIZ_ABS;
|
|
||||||
}
|
|
||||||
if (filterStatus.flags.vert_pos) {
|
|
||||||
flags |= EKF_POS_VERT_ABS;
|
|
||||||
}
|
|
||||||
if (filterStatus.flags.terrain_alt) {
|
|
||||||
flags |= EKF_POS_VERT_AGL;
|
|
||||||
}
|
|
||||||
if (filterStatus.flags.const_pos_mode) {
|
|
||||||
flags |= EKF_CONST_POS_MODE;
|
|
||||||
}
|
|
||||||
if (filterStatus.flags.pred_horiz_pos_rel) {
|
|
||||||
flags |= EKF_PRED_POS_HORIZ_REL;
|
|
||||||
}
|
|
||||||
if (filterStatus.flags.pred_horiz_pos_abs) {
|
|
||||||
flags |= EKF_PRED_POS_HORIZ_ABS;
|
|
||||||
}
|
|
||||||
if (!filterStatus.flags.initalized) {
|
|
||||||
flags |= EKF_UNINITIALIZED;
|
|
||||||
}
|
|
||||||
|
|
||||||
// send message
|
|
||||||
const float vel_gate = 4; // represents hz value data is posted at
|
|
||||||
const float pos_gate = 4; // represents hz value data is posted at
|
|
||||||
const float hgt_gate = 4; // represents hz value data is posted at
|
|
||||||
const float mag_var = 0; //we may need to change this to be like the other gates, set to 0 because mag is ignored by the ins filter in vectornav
|
|
||||||
mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
|
|
||||||
gnss_data.speed_accuracy/vel_gate, gnss_data.horizontal_position_accuracy/pos_gate, gnss_data.vertical_position_accuracy/hgt_gate,
|
|
||||||
mag_var, 0, 0);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
Vector3f AP_ExternalAHRS_MicroStrain::populate_vector3f(const uint8_t *data, uint8_t offset) const
|
|
||||||
{
|
|
||||||
return Vector3f {
|
|
||||||
be32tofloat_ptr(data, offset),
|
|
||||||
be32tofloat_ptr(data, offset+4),
|
|
||||||
be32tofloat_ptr(data, offset+8)
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
Quaternion AP_ExternalAHRS_MicroStrain::populate_quaternion(const uint8_t *data, uint8_t offset) const
|
|
||||||
{
|
|
||||||
return Quaternion {
|
|
||||||
be32tofloat_ptr(data, offset),
|
|
||||||
be32tofloat_ptr(data, offset+4),
|
|
||||||
be32tofloat_ptr(data, offset+8),
|
|
||||||
be32tofloat_ptr(data, offset+12)
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED
|
|
||||||
313
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp
Normal file
313
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp
Normal file
@ -0,0 +1,313 @@
|
|||||||
|
/*
|
||||||
|
This program is free software: you can redistribute it and/or modify
|
||||||
|
it under the terms of the GNU General Public License as published by
|
||||||
|
the Free Software Foundation, either version 3 of the License, or
|
||||||
|
(at your option) any later version.
|
||||||
|
This program is distributed in the hope that it will be useful,
|
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
GNU General Public License for more details.
|
||||||
|
You should have received a copy of the GNU General Public License
|
||||||
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
/*
|
||||||
|
suppport for MicroStrain CX5/GX5-45 serially connected AHRS Systems
|
||||||
|
*/
|
||||||
|
|
||||||
|
#define ALLOW_DOUBLE_MATH_FUNCTIONS
|
||||||
|
|
||||||
|
#include "AP_ExternalAHRS_config.h"
|
||||||
|
|
||||||
|
#if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
|
||||||
|
|
||||||
|
#include "AP_ExternalAHRS_MicroStrain5.h"
|
||||||
|
#include "AP_Compass/AP_Compass_config.h"
|
||||||
|
#include <AP_Baro/AP_Baro.h>
|
||||||
|
#include <AP_Compass/AP_Compass.h>
|
||||||
|
#include <AP_GPS/AP_GPS.h>
|
||||||
|
#include <AP_HAL/utility/sparse-endian.h>
|
||||||
|
#include <AP_InertialSensor/AP_InertialSensor.h>
|
||||||
|
#include <GCS_MAVLink/GCS.h>
|
||||||
|
#include <AP_Logger/AP_Logger.h>
|
||||||
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||||
|
|
||||||
|
extern const AP_HAL::HAL &hal;
|
||||||
|
|
||||||
|
AP_ExternalAHRS_MicroStrain5::AP_ExternalAHRS_MicroStrain5(AP_ExternalAHRS *_frontend,
|
||||||
|
AP_ExternalAHRS::state_t &_state): AP_ExternalAHRS_backend(_frontend, _state)
|
||||||
|
{
|
||||||
|
auto &sm = AP::serialmanager();
|
||||||
|
uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0);
|
||||||
|
|
||||||
|
baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0);
|
||||||
|
port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
|
||||||
|
|
||||||
|
if (!uart) {
|
||||||
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS no UART");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_MicroStrain5::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
|
||||||
|
AP_BoardConfig::allocation_error("Failed to allocate ExternalAHRS update thread");
|
||||||
|
}
|
||||||
|
|
||||||
|
hal.scheduler->delay(5000);
|
||||||
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain ExternalAHRS initialised");
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_ExternalAHRS_MicroStrain5::update_thread(void)
|
||||||
|
{
|
||||||
|
if (!port_open) {
|
||||||
|
port_open = true;
|
||||||
|
uart->begin(baudrate);
|
||||||
|
}
|
||||||
|
|
||||||
|
while (true) {
|
||||||
|
build_packet();
|
||||||
|
hal.scheduler->delay_microseconds(100);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Builds packets by looking at each individual byte, once a full packet has been read in it checks the checksum then handles the packet.
|
||||||
|
void AP_ExternalAHRS_MicroStrain5::build_packet()
|
||||||
|
{
|
||||||
|
WITH_SEMAPHORE(sem);
|
||||||
|
uint32_t nbytes = MIN(uart->available(), 2048u);
|
||||||
|
while (nbytes--> 0) {
|
||||||
|
uint8_t b;
|
||||||
|
if (!uart->read(b)) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
DescriptorSet descriptor;
|
||||||
|
if (handle_byte(b, descriptor)) {
|
||||||
|
switch (descriptor) {
|
||||||
|
case DescriptorSet::IMUData:
|
||||||
|
post_imu();
|
||||||
|
break;
|
||||||
|
case DescriptorSet::GNSSData:
|
||||||
|
break;
|
||||||
|
case DescriptorSet::EstimationData:
|
||||||
|
post_filter();
|
||||||
|
break;
|
||||||
|
case DescriptorSet::BaseCommand:
|
||||||
|
case DescriptorSet::DMCommand:
|
||||||
|
case DescriptorSet::SystemCommand:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Posts data from an imu packet to `state` and `handle_external` methods
|
||||||
|
void AP_ExternalAHRS_MicroStrain5::post_imu() const
|
||||||
|
{
|
||||||
|
{
|
||||||
|
WITH_SEMAPHORE(state.sem);
|
||||||
|
state.accel = imu_data.accel;
|
||||||
|
state.gyro = imu_data.gyro;
|
||||||
|
|
||||||
|
state.quat = imu_data.quat;
|
||||||
|
state.have_quaternion = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
AP_ExternalAHRS::ins_data_message_t ins {
|
||||||
|
accel: imu_data.accel,
|
||||||
|
gyro: imu_data.gyro,
|
||||||
|
temperature: -300
|
||||||
|
};
|
||||||
|
AP::ins().handle_external(ins);
|
||||||
|
}
|
||||||
|
|
||||||
|
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||||
|
{
|
||||||
|
AP_ExternalAHRS::mag_data_message_t mag {
|
||||||
|
field: imu_data.mag
|
||||||
|
};
|
||||||
|
AP::compass().handle_external(mag);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if AP_BARO_EXTERNALAHRS_ENABLED
|
||||||
|
{
|
||||||
|
const AP_ExternalAHRS::baro_data_message_t baro {
|
||||||
|
instance: 0,
|
||||||
|
pressure_pa: imu_data.pressure,
|
||||||
|
// setting temp to 25 effectively disables barometer temperature calibrations - these are already performed by MicroStrain
|
||||||
|
temperature: 25,
|
||||||
|
};
|
||||||
|
AP::baro().handle_external(baro);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_ExternalAHRS_MicroStrain5::post_filter() const
|
||||||
|
{
|
||||||
|
{
|
||||||
|
WITH_SEMAPHORE(state.sem);
|
||||||
|
state.velocity = Vector3f{filter_data.ned_velocity_north, filter_data.ned_velocity_east, filter_data.ned_velocity_down};
|
||||||
|
state.have_velocity = true;
|
||||||
|
|
||||||
|
state.location = Location{filter_data.lat, filter_data.lon, gnss_data.msl_altitude, Location::AltFrame::ABSOLUTE};
|
||||||
|
state.have_location = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
AP_ExternalAHRS::gps_data_message_t gps {
|
||||||
|
gps_week: filter_data.week,
|
||||||
|
ms_tow: filter_data.tow_ms,
|
||||||
|
fix_type: (uint8_t) gnss_data.fix_type,
|
||||||
|
satellites_in_view: gnss_data.satellites,
|
||||||
|
|
||||||
|
horizontal_pos_accuracy: gnss_data.horizontal_position_accuracy,
|
||||||
|
vertical_pos_accuracy: gnss_data.vertical_position_accuracy,
|
||||||
|
horizontal_vel_accuracy: gnss_data.speed_accuracy,
|
||||||
|
|
||||||
|
hdop: gnss_data.hdop,
|
||||||
|
vdop: gnss_data.vdop,
|
||||||
|
|
||||||
|
longitude: filter_data.lon,
|
||||||
|
latitude: filter_data.lat,
|
||||||
|
msl_altitude: gnss_data.msl_altitude,
|
||||||
|
|
||||||
|
ned_vel_north: filter_data.ned_velocity_north,
|
||||||
|
ned_vel_east: filter_data.ned_velocity_east,
|
||||||
|
ned_vel_down: filter_data.ned_velocity_down,
|
||||||
|
};
|
||||||
|
|
||||||
|
if (gps.fix_type >= 3 && !state.have_origin) {
|
||||||
|
WITH_SEMAPHORE(state.sem);
|
||||||
|
state.origin = Location{int32_t(filter_data.lat),
|
||||||
|
int32_t(filter_data.lon),
|
||||||
|
int32_t(gnss_data.msl_altitude),
|
||||||
|
Location::AltFrame::ABSOLUTE};
|
||||||
|
state.have_origin = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t instance;
|
||||||
|
if (AP::gps().get_first_external_instance(instance)) {
|
||||||
|
AP::gps().handle_external(gps, instance);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int8_t AP_ExternalAHRS_MicroStrain5::get_port(void) const
|
||||||
|
{
|
||||||
|
if (!uart) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
return port_num;
|
||||||
|
};
|
||||||
|
|
||||||
|
// Get model/type name
|
||||||
|
const char* AP_ExternalAHRS_MicroStrain5::get_name() const
|
||||||
|
{
|
||||||
|
return "MicroStrain5";
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AP_ExternalAHRS_MicroStrain5::healthy(void) const
|
||||||
|
{
|
||||||
|
uint32_t now = AP_HAL::millis();
|
||||||
|
return (now - last_ins_pkt < 40 && now - last_gps_pkt < 500 && now - last_filter_pkt < 500);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AP_ExternalAHRS_MicroStrain5::initialised(void) const
|
||||||
|
{
|
||||||
|
return last_ins_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AP_ExternalAHRS_MicroStrain5::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
|
||||||
|
{
|
||||||
|
if (!healthy()) {
|
||||||
|
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain unhealthy");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (gnss_data.fix_type < 3) {
|
||||||
|
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain no GPS lock");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (filter_status.state != 0x02) {
|
||||||
|
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain filter not running");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_ExternalAHRS_MicroStrain5::get_filter_status(nav_filter_status &status) const
|
||||||
|
{
|
||||||
|
memset(&status, 0, sizeof(status));
|
||||||
|
if (last_ins_pkt != 0 && last_gps_pkt != 0) {
|
||||||
|
status.flags.initalized = 1;
|
||||||
|
}
|
||||||
|
if (healthy() && last_ins_pkt != 0) {
|
||||||
|
status.flags.attitude = 1;
|
||||||
|
status.flags.vert_vel = 1;
|
||||||
|
status.flags.vert_pos = 1;
|
||||||
|
|
||||||
|
if (gnss_data.fix_type >= 3) {
|
||||||
|
status.flags.horiz_vel = 1;
|
||||||
|
status.flags.horiz_pos_rel = 1;
|
||||||
|
status.flags.horiz_pos_abs = 1;
|
||||||
|
status.flags.pred_horiz_pos_rel = 1;
|
||||||
|
status.flags.pred_horiz_pos_abs = 1;
|
||||||
|
status.flags.using_gps = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_ExternalAHRS_MicroStrain5::send_status_report(GCS_MAVLINK &link) const
|
||||||
|
{
|
||||||
|
// prepare flags
|
||||||
|
uint16_t flags = 0;
|
||||||
|
nav_filter_status filterStatus;
|
||||||
|
get_filter_status(filterStatus);
|
||||||
|
if (filterStatus.flags.attitude) {
|
||||||
|
flags |= EKF_ATTITUDE;
|
||||||
|
}
|
||||||
|
if (filterStatus.flags.horiz_vel) {
|
||||||
|
flags |= EKF_VELOCITY_HORIZ;
|
||||||
|
}
|
||||||
|
if (filterStatus.flags.vert_vel) {
|
||||||
|
flags |= EKF_VELOCITY_VERT;
|
||||||
|
}
|
||||||
|
if (filterStatus.flags.horiz_pos_rel) {
|
||||||
|
flags |= EKF_POS_HORIZ_REL;
|
||||||
|
}
|
||||||
|
if (filterStatus.flags.horiz_pos_abs) {
|
||||||
|
flags |= EKF_POS_HORIZ_ABS;
|
||||||
|
}
|
||||||
|
if (filterStatus.flags.vert_pos) {
|
||||||
|
flags |= EKF_POS_VERT_ABS;
|
||||||
|
}
|
||||||
|
if (filterStatus.flags.terrain_alt) {
|
||||||
|
flags |= EKF_POS_VERT_AGL;
|
||||||
|
}
|
||||||
|
if (filterStatus.flags.const_pos_mode) {
|
||||||
|
flags |= EKF_CONST_POS_MODE;
|
||||||
|
}
|
||||||
|
if (filterStatus.flags.pred_horiz_pos_rel) {
|
||||||
|
flags |= EKF_PRED_POS_HORIZ_REL;
|
||||||
|
}
|
||||||
|
if (filterStatus.flags.pred_horiz_pos_abs) {
|
||||||
|
flags |= EKF_PRED_POS_HORIZ_ABS;
|
||||||
|
}
|
||||||
|
if (!filterStatus.flags.initalized) {
|
||||||
|
flags |= EKF_UNINITIALIZED;
|
||||||
|
}
|
||||||
|
|
||||||
|
// send message
|
||||||
|
const float vel_gate = 4; // represents hz value data is posted at
|
||||||
|
const float pos_gate = 4; // represents hz value data is posted at
|
||||||
|
const float hgt_gate = 4; // represents hz value data is posted at
|
||||||
|
const float mag_var = 0; //we may need to change this to be like the other gates, set to 0 because mag is ignored by the ins filter in vectornav
|
||||||
|
mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
|
||||||
|
gnss_data.speed_accuracy/vel_gate, gnss_data.horizontal_position_accuracy/pos_gate, gnss_data.vertical_position_accuracy/hgt_gate,
|
||||||
|
mag_var, 0, 0);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#endif // AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
|
||||||
73
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h
Normal file
73
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h
Normal file
@ -0,0 +1,73 @@
|
|||||||
|
/*
|
||||||
|
This program is free software: you can redistribute it and/or modify
|
||||||
|
it under the terms of the GNU General Public License as published by
|
||||||
|
the Free Software Foundation, either version 3 of the License, or
|
||||||
|
(at your option) any later version.
|
||||||
|
This program is distributed in the hope that it will be useful,
|
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
GNU General Public License for more details.
|
||||||
|
You should have received a copy of the GNU General Public License
|
||||||
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
/*
|
||||||
|
suppport for MicroStrain CX5/GX5-45 serially connected AHRS Systems
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "AP_ExternalAHRS_config.h"
|
||||||
|
|
||||||
|
#if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
|
||||||
|
|
||||||
|
#include "AP_ExternalAHRS_backend.h"
|
||||||
|
#include <AP_GPS/AP_GPS.h>
|
||||||
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
#include "MicroStrain_common.h"
|
||||||
|
|
||||||
|
class AP_ExternalAHRS_MicroStrain5: public AP_ExternalAHRS_backend, public AP_MicroStrain
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
AP_ExternalAHRS_MicroStrain5(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state);
|
||||||
|
|
||||||
|
// get serial port number, -1 for not enabled
|
||||||
|
int8_t get_port(void) const override;
|
||||||
|
|
||||||
|
// Get model/type name
|
||||||
|
const char* get_name() const override;
|
||||||
|
|
||||||
|
// accessors for AP_AHRS
|
||||||
|
bool healthy(void) const override;
|
||||||
|
bool initialised(void) const override;
|
||||||
|
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
|
||||||
|
void get_filter_status(nav_filter_status &status) const override;
|
||||||
|
void send_status_report(class GCS_MAVLINK &link) const override;
|
||||||
|
|
||||||
|
// check for new data
|
||||||
|
void update() override {
|
||||||
|
build_packet();
|
||||||
|
};
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
uint32_t baudrate;
|
||||||
|
int8_t port_num;
|
||||||
|
bool port_open = false;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void build_packet();
|
||||||
|
|
||||||
|
void post_imu() const;
|
||||||
|
void post_gnss() const;
|
||||||
|
void post_filter() const;
|
||||||
|
|
||||||
|
void update_thread();
|
||||||
|
|
||||||
|
AP_HAL::UARTDriver *uart;
|
||||||
|
HAL_Semaphore sem;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
|
||||||
@ -10,8 +10,12 @@
|
|||||||
#define AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED HAL_EXTERNAL_AHRS_ENABLED
|
#define AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED HAL_EXTERNAL_AHRS_ENABLED
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED
|
#ifndef AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
|
||||||
#define AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED
|
#define AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef AP_MICROSTRAIN_ENABLED
|
||||||
|
#define AP_MICROSTRAIN_ENABLED AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef AP_EXTERNAL_AHRS_VECTORNAV_ENABLED
|
#ifndef AP_EXTERNAL_AHRS_VECTORNAV_ENABLED
|
||||||
|
|||||||
307
libraries/AP_ExternalAHRS/MicroStrain_common.cpp
Normal file
307
libraries/AP_ExternalAHRS/MicroStrain_common.cpp
Normal file
@ -0,0 +1,307 @@
|
|||||||
|
/*
|
||||||
|
This program is free software: you can redistribute it and/or modify
|
||||||
|
it under the terms of the GNU General Public License as published by
|
||||||
|
the Free Software Foundation, either version 3 of the License, or
|
||||||
|
(at your option) any later version.
|
||||||
|
This program is distributed in the hope that it will be useful,
|
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
GNU General Public License for more details.
|
||||||
|
You should have received a copy of the GNU General Public License
|
||||||
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
/*
|
||||||
|
suppport for MicroStrain CX5/GX5-45 serially connected AHRS Systems
|
||||||
|
*/
|
||||||
|
|
||||||
|
#define ALLOW_DOUBLE_MATH_FUNCTIONS
|
||||||
|
|
||||||
|
#include "AP_ExternalAHRS_config.h"
|
||||||
|
|
||||||
|
#if AP_MICROSTRAIN_ENABLED
|
||||||
|
|
||||||
|
#include "MicroStrain_common.h"
|
||||||
|
#include <AP_HAL/utility/sparse-endian.h>
|
||||||
|
|
||||||
|
enum class INSPacketField {
|
||||||
|
ACCEL = 0x04,
|
||||||
|
GYRO = 0x05,
|
||||||
|
QUAT = 0x0A,
|
||||||
|
MAG = 0x06,
|
||||||
|
PRESSURE = 0x17
|
||||||
|
};
|
||||||
|
|
||||||
|
enum class GNSSPacketField {
|
||||||
|
LLH_POSITION = 0x03,
|
||||||
|
NED_VELOCITY = 0x05,
|
||||||
|
DOP_DATA = 0x07,
|
||||||
|
GPS_TIME = 0x09,
|
||||||
|
FIX_INFO = 0x0B
|
||||||
|
};
|
||||||
|
|
||||||
|
enum class GNSSFixType {
|
||||||
|
FIX_3D = 0x00,
|
||||||
|
FIX_2D = 0x01,
|
||||||
|
TIME_ONLY = 0x02,
|
||||||
|
NONE = 0x03,
|
||||||
|
INVALID = 0x04
|
||||||
|
};
|
||||||
|
|
||||||
|
enum class FilterPacketField {
|
||||||
|
FILTER_STATUS = 0x10,
|
||||||
|
GPS_TIME = 0x11,
|
||||||
|
LLH_POSITION = 0x01,
|
||||||
|
NED_VELOCITY = 0x02
|
||||||
|
};
|
||||||
|
|
||||||
|
bool AP_MicroStrain::handle_byte(const uint8_t b, DescriptorSet& descriptor)
|
||||||
|
{
|
||||||
|
switch (message_in.state) {
|
||||||
|
case ParseState::WaitingFor_SyncOne:
|
||||||
|
if (b == SYNC_ONE) {
|
||||||
|
message_in.packet.header[0] = b;
|
||||||
|
message_in.state = ParseState::WaitingFor_SyncTwo;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case ParseState::WaitingFor_SyncTwo:
|
||||||
|
if (b == SYNC_TWO) {
|
||||||
|
message_in.packet.header[1] = b;
|
||||||
|
message_in.state = ParseState::WaitingFor_Descriptor;
|
||||||
|
} else {
|
||||||
|
message_in.state = ParseState::WaitingFor_SyncOne;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case ParseState::WaitingFor_Descriptor:
|
||||||
|
message_in.packet.header[2] = b;
|
||||||
|
message_in.state = ParseState::WaitingFor_PayloadLength;
|
||||||
|
break;
|
||||||
|
case ParseState::WaitingFor_PayloadLength:
|
||||||
|
message_in.packet.header[3] = b;
|
||||||
|
message_in.state = ParseState::WaitingFor_Data;
|
||||||
|
message_in.index = 0;
|
||||||
|
break;
|
||||||
|
case ParseState::WaitingFor_Data:
|
||||||
|
message_in.packet.payload[message_in.index++] = b;
|
||||||
|
if (message_in.index >= message_in.packet.header[3]) {
|
||||||
|
message_in.state = ParseState::WaitingFor_Checksum;
|
||||||
|
message_in.index = 0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case ParseState::WaitingFor_Checksum:
|
||||||
|
message_in.packet.checksum[message_in.index++] = b;
|
||||||
|
if (message_in.index >= 2) {
|
||||||
|
message_in.state = ParseState::WaitingFor_SyncOne;
|
||||||
|
message_in.index = 0;
|
||||||
|
|
||||||
|
if (valid_packet(message_in.packet)) {
|
||||||
|
descriptor = handle_packet(message_in.packet);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AP_MicroStrain::valid_packet(const MicroStrain_Packet & packet)
|
||||||
|
{
|
||||||
|
uint8_t checksum_one = 0;
|
||||||
|
uint8_t checksum_two = 0;
|
||||||
|
|
||||||
|
for (int i = 0; i < 4; i++) {
|
||||||
|
checksum_one += packet.header[i];
|
||||||
|
checksum_two += checksum_one;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int i = 0; i < packet.header[3]; i++) {
|
||||||
|
checksum_one += packet.payload[i];
|
||||||
|
checksum_two += checksum_one;
|
||||||
|
}
|
||||||
|
|
||||||
|
return packet.checksum[0] == checksum_one && packet.checksum[1] == checksum_two;
|
||||||
|
}
|
||||||
|
|
||||||
|
AP_MicroStrain::DescriptorSet AP_MicroStrain::handle_packet(const MicroStrain_Packet& packet)
|
||||||
|
{
|
||||||
|
const DescriptorSet descriptor = DescriptorSet(packet.header[2]);
|
||||||
|
switch (descriptor) {
|
||||||
|
case DescriptorSet::IMUData:
|
||||||
|
handle_imu(packet);
|
||||||
|
break;
|
||||||
|
case DescriptorSet::GNSSData:
|
||||||
|
handle_gnss(packet);
|
||||||
|
break;
|
||||||
|
case DescriptorSet::EstimationData:
|
||||||
|
handle_filter(packet);
|
||||||
|
break;
|
||||||
|
case DescriptorSet::BaseCommand:
|
||||||
|
case DescriptorSet::DMCommand:
|
||||||
|
case DescriptorSet::SystemCommand:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
return descriptor;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void AP_MicroStrain::handle_imu(const MicroStrain_Packet& packet)
|
||||||
|
{
|
||||||
|
last_ins_pkt = AP_HAL::millis();
|
||||||
|
|
||||||
|
// Iterate through fields of varying lengths in INS packet
|
||||||
|
for (uint8_t i = 0; i < packet.header[3]; i += packet.payload[i]) {
|
||||||
|
switch ((INSPacketField) packet.payload[i+1]) {
|
||||||
|
// Scaled Ambient Pressure
|
||||||
|
case INSPacketField::PRESSURE: {
|
||||||
|
imu_data.pressure = be32tofloat_ptr(packet.payload, i+2) * 100; // Convert millibar to pascals
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// Scaled Magnetometer Vector
|
||||||
|
case INSPacketField::MAG: {
|
||||||
|
imu_data.mag = populate_vector3f(packet.payload, i+2) * 1000; // Convert gauss to milligauss
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// Scaled Accelerometer Vector
|
||||||
|
case INSPacketField::ACCEL: {
|
||||||
|
imu_data.accel = populate_vector3f(packet.payload, i+2) * GRAVITY_MSS; // Convert g's to m/s^2
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// Scaled Gyro Vector
|
||||||
|
case INSPacketField::GYRO: {
|
||||||
|
imu_data.gyro = populate_vector3f(packet.payload, i+2);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// Quaternion
|
||||||
|
case INSPacketField::QUAT: {
|
||||||
|
imu_data.quat = populate_quaternion(packet.payload, i+2);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void AP_MicroStrain::handle_gnss(const MicroStrain_Packet &packet)
|
||||||
|
{
|
||||||
|
last_gps_pkt = AP_HAL::millis();
|
||||||
|
|
||||||
|
// Iterate through fields of varying lengths in GNSS packet
|
||||||
|
for (uint8_t i = 0; i < packet.header[3]; i += packet.payload[i]) {
|
||||||
|
switch ((GNSSPacketField) packet.payload[i+1]) {
|
||||||
|
// GPS Time
|
||||||
|
case GNSSPacketField::GPS_TIME: {
|
||||||
|
gnss_data.tow_ms = double_to_uint32(be64todouble_ptr(packet.payload, i+2) * 1000); // Convert seconds to ms
|
||||||
|
gnss_data.week = be16toh_ptr(&packet.payload[i+10]);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// GNSS Fix Information
|
||||||
|
case GNSSPacketField::FIX_INFO: {
|
||||||
|
switch ((GNSSFixType) packet.payload[i+2]) {
|
||||||
|
case (GNSSFixType::FIX_3D): {
|
||||||
|
gnss_data.fix_type = GPS_FIX_TYPE_3D_FIX;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (GNSSFixType::FIX_2D): {
|
||||||
|
gnss_data.fix_type = GPS_FIX_TYPE_2D_FIX;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (GNSSFixType::TIME_ONLY):
|
||||||
|
case (GNSSFixType::NONE): {
|
||||||
|
gnss_data.fix_type = GPS_FIX_TYPE_NO_FIX;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
default:
|
||||||
|
case (GNSSFixType::INVALID): {
|
||||||
|
gnss_data.fix_type = GPS_FIX_TYPE_NO_GPS;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
gnss_data.satellites = packet.payload[i+3];
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// LLH Position
|
||||||
|
case GNSSPacketField::LLH_POSITION: {
|
||||||
|
gnss_data.lat = be64todouble_ptr(packet.payload, i+2) * 1.0e7; // Decimal degrees to degrees
|
||||||
|
gnss_data.lon = be64todouble_ptr(packet.payload, i+10) * 1.0e7;
|
||||||
|
gnss_data.msl_altitude = be64todouble_ptr(packet.payload, i+26) * 1.0e2; // Meters to cm
|
||||||
|
gnss_data.horizontal_position_accuracy = be32tofloat_ptr(packet.payload, i+34);
|
||||||
|
gnss_data.vertical_position_accuracy = be32tofloat_ptr(packet.payload, i+38);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// DOP Data
|
||||||
|
case GNSSPacketField::DOP_DATA: {
|
||||||
|
gnss_data.hdop = be32tofloat_ptr(packet.payload, i+10);
|
||||||
|
gnss_data.vdop = be32tofloat_ptr(packet.payload, i+14);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// NED Velocity
|
||||||
|
case GNSSPacketField::NED_VELOCITY: {
|
||||||
|
gnss_data.ned_velocity_north = be32tofloat_ptr(packet.payload, i+2);
|
||||||
|
gnss_data.ned_velocity_east = be32tofloat_ptr(packet.payload, i+6);
|
||||||
|
gnss_data.ned_velocity_down = be32tofloat_ptr(packet.payload, i+10);
|
||||||
|
gnss_data.speed_accuracy = be32tofloat_ptr(packet.payload, i+26);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_MicroStrain::handle_filter(const MicroStrain_Packet &packet)
|
||||||
|
{
|
||||||
|
last_filter_pkt = AP_HAL::millis();
|
||||||
|
|
||||||
|
// Iterate through fields of varying lengths in filter packet
|
||||||
|
for (uint8_t i = 0; i < packet.header[3]; i += packet.payload[i]) {
|
||||||
|
switch ((FilterPacketField) packet.payload[i+1]) {
|
||||||
|
// GPS Timestamp
|
||||||
|
case FilterPacketField::GPS_TIME: {
|
||||||
|
filter_data.tow_ms = be64todouble_ptr(packet.payload, i+2) * 1000; // Convert seconds to ms
|
||||||
|
filter_data.week = be16toh_ptr(&packet.payload[i+10]);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// LLH Position
|
||||||
|
case FilterPacketField::LLH_POSITION: {
|
||||||
|
filter_data.lat = be64todouble_ptr(packet.payload, i+2) * 1.0e7; // Decimal degrees to degrees
|
||||||
|
filter_data.lon = be64todouble_ptr(packet.payload, i+10) * 1.0e7;
|
||||||
|
filter_data.hae_altitude = be64todouble_ptr(packet.payload, i+26) * 1.0e2; // Meters to cm
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// NED Velocity
|
||||||
|
case FilterPacketField::NED_VELOCITY: {
|
||||||
|
filter_data.ned_velocity_north = be32tofloat_ptr(packet.payload, i+2);
|
||||||
|
filter_data.ned_velocity_east = be32tofloat_ptr(packet.payload, i+6);
|
||||||
|
filter_data.ned_velocity_down = be32tofloat_ptr(packet.payload, i+10);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
// Filter Status
|
||||||
|
case FilterPacketField::FILTER_STATUS: {
|
||||||
|
filter_status.state = be16toh_ptr(&packet.payload[i+2]);
|
||||||
|
filter_status.mode = be16toh_ptr(&packet.payload[i+4]);
|
||||||
|
filter_status.flags = be16toh_ptr(&packet.payload[i+6]);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Vector3f AP_MicroStrain::populate_vector3f(const uint8_t *data, uint8_t offset)
|
||||||
|
{
|
||||||
|
return Vector3f {
|
||||||
|
be32tofloat_ptr(data, offset),
|
||||||
|
be32tofloat_ptr(data, offset+4),
|
||||||
|
be32tofloat_ptr(data, offset+8)
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
Quaternion AP_MicroStrain::populate_quaternion(const uint8_t *data, uint8_t offset)
|
||||||
|
{
|
||||||
|
return Quaternion {
|
||||||
|
be32tofloat_ptr(data, offset),
|
||||||
|
be32tofloat_ptr(data, offset+4),
|
||||||
|
be32tofloat_ptr(data, offset+8),
|
||||||
|
be32tofloat_ptr(data, offset+12)
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#endif // AP_MICROSTRAIN_ENABLED
|
||||||
@ -11,43 +11,25 @@
|
|||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
/*
|
/*
|
||||||
suppport for serial connected AHRS systems
|
suppport for MicroStrain MIP parsing
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "AP_ExternalAHRS_config.h"
|
#include "AP_ExternalAHRS_config.h"
|
||||||
|
|
||||||
#if AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED
|
#if AP_MICROSTRAIN_ENABLED
|
||||||
|
|
||||||
#include "AP_ExternalAHRS_backend.h"
|
|
||||||
#include <AP_GPS/AP_GPS.h>
|
#include <AP_GPS/AP_GPS.h>
|
||||||
|
#include <AP_Math/vector3.h>
|
||||||
|
#include <AP_Math/quaternion.h>
|
||||||
|
|
||||||
class AP_ExternalAHRS_MicroStrain: public AP_ExternalAHRS_backend
|
class AP_MicroStrain
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
|
||||||
AP_ExternalAHRS_MicroStrain(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state);
|
|
||||||
|
|
||||||
// get serial port number, -1 for not enabled
|
protected:
|
||||||
int8_t get_port(void) const override;
|
|
||||||
|
|
||||||
// Get model/type name
|
|
||||||
const char* get_name() const override;
|
|
||||||
|
|
||||||
// accessors for AP_AHRS
|
|
||||||
bool healthy(void) const override;
|
|
||||||
bool initialised(void) const override;
|
|
||||||
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
|
|
||||||
void get_filter_status(nav_filter_status &status) const override;
|
|
||||||
void send_status_report(class GCS_MAVLINK &link) const override;
|
|
||||||
|
|
||||||
// check for new data
|
|
||||||
void update() override {
|
|
||||||
build_packet();
|
|
||||||
};
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
enum class ParseState {
|
enum class ParseState {
|
||||||
WaitingFor_SyncOne,
|
WaitingFor_SyncOne,
|
||||||
@ -58,23 +40,7 @@ private:
|
|||||||
WaitingFor_Checksum
|
WaitingFor_Checksum
|
||||||
};
|
};
|
||||||
|
|
||||||
void update_thread();
|
// A MicroStrain packet can be a maximum of 261 bytes
|
||||||
|
|
||||||
AP_HAL::UARTDriver *uart;
|
|
||||||
HAL_Semaphore sem;
|
|
||||||
|
|
||||||
uint32_t baudrate;
|
|
||||||
int8_t port_num;
|
|
||||||
bool port_open = false;
|
|
||||||
|
|
||||||
const uint8_t SYNC_ONE = 0x75;
|
|
||||||
const uint8_t SYNC_TWO = 0x65;
|
|
||||||
|
|
||||||
uint32_t last_ins_pkt;
|
|
||||||
uint32_t last_gps_pkt;
|
|
||||||
uint32_t last_filter_pkt;
|
|
||||||
|
|
||||||
// A MicroStrain packet can be a maximum of 261 bytes
|
|
||||||
struct MicroStrain_Packet {
|
struct MicroStrain_Packet {
|
||||||
uint8_t header[4];
|
uint8_t header[4];
|
||||||
uint8_t payload[255];
|
uint8_t payload[255];
|
||||||
@ -83,7 +49,7 @@ private:
|
|||||||
|
|
||||||
struct {
|
struct {
|
||||||
MicroStrain_Packet packet;
|
MicroStrain_Packet packet;
|
||||||
ParseState state;
|
AP_MicroStrain::ParseState state;
|
||||||
uint8_t index;
|
uint8_t index;
|
||||||
} message_in;
|
} message_in;
|
||||||
|
|
||||||
@ -133,19 +99,36 @@ private:
|
|||||||
float speed_accuracy;
|
float speed_accuracy;
|
||||||
} filter_data;
|
} filter_data;
|
||||||
|
|
||||||
void build_packet();
|
enum class DescriptorSet {
|
||||||
bool valid_packet(const MicroStrain_Packet &packet) const;
|
BaseCommand = 0x01,
|
||||||
void handle_packet(const MicroStrain_Packet &packet);
|
DMCommand = 0x0C,
|
||||||
|
SystemCommand = 0x7F,
|
||||||
|
IMUData = 0x80,
|
||||||
|
GNSSData = 0x81,
|
||||||
|
EstimationData = 0x82
|
||||||
|
};
|
||||||
|
|
||||||
|
const uint8_t SYNC_ONE = 0x75;
|
||||||
|
const uint8_t SYNC_TWO = 0x65;
|
||||||
|
|
||||||
|
uint32_t last_ins_pkt;
|
||||||
|
uint32_t last_gps_pkt;
|
||||||
|
uint32_t last_filter_pkt;
|
||||||
|
|
||||||
|
// Handle a single byte.
|
||||||
|
// If the byte matches a descriptor, it returns true and that type should be handled.
|
||||||
|
bool handle_byte(const uint8_t b, DescriptorSet& descriptor);
|
||||||
|
// Returns true if the fletcher checksum for the packet is valid, else false.
|
||||||
|
static bool valid_packet(const MicroStrain_Packet &packet);
|
||||||
|
// Calls the correct functions based on the packet descriptor of the packet
|
||||||
|
DescriptorSet handle_packet(const MicroStrain_Packet &packet);
|
||||||
|
// Collects data from an imu packet into `imu_data`
|
||||||
void handle_imu(const MicroStrain_Packet &packet);
|
void handle_imu(const MicroStrain_Packet &packet);
|
||||||
|
// Collects data from a gnss packet into `gnss_data`
|
||||||
void handle_gnss(const MicroStrain_Packet &packet);
|
void handle_gnss(const MicroStrain_Packet &packet);
|
||||||
void handle_filter(const MicroStrain_Packet &packet);
|
void handle_filter(const MicroStrain_Packet &packet);
|
||||||
void post_imu() const;
|
static Vector3f populate_vector3f(const uint8_t* data, uint8_t offset);
|
||||||
void post_gnss() const;
|
static Quaternion populate_quaternion(const uint8_t* data, uint8_t offset);
|
||||||
void post_filter() const;
|
|
||||||
|
|
||||||
Vector3f populate_vector3f(const uint8_t* data, uint8_t offset) const;
|
|
||||||
Quaternion populate_quaternion(const uint8_t* data, uint8_t offset) const;
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED
|
#endif // AP_MICROSTRAIN_ENABLED
|
||||||
Loading…
Reference in New Issue
Block a user