diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 683918f8b7..40c739d7ae 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -23,7 +23,7 @@ #include "AP_ExternalAHRS.h" #include "AP_ExternalAHRS_backend.h" #include "AP_ExternalAHRS_VectorNav.h" -#include "AP_ExternalAHRS_MicroStrain.h" +#include "AP_ExternalAHRS_MicroStrain5.h" #include @@ -98,9 +98,9 @@ void AP_ExternalAHRS::init(void) backend = new AP_ExternalAHRS_VectorNav(this, state); break; #endif -#if AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED - case DevType::MicroStrain: - backend = new AP_ExternalAHRS_MicroStrain(this, state); +#if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED + case DevType::MicroStrain5: + backend = new AP_ExternalAHRS_MicroStrain5(this, state); break; default: #endif diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index 992a49373d..9541db5672 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -46,8 +46,8 @@ public: #if AP_EXTERNAL_AHRS_VECTORNAV_ENABLED VecNav = 1, #endif -#if AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED - MicroStrain = 2, +#if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED + MicroStrain5 = 2, #endif }; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain.cpp deleted file mode 100644 index cc12359fad..0000000000 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain.cpp +++ /dev/null @@ -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 . - */ -/* - 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 -#include -#include -#include -#include -#include -#include -#include -#include - - -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 diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp new file mode 100644 index 0000000000..6a4217bd52 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp @@ -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 . + */ +/* + 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 +#include +#include +#include +#include +#include +#include +#include + +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 diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h new file mode 100644 index 0000000000..0b0575a608 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h @@ -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 . + */ +/* + 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 +#include +#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 diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h index bd35d7b438..ef9ef52158 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h @@ -10,8 +10,12 @@ #define AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED HAL_EXTERNAL_AHRS_ENABLED #endif -#ifndef AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED -#define AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED +#ifndef AP_EXTERNAL_AHRS_MICROSTRAIN5_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 #ifndef AP_EXTERNAL_AHRS_VECTORNAV_ENABLED diff --git a/libraries/AP_ExternalAHRS/MicroStrain_common.cpp b/libraries/AP_ExternalAHRS/MicroStrain_common.cpp new file mode 100644 index 0000000000..567ac7d70d --- /dev/null +++ b/libraries/AP_ExternalAHRS/MicroStrain_common.cpp @@ -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 . + */ +/* + 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 + +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 \ No newline at end of file diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain.h b/libraries/AP_ExternalAHRS/MicroStrain_common.h similarity index 62% rename from libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain.h rename to libraries/AP_ExternalAHRS/MicroStrain_common.h index 39401afc7b..506063b583 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain.h +++ b/libraries/AP_ExternalAHRS/MicroStrain_common.h @@ -11,43 +11,25 @@ along with this program. If not, see . */ /* - suppport for serial connected AHRS systems + suppport for MicroStrain MIP parsing */ #pragma once #include "AP_ExternalAHRS_config.h" -#if AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED +#if AP_MICROSTRAIN_ENABLED -#include "AP_ExternalAHRS_backend.h" #include +#include +#include -class AP_ExternalAHRS_MicroStrain: public AP_ExternalAHRS_backend +class AP_MicroStrain { public: - AP_ExternalAHRS_MicroStrain(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: +protected: enum class ParseState { WaitingFor_SyncOne, @@ -58,23 +40,7 @@ private: WaitingFor_Checksum }; - void update_thread(); - - 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 + // A MicroStrain packet can be a maximum of 261 bytes struct MicroStrain_Packet { uint8_t header[4]; uint8_t payload[255]; @@ -83,7 +49,7 @@ private: struct { MicroStrain_Packet packet; - ParseState state; + AP_MicroStrain::ParseState state; uint8_t index; } message_in; @@ -133,19 +99,36 @@ private: float speed_accuracy; } filter_data; - void build_packet(); - bool valid_packet(const MicroStrain_Packet &packet) const; - void handle_packet(const MicroStrain_Packet &packet); + enum class DescriptorSet { + BaseCommand = 0x01, + 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); + // Collects data from a gnss packet into `gnss_data` void handle_gnss(const MicroStrain_Packet &packet); void handle_filter(const MicroStrain_Packet &packet); - void post_imu() const; - void post_gnss() const; - 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; - + static Vector3f populate_vector3f(const uint8_t* data, uint8_t offset); + static Quaternion populate_quaternion(const uint8_t* data, uint8_t offset); }; -#endif // AP_EXTERNAL_AHRS_MICROSTRAIN_ENABLED +#endif // AP_MICROSTRAIN_ENABLED