mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 15:23:57 -04:00
AP_ExternalAHRS: Add MicroStrain7 support
* Implement filter state without relying on GPS * Implement health and initialization check based on filter state * Fix pre-arm checks missing version * Add version specifics to microstrain 5 log msgs * Add utilities to microstrain packet to remove magic number array access Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman
Andrew Tridgell
parent
433f98fee1
commit
7c183f4ab3
@ -24,6 +24,7 @@
|
||||
#include "AP_ExternalAHRS_backend.h"
|
||||
#include "AP_ExternalAHRS_VectorNav.h"
|
||||
#include "AP_ExternalAHRS_MicroStrain5.h"
|
||||
#include "AP_ExternalAHRS_MicroStrain7.h"
|
||||
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
@ -53,7 +54,7 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = {
|
||||
// @Param: _TYPE
|
||||
// @DisplayName: AHRS type
|
||||
// @Description: Type of AHRS device
|
||||
// @Values: 0:None,1:VectorNav,2:MicroStrain
|
||||
// @Values: 0:None,1:VectorNav,2:MicroStrain5,7:MicroStrain7
|
||||
// @User: Standard
|
||||
AP_GROUPINFO_FLAGS("_TYPE", 1, AP_ExternalAHRS, devtype, HAL_EXTERNAL_AHRS_DEFAULT, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
@ -102,6 +103,11 @@ void AP_ExternalAHRS::init(void)
|
||||
case DevType::MicroStrain5:
|
||||
backend = new AP_ExternalAHRS_MicroStrain5(this, state);
|
||||
return;
|
||||
#endif
|
||||
#if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
|
||||
case DevType::MicroStrain7:
|
||||
backend = new AP_ExternalAHRS_MicroStrain7(this, state);
|
||||
return;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -53,7 +53,9 @@ public:
|
||||
// 4 reserved for CINS
|
||||
// 5 reserved for InertialLabs
|
||||
// 6 reserved for Trimble
|
||||
// 7 reserved for MicroStrain7
|
||||
#if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
|
||||
MicroStrain7 = 7,
|
||||
#endif
|
||||
// 8 reserved for SBG
|
||||
// 9 reserved for EulerNav
|
||||
};
|
||||
|
||||
@ -34,6 +34,8 @@
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
static constexpr uint8_t gnss_instance = 0;
|
||||
|
||||
AP_ExternalAHRS_MicroStrain5::AP_ExternalAHRS_MicroStrain5(AP_ExternalAHRS *_frontend,
|
||||
AP_ExternalAHRS::state_t &_state): AP_ExternalAHRS_backend(_frontend, _state)
|
||||
{
|
||||
@ -44,16 +46,16 @@ AP_ExternalAHRS_MicroStrain5::AP_ExternalAHRS_MicroStrain5(AP_ExternalAHRS *_fro
|
||||
port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0);
|
||||
|
||||
if (!uart) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS no UART");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "MicroStrain5 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");
|
||||
AP_BoardConfig::allocation_error("MicroStrain5 failed to allocate ExternalAHRS update thread");
|
||||
}
|
||||
|
||||
hal.scheduler->delay(5000);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain ExternalAHRS initialised");
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain5 ExternalAHRS initialised");
|
||||
}
|
||||
|
||||
void AP_ExternalAHRS_MicroStrain5::update_thread(void)
|
||||
@ -92,8 +94,10 @@ void AP_ExternalAHRS_MicroStrain5::build_packet()
|
||||
post_imu();
|
||||
break;
|
||||
case DescriptorSet::GNSSData:
|
||||
case DescriptorSet::GNSSRecv1:
|
||||
case DescriptorSet::GNSSRecv2:
|
||||
break;
|
||||
case DescriptorSet::EstimationData:
|
||||
case DescriptorSet::FilterData:
|
||||
post_filter();
|
||||
break;
|
||||
case DescriptorSet::BaseCommand:
|
||||
@ -157,26 +161,26 @@ void AP_ExternalAHRS_MicroStrain5::post_filter() const
|
||||
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.location = Location{filter_data.lat, filter_data.lon, gnss_data[gnss_instance].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,
|
||||
fix_type: (uint8_t) gnss_data[gnss_instance].fix_type,
|
||||
satellites_in_view: gnss_data[gnss_instance].satellites,
|
||||
|
||||
horizontal_pos_accuracy: gnss_data.horizontal_position_accuracy,
|
||||
vertical_pos_accuracy: gnss_data.vertical_position_accuracy,
|
||||
horizontal_vel_accuracy: gnss_data.speed_accuracy,
|
||||
horizontal_pos_accuracy: gnss_data[gnss_instance].horizontal_position_accuracy,
|
||||
vertical_pos_accuracy: gnss_data[gnss_instance].vertical_position_accuracy,
|
||||
horizontal_vel_accuracy: gnss_data[gnss_instance].speed_accuracy,
|
||||
|
||||
hdop: gnss_data.hdop,
|
||||
vdop: gnss_data.vdop,
|
||||
hdop: gnss_data[gnss_instance].hdop,
|
||||
vdop: gnss_data[gnss_instance].vdop,
|
||||
|
||||
longitude: filter_data.lon,
|
||||
latitude: filter_data.lat,
|
||||
msl_altitude: gnss_data.msl_altitude,
|
||||
msl_altitude: gnss_data[gnss_instance].msl_altitude,
|
||||
|
||||
ned_vel_north: filter_data.ned_velocity_north,
|
||||
ned_vel_east: filter_data.ned_velocity_east,
|
||||
@ -187,14 +191,14 @@ void AP_ExternalAHRS_MicroStrain5::post_filter() const
|
||||
WITH_SEMAPHORE(state.sem);
|
||||
state.origin = Location{int32_t(filter_data.lat),
|
||||
int32_t(filter_data.lon),
|
||||
int32_t(gnss_data.msl_altitude),
|
||||
int32_t(gnss_data[gnss_instance].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);
|
||||
uint8_t gps_instance;
|
||||
if (AP::gps().get_first_external_instance(gps_instance)) {
|
||||
AP::gps().handle_external(gps, gps_instance);
|
||||
}
|
||||
}
|
||||
|
||||
@ -215,26 +219,26 @@ const char* AP_ExternalAHRS_MicroStrain5::get_name() const
|
||||
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);
|
||||
return (now - last_imu_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;
|
||||
return last_imu_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");
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain5 unhealthy");
|
||||
return false;
|
||||
}
|
||||
if (gnss_data.fix_type < 3) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain no GPS lock");
|
||||
if (gnss_data[gnss_instance].fix_type < 3) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain5 no GPS lock");
|
||||
return false;
|
||||
}
|
||||
if (filter_status.state != 0x02) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain filter not running");
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain5 filter not running");
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -244,15 +248,15 @@ bool AP_ExternalAHRS_MicroStrain5::pre_arm_check(char *failure_msg, uint8_t fail
|
||||
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) {
|
||||
if (last_imu_pkt != 0 && last_gps_pkt != 0) {
|
||||
status.flags.initalized = true;
|
||||
}
|
||||
if (healthy() && last_ins_pkt != 0) {
|
||||
if (healthy() && last_imu_pkt != 0) {
|
||||
status.flags.attitude = true;
|
||||
status.flags.vert_vel = true;
|
||||
status.flags.vert_pos = true;
|
||||
|
||||
if (gnss_data.fix_type >= 3) {
|
||||
if (gnss_data[gnss_instance].fix_type >= 3) {
|
||||
status.flags.horiz_vel = true;
|
||||
status.flags.horiz_pos_rel = true;
|
||||
status.flags.horiz_pos_abs = true;
|
||||
@ -309,7 +313,7 @@ void AP_ExternalAHRS_MicroStrain5::send_status_report(GCS_MAVLINK &link) const
|
||||
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,
|
||||
gnss_data[gnss_instance].speed_accuracy/vel_gate, gnss_data[gnss_instance].horizontal_position_accuracy/pos_gate, gnss_data[gnss_instance].vertical_position_accuracy/hgt_gate,
|
||||
mag_var, 0, 0);
|
||||
|
||||
}
|
||||
|
||||
379
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp
Normal file
379
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp
Normal file
@ -0,0 +1,379 @@
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
/*
|
||||
Support for MicroStrain GQ7 serially connected AHRS Systems
|
||||
Usage in SITL with hardware for debugging:
|
||||
$ sim_vehicle.py -v Plane -A "--serial3=uart:/dev/3dm-gq7" --console --map -DG
|
||||
$ ./Tools/autotest/sim_vehicle.py -v Plane -A "--serial3=uart:/dev/3dm-gq7" -DG
|
||||
param set AHRS_EKF_TYPE 11
|
||||
param set EAHRS_TYPE 4
|
||||
param set GPS_TYPE 21
|
||||
param set SERIAL3_BAUD 115
|
||||
param set SERIAL3_PROTOCOL 36
|
||||
UDEV rules for repeatable USB connection:
|
||||
$ cat /etc/udev/rules.d/99-usb-serial.rules
|
||||
SUBSYSTEM=="tty", ATTRS{manufacturer}=="Lord Microstrain", SYMLINK+="3dm-gq7"
|
||||
Usage with simulated MicroStrain7:
|
||||
./Tools/autotest/sim_vehicle.py -v Plane -A "--serial3=sim:MicroStrain7" --console --map -DG
|
||||
*/
|
||||
|
||||
#define ALLOW_DOUBLE_MATH_FUNCTIONS
|
||||
|
||||
#include "AP_ExternalAHRS_config.h"
|
||||
|
||||
#if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
|
||||
|
||||
#include "AP_ExternalAHRS_MicroStrain7.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>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
AP_ExternalAHRS_MicroStrain7::AP_ExternalAHRS_MicroStrain7(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, "MicroStrain7 ExternalAHRS no UART");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_MicroStrain7::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) {
|
||||
AP_BoardConfig::allocation_error("MicroStrain7 failed to allocate ExternalAHRS update thread");
|
||||
}
|
||||
|
||||
hal.scheduler->delay(5000);
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain7 ExternalAHRS initialised");
|
||||
}
|
||||
|
||||
void AP_ExternalAHRS_MicroStrain7::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_MicroStrain7::build_packet()
|
||||
{
|
||||
if (uart == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
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:
|
||||
case DescriptorSet::GNSSRecv1:
|
||||
case DescriptorSet::GNSSRecv2:
|
||||
break;
|
||||
case DescriptorSet::FilterData:
|
||||
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_MicroStrain7::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;
|
||||
}
|
||||
|
||||
{
|
||||
// *INDENT-OFF*
|
||||
AP_ExternalAHRS::ins_data_message_t ins {
|
||||
accel: imu_data.accel,
|
||||
gyro: imu_data.gyro,
|
||||
temperature: -300
|
||||
};
|
||||
// *INDENT-ON*
|
||||
AP::ins().handle_external(ins);
|
||||
}
|
||||
|
||||
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
{
|
||||
// *INDENT-OFF*
|
||||
AP_ExternalAHRS::mag_data_message_t mag {
|
||||
field: imu_data.mag
|
||||
};
|
||||
// *INDENT-ON*
|
||||
AP::compass().handle_external(mag);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if AP_BARO_EXTERNALAHRS_ENABLED
|
||||
{
|
||||
// *INDENT-OFF*
|
||||
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,
|
||||
};
|
||||
// *INDENT-ON*
|
||||
AP::baro().handle_external(baro);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void AP_ExternalAHRS_MicroStrain7::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;
|
||||
|
||||
// TODO the filter does not supply MSL altitude.
|
||||
// The GNSS system has both MSL and WGS-84 ellipsoid height.
|
||||
// Use GNSS 0 even though it may be bad.
|
||||
state.location = Location{filter_data.lat, filter_data.lon, gnss_data[0].msl_altitude, Location::AltFrame::ABSOLUTE};
|
||||
state.have_location = true;
|
||||
}
|
||||
|
||||
for (int instance = 0; instance < NUM_GNSS_INSTANCES; instance++) {
|
||||
// *INDENT-OFF*
|
||||
AP_ExternalAHRS::gps_data_message_t gps {
|
||||
gps_week: filter_data.week,
|
||||
ms_tow: filter_data.tow_ms,
|
||||
fix_type: (uint8_t) gnss_data[instance].fix_type,
|
||||
satellites_in_view: gnss_data[instance].satellites,
|
||||
|
||||
horizontal_pos_accuracy: gnss_data[instance].horizontal_position_accuracy,
|
||||
vertical_pos_accuracy: gnss_data[instance].vertical_position_accuracy,
|
||||
horizontal_vel_accuracy: gnss_data[instance].speed_accuracy,
|
||||
|
||||
hdop: gnss_data[instance].hdop,
|
||||
vdop: gnss_data[instance].vdop,
|
||||
|
||||
longitude: filter_data.lon,
|
||||
latitude: filter_data.lat,
|
||||
msl_altitude: gnss_data[instance].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,
|
||||
};
|
||||
// *INDENT-ON*
|
||||
|
||||
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[instance].msl_altitude),
|
||||
Location::AltFrame::ABSOLUTE};
|
||||
state.have_origin = true;
|
||||
}
|
||||
AP::gps().handle_external(gps, instance);
|
||||
}
|
||||
}
|
||||
|
||||
int8_t AP_ExternalAHRS_MicroStrain7::get_port(void) const
|
||||
{
|
||||
if (!uart) {
|
||||
return -1;
|
||||
}
|
||||
return port_num;
|
||||
};
|
||||
|
||||
// Get model/type name
|
||||
const char* AP_ExternalAHRS_MicroStrain7::get_name() const
|
||||
{
|
||||
return "MICROSTRAIN7";
|
||||
}
|
||||
|
||||
bool AP_ExternalAHRS_MicroStrain7::healthy(void) const
|
||||
{
|
||||
uint32_t now = AP_HAL::millis();
|
||||
|
||||
// Expect the following rates:
|
||||
// * Navigation Filter: 25Hz = 40mS
|
||||
// * GPS: 2Hz = 500mS
|
||||
// * IMU: 25Hz = 40mS
|
||||
|
||||
// Allow for some slight variance of 10%
|
||||
constexpr float RateFoS = 1.1;
|
||||
|
||||
constexpr uint32_t expected_filter_time_delta_ms = 40;
|
||||
constexpr uint32_t expected_gps_time_delta_ms = 500;
|
||||
constexpr uint32_t expected_imu_time_delta_ms = 40;
|
||||
|
||||
const bool times_healthy = (now - last_imu_pkt < expected_imu_time_delta_ms * RateFoS && \
|
||||
now - last_gps_pkt < expected_gps_time_delta_ms * RateFoS && \
|
||||
now - last_filter_pkt < expected_filter_time_delta_ms * RateFoS);
|
||||
const auto filter_state = static_cast<FilterState>(filter_status.state);
|
||||
const bool filter_healthy = (filter_state == FilterState::GQ7_FULL_NAV || filter_state == FilterState::GQ7_AHRS);
|
||||
return times_healthy && filter_healthy;
|
||||
}
|
||||
|
||||
bool AP_ExternalAHRS_MicroStrain7::initialised(void) const
|
||||
{
|
||||
const bool got_packets = last_imu_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0;
|
||||
const auto filter_state = static_cast<FilterState>(filter_status.state);
|
||||
const bool filter_healthy = filter_state_healthy(filter_state);
|
||||
return got_packets && filter_healthy;
|
||||
}
|
||||
|
||||
bool AP_ExternalAHRS_MicroStrain7::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
|
||||
{
|
||||
if (!healthy()) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain7 unhealthy");
|
||||
return false;
|
||||
}
|
||||
// TODO is this necessary? hard coding the first instance.
|
||||
if (gnss_data[0].fix_type < 3) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain7 no GPS lock");
|
||||
return false;
|
||||
}
|
||||
if (!filter_state_healthy(FilterState(filter_status.state))) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain7 filter not running");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void AP_ExternalAHRS_MicroStrain7::get_filter_status(nav_filter_status &status) const
|
||||
{
|
||||
memset(&status, 0, sizeof(status));
|
||||
if (last_imu_pkt != 0 && last_gps_pkt != 0) {
|
||||
status.flags.initalized = true;
|
||||
}
|
||||
if (healthy() && last_imu_pkt != 0) {
|
||||
status.flags.attitude = true;
|
||||
status.flags.vert_vel = true;
|
||||
status.flags.vert_pos = true;
|
||||
|
||||
const auto filter_state = static_cast<FilterState>(filter_status.state);
|
||||
if (filter_state_healthy(filter_state)) {
|
||||
status.flags.horiz_vel = true;
|
||||
status.flags.horiz_pos_rel = true;
|
||||
status.flags.horiz_pos_abs = true;
|
||||
status.flags.pred_horiz_pos_rel = true;
|
||||
status.flags.pred_horiz_pos_abs = true;
|
||||
status.flags.using_gps = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AP_ExternalAHRS_MicroStrain7::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
|
||||
|
||||
// TODO fix to use NED filter speed accuracy instead of first gnss
|
||||
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_ned_vel_uncertainty.htm
|
||||
mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
|
||||
gnss_data[0].speed_accuracy/vel_gate, gnss_data[0].horizontal_position_accuracy/pos_gate, gnss_data[0].vertical_position_accuracy/hgt_gate,
|
||||
mag_var, 0, 0);
|
||||
|
||||
}
|
||||
|
||||
bool AP_ExternalAHRS_MicroStrain7::filter_state_healthy(FilterState state)
|
||||
{
|
||||
switch (state) {
|
||||
case FilterState::GQ7_FULL_NAV:
|
||||
case FilterState::GQ7_AHRS:
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
// return state == FilterState::GQ7_FULL_NAV || state == FilterState::GQ7_AHRS;
|
||||
}
|
||||
|
||||
#endif // AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
|
||||
88
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h
Normal file
88
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h
Normal file
@ -0,0 +1,88 @@
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
/*
|
||||
support for MicroStrain GQ7 serially connected AHRS Systems
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "AP_ExternalAHRS_config.h"
|
||||
|
||||
#if AP_EXTERNAL_AHRS_MICROSTRAIN7_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_MicroStrain7: public AP_ExternalAHRS_backend, public AP_MicroStrain
|
||||
{
|
||||
public:
|
||||
|
||||
AP_ExternalAHRS_MicroStrain7(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:
|
||||
|
||||
// GQ7 Filter States
|
||||
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_status.htm
|
||||
enum class FilterState {
|
||||
GQ7_INIT = 0x01,
|
||||
GQ7_VERT_GYRO = 0x02,
|
||||
GQ7_AHRS = 0x03,
|
||||
GQ7_FULL_NAV = 0x04
|
||||
};
|
||||
|
||||
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();
|
||||
|
||||
// Only some of the fix types satisfy a healthy filter.
|
||||
// GQ7_VERT_GYRO is NOT considered healthy for now.
|
||||
// This may be vehicle-dependent in the future.
|
||||
static bool filter_state_healthy(FilterState state) WARN_IF_UNUSED;
|
||||
|
||||
AP_HAL::UARTDriver *uart;
|
||||
HAL_Semaphore sem;
|
||||
|
||||
};
|
||||
|
||||
#endif // AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
|
||||
@ -14,6 +14,10 @@
|
||||
#define AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED
|
||||
#endif
|
||||
|
||||
#ifndef AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
|
||||
#define AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED
|
||||
#endif
|
||||
|
||||
#ifndef AP_MICROSTRAIN_ENABLED
|
||||
#define AP_MICROSTRAIN_ENABLED AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
|
||||
#endif
|
||||
|
||||
@ -23,35 +23,43 @@
|
||||
#include "MicroStrain_common.h"
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
|
||||
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/sensor_data/sensor_data_links.htm
|
||||
enum class INSPacketField {
|
||||
ACCEL = 0x04,
|
||||
GYRO = 0x05,
|
||||
QUAT = 0x0A,
|
||||
MAG = 0x06,
|
||||
PRESSURE = 0x17
|
||||
PRESSURE = 0x17,
|
||||
};
|
||||
|
||||
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/gnss_recv_1_links.htm
|
||||
enum class GNSSPacketField {
|
||||
LLH_POSITION = 0x03,
|
||||
NED_VELOCITY = 0x05,
|
||||
DOP_DATA = 0x07,
|
||||
GPS_TIME = 0x09,
|
||||
FIX_INFO = 0x0B
|
||||
FIX_INFO = 0x0B,
|
||||
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm
|
||||
GPS_TIMESTAMP = 0xD3,
|
||||
};
|
||||
|
||||
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_fix_info.htm
|
||||
enum class GNSSFixType {
|
||||
FIX_3D = 0x00,
|
||||
FIX_2D = 0x01,
|
||||
TIME_ONLY = 0x02,
|
||||
NONE = 0x03,
|
||||
INVALID = 0x04
|
||||
INVALID = 0x04,
|
||||
FIX_RTK_FLOAT = 0x05,
|
||||
FIX_RTK_FIXED = 0x06,
|
||||
};
|
||||
|
||||
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/filter_data_links.htm
|
||||
enum class FilterPacketField {
|
||||
FILTER_STATUS = 0x10,
|
||||
GPS_TIME = 0x11,
|
||||
LLH_POSITION = 0x01,
|
||||
NED_VELOCITY = 0x02
|
||||
NED_VELOCITY = 0x02,
|
||||
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm
|
||||
GPS_TIMESTAMP = 0xD3,
|
||||
};
|
||||
|
||||
bool AP_MicroStrain::handle_byte(const uint8_t b, DescriptorSet& descriptor)
|
||||
@ -72,17 +80,17 @@ bool AP_MicroStrain::handle_byte(const uint8_t b, DescriptorSet& descriptor)
|
||||
}
|
||||
break;
|
||||
case ParseState::WaitingFor_Descriptor:
|
||||
message_in.packet.header[2] = b;
|
||||
message_in.packet.descriptor_set(b);
|
||||
message_in.state = ParseState::WaitingFor_PayloadLength;
|
||||
break;
|
||||
case ParseState::WaitingFor_PayloadLength:
|
||||
message_in.packet.header[3] = b;
|
||||
message_in.packet.payload_length(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]) {
|
||||
if (message_in.index >= message_in.packet.payload_length()) {
|
||||
message_in.state = ParseState::WaitingFor_Checksum;
|
||||
message_in.index = 0;
|
||||
}
|
||||
@ -113,7 +121,7 @@ bool AP_MicroStrain::valid_packet(const MicroStrain_Packet & packet)
|
||||
checksum_two += checksum_one;
|
||||
}
|
||||
|
||||
for (int i = 0; i < packet.header[3]; i++) {
|
||||
for (int i = 0; i < packet.payload_length(); i++) {
|
||||
checksum_one += packet.payload[i];
|
||||
checksum_two += checksum_one;
|
||||
}
|
||||
@ -123,21 +131,23 @@ bool AP_MicroStrain::valid_packet(const MicroStrain_Packet & packet)
|
||||
|
||||
AP_MicroStrain::DescriptorSet AP_MicroStrain::handle_packet(const MicroStrain_Packet& packet)
|
||||
{
|
||||
const DescriptorSet descriptor = DescriptorSet(packet.header[2]);
|
||||
const DescriptorSet descriptor = packet.descriptor_set();
|
||||
switch (descriptor) {
|
||||
case DescriptorSet::IMUData:
|
||||
handle_imu(packet);
|
||||
break;
|
||||
case DescriptorSet::GNSSData:
|
||||
handle_gnss(packet);
|
||||
break;
|
||||
case DescriptorSet::EstimationData:
|
||||
case DescriptorSet::FilterData:
|
||||
handle_filter(packet);
|
||||
break;
|
||||
case DescriptorSet::BaseCommand:
|
||||
case DescriptorSet::DMCommand:
|
||||
case DescriptorSet::SystemCommand:
|
||||
break;
|
||||
case DescriptorSet::GNSSData:
|
||||
case DescriptorSet::GNSSRecv1:
|
||||
case DescriptorSet::GNSSRecv2:
|
||||
handle_gnss(packet);
|
||||
break;
|
||||
}
|
||||
return descriptor;
|
||||
}
|
||||
@ -145,10 +155,10 @@ AP_MicroStrain::DescriptorSet AP_MicroStrain::handle_packet(const MicroStrain_Pa
|
||||
|
||||
void AP_MicroStrain::handle_imu(const MicroStrain_Packet& packet)
|
||||
{
|
||||
last_ins_pkt = AP_HAL::millis();
|
||||
last_imu_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]) {
|
||||
for (uint8_t i = 0; i < packet.payload_length(); i += packet.payload[i]) {
|
||||
switch ((INSPacketField) packet.payload[i+1]) {
|
||||
// Scaled Ambient Pressure
|
||||
case INSPacketField::PRESSURE: {
|
||||
@ -183,63 +193,71 @@ void AP_MicroStrain::handle_imu(const MicroStrain_Packet& packet)
|
||||
void AP_MicroStrain::handle_gnss(const MicroStrain_Packet &packet)
|
||||
{
|
||||
last_gps_pkt = AP_HAL::millis();
|
||||
uint8_t gnss_instance;
|
||||
const DescriptorSet descriptor = DescriptorSet(packet.descriptor_set());
|
||||
if (!get_gnss_instance(descriptor, gnss_instance)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Iterate through fields of varying lengths in GNSS packet
|
||||
for (uint8_t i = 0; i < packet.header[3]; i += packet.payload[i]) {
|
||||
for (uint8_t i = 0; i < packet.payload_length(); 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]);
|
||||
case GNSSPacketField::GPS_TIMESTAMP: {
|
||||
gnss_data[gnss_instance].tow_ms = double_to_uint32(be64todouble_ptr(packet.payload, i+2) * 1000); // Convert seconds to ms
|
||||
gnss_data[gnss_instance].week = be16toh_ptr(&packet.payload[i+10]);
|
||||
break;
|
||||
}
|
||||
// GNSS Fix Information
|
||||
case GNSSPacketField::FIX_INFO: {
|
||||
switch ((GNSSFixType) packet.payload[i+2]) {
|
||||
case (GNSSFixType::FIX_RTK_FLOAT): {
|
||||
gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_RTK_FLOAT;
|
||||
break;
|
||||
}
|
||||
case (GNSSFixType::FIX_RTK_FIXED): {
|
||||
gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_RTK_FIXED;
|
||||
break;
|
||||
}
|
||||
case (GNSSFixType::FIX_3D): {
|
||||
gnss_data.fix_type = GPS_FIX_TYPE_3D_FIX;
|
||||
gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_3D_FIX;
|
||||
break;
|
||||
}
|
||||
case (GNSSFixType::FIX_2D): {
|
||||
gnss_data.fix_type = GPS_FIX_TYPE_2D_FIX;
|
||||
gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_2D_FIX;
|
||||
break;
|
||||
}
|
||||
case (GNSSFixType::TIME_ONLY):
|
||||
case (GNSSFixType::NONE): {
|
||||
gnss_data.fix_type = GPS_FIX_TYPE_NO_FIX;
|
||||
gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_NO_FIX;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
case (GNSSFixType::INVALID): {
|
||||
gnss_data.fix_type = GPS_FIX_TYPE_NO_GPS;
|
||||
gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_NO_GPS;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
gnss_data.satellites = packet.payload[i+3];
|
||||
gnss_data[gnss_instance].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);
|
||||
gnss_data[gnss_instance].lat = be64todouble_ptr(packet.payload, i+2) * 1.0e7; // Decimal degrees to degrees
|
||||
gnss_data[gnss_instance].lon = be64todouble_ptr(packet.payload, i+10) * 1.0e7;
|
||||
gnss_data[gnss_instance].msl_altitude = be64todouble_ptr(packet.payload, i+26) * 1.0e2; // Meters to cm
|
||||
gnss_data[gnss_instance].horizontal_position_accuracy = be32tofloat_ptr(packet.payload, i+34);
|
||||
gnss_data[gnss_instance].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);
|
||||
gnss_data[gnss_instance].hdop = be32tofloat_ptr(packet.payload, i+10);
|
||||
gnss_data[gnss_instance].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);
|
||||
gnss_data[gnss_instance].ned_velocity_north = be32tofloat_ptr(packet.payload, i+2);
|
||||
gnss_data[gnss_instance].ned_velocity_east = be32tofloat_ptr(packet.payload, i+6);
|
||||
gnss_data[gnss_instance].ned_velocity_down = be32tofloat_ptr(packet.payload, i+10);
|
||||
gnss_data[gnss_instance].speed_accuracy = be32tofloat_ptr(packet.payload, i+26);
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -251,29 +269,25 @@ 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]) {
|
||||
for (uint8_t i = 0; i < packet.payload_length(); i += packet.payload[i]) {
|
||||
switch ((FilterPacketField) packet.payload[i+1]) {
|
||||
// GPS Timestamp
|
||||
case FilterPacketField::GPS_TIME: {
|
||||
case FilterPacketField::GPS_TIMESTAMP: {
|
||||
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]);
|
||||
@ -303,5 +317,25 @@ Quaternion AP_MicroStrain::populate_quaternion(const uint8_t *data, uint8_t offs
|
||||
};
|
||||
}
|
||||
|
||||
bool AP_MicroStrain::get_gnss_instance(const DescriptorSet& descriptor, uint8_t& instance){
|
||||
bool success = false;
|
||||
|
||||
switch(descriptor) {
|
||||
case DescriptorSet::GNSSData:
|
||||
case DescriptorSet::GNSSRecv1:
|
||||
instance = 0;
|
||||
success = true;
|
||||
break;
|
||||
case DescriptorSet::GNSSRecv2:
|
||||
instance = 1;
|
||||
success = true;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif // AP_MICROSTRAIN_ENABLED
|
||||
@ -40,19 +40,6 @@ protected:
|
||||
WaitingFor_Checksum
|
||||
};
|
||||
|
||||
// A MicroStrain packet can be a maximum of 261 bytes
|
||||
struct MicroStrain_Packet {
|
||||
uint8_t header[4];
|
||||
uint8_t payload[255];
|
||||
uint8_t checksum[2];
|
||||
};
|
||||
|
||||
struct {
|
||||
MicroStrain_Packet packet;
|
||||
AP_MicroStrain::ParseState state;
|
||||
uint8_t index;
|
||||
} message_in;
|
||||
|
||||
struct {
|
||||
Vector3f accel;
|
||||
Vector3f gyro;
|
||||
@ -61,6 +48,8 @@ protected:
|
||||
float pressure;
|
||||
} imu_data;
|
||||
|
||||
static constexpr uint8_t NUM_GNSS_INSTANCES = 2;
|
||||
|
||||
struct {
|
||||
uint16_t week;
|
||||
uint32_t tow_ms;
|
||||
@ -77,7 +66,7 @@ protected:
|
||||
float ned_velocity_east;
|
||||
float ned_velocity_down;
|
||||
float speed_accuracy;
|
||||
} gnss_data;
|
||||
} gnss_data[NUM_GNSS_INSTANCES];
|
||||
|
||||
struct {
|
||||
uint16_t state;
|
||||
@ -105,13 +94,47 @@ protected:
|
||||
SystemCommand = 0x7F,
|
||||
IMUData = 0x80,
|
||||
GNSSData = 0x81,
|
||||
EstimationData = 0x82
|
||||
FilterData = 0x82,
|
||||
GNSSRecv1 = 0x91,
|
||||
GNSSRecv2 = 0x92
|
||||
};
|
||||
|
||||
const uint8_t SYNC_ONE = 0x75;
|
||||
const uint8_t SYNC_TWO = 0x65;
|
||||
|
||||
uint32_t last_ins_pkt;
|
||||
struct MicroStrain_Packet {
|
||||
uint8_t header[4];
|
||||
uint8_t payload[255];
|
||||
uint8_t checksum[2];
|
||||
|
||||
// Gets the payload length
|
||||
uint8_t payload_length() const WARN_IF_UNUSED {
|
||||
return header[3];
|
||||
}
|
||||
|
||||
// Sets the payload length
|
||||
void payload_length(const uint8_t len) {
|
||||
header[3] = len;
|
||||
}
|
||||
|
||||
// Gets the descriptor set
|
||||
DescriptorSet descriptor_set() const WARN_IF_UNUSED {
|
||||
return DescriptorSet(header[2]);
|
||||
}
|
||||
|
||||
// Sets the descriptor set (without validation)
|
||||
void descriptor_set(const uint8_t descriptor_set) {
|
||||
header[2] = descriptor_set;
|
||||
}
|
||||
};
|
||||
|
||||
struct {
|
||||
MicroStrain_Packet packet;
|
||||
AP_MicroStrain::ParseState state;
|
||||
uint8_t index;
|
||||
} message_in;
|
||||
|
||||
uint32_t last_imu_pkt;
|
||||
uint32_t last_gps_pkt;
|
||||
uint32_t last_filter_pkt;
|
||||
|
||||
@ -129,6 +152,8 @@ protected:
|
||||
void handle_filter(const MicroStrain_Packet &packet);
|
||||
static Vector3f populate_vector3f(const uint8_t* data, uint8_t offset);
|
||||
static Quaternion populate_quaternion(const uint8_t* data, uint8_t offset);
|
||||
// Depending on the descriptor, the data corresponds to a different GNSS instance.
|
||||
static bool get_gnss_instance(const DescriptorSet& descriptor, uint8_t& instance);
|
||||
};
|
||||
|
||||
#endif // AP_MICROSTRAIN_ENABLED
|
||||
|
||||
Loading…
Reference in New Issue
Block a user