diff --git a/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS7/ap1.txt b/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS7/ap1.txt new file mode 100644 index 0000000000000..ab2be0d1a4547 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS7/ap1.txt @@ -0,0 +1,7 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.090027 1 +1 0 3 22 0.000000 0.000000 0.000000 0.000000 -35.361553 149.163956 20.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364540 149.162857 50.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.367333 149.163164 28.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366814 149.165878 28.000000 1 +5 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.362947 149.165179 0.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 31a127f3e0117..fb78e2c186b53 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3275,6 +3275,10 @@ def MicroStrainEAHRS5(self): '''Test MicroStrain EAHRS series 5 support''' self.fly_external_AHRS("MicroStrain5", 2, "ap1.txt") + def MicroStrainEAHRS7(self): + '''Test MicroStrain EAHRS series 7 support''' + self.fly_external_AHRS("MicroStrain7", 7, "ap1.txt") + def get_accelvec(self, m): return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81 @@ -5353,6 +5357,7 @@ def tests(self): self.TerrainLoiter, self.VectorNavEAHRS, self.MicroStrainEAHRS5, + self.MicroStrainEAHRS7, self.Deadreckoning, self.DeadreckoningNoAirSpeed, self.EKFlaneswitch, diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index dfbef411a7822..7dc8f69cfe6a8 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -32,6 +32,7 @@ def __init__(self, Feature('AHRS', 'EKF2', 'HAL_NAVEKF2_AVAILABLE', 'Enable EKF2', 0, None), Feature('AHRS', 'AHRS_EXT', 'HAL_EXTERNAL_AHRS_ENABLED', 'Enable External AHRS', 0, None), Feature('AHRS', 'AHRS_EXT_MICROSTRAIN5', 'AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED', 'Enable MICROSTRAIN 5-series External AHRS', 0, "AHRS_EXT"), # noqa: E501 + Feature('AHRS', 'AHRS_EXT_MICROSTRAIN7', 'AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED', 'Enable MICROSTRAIN 7-series External AHRS', 0, "AHRS_EXT"), # noqa: E501 Feature('AHRS', 'AHRS_EXT_VECTORNAV', 'AP_EXTERNAL_AHRS_VECTORNAV_ENABLED', 'Enable VectorNav External AHRS', 0, "AHRS_EXT"), # noqa Feature('AHRS', 'TEMPCAL', 'HAL_INS_TEMPERATURE_CAL_ENABLE', 'Enable IMU Temperature Calibration', 0, None), Feature('AHRS', 'VISUALODOM', 'HAL_VISUALODOM_ENABLED', 'Enable Visual Odometry', 0, 'EKF3_EXTNAV'), diff --git a/Tools/scripts/run_astyle.py b/Tools/scripts/run_astyle.py index bf6ff415c97c4..4e495994735b5 100755 --- a/Tools/scripts/run_astyle.py +++ b/Tools/scripts/run_astyle.py @@ -22,9 +22,14 @@ def __init__(self, *, dry_run=DRY_RUN_DEFAULT): self.retcode = 0 self.directories_to_check = [ 'libraries/AP_DDS', - 'libraries/AP_ExternalControl' + 'libraries/AP_ExternalControl', + ] + self.files_to_check = [ + pathlib.Path(s) for s in [ + 'libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp', + 'libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h', + ] ] - self.files_to_check = [] self.dry_run = dry_run def progress(self, string): diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index c1769fe2a00a8..f75d6ac1d4fd7 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -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 @@ -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 } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index debf2d27fecbe..c825b0f609b6b 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -53,7 +53,9 @@ class AP_ExternalAHRS { // 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 }; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp index 47bffb606a1e2..141b4ff857e7c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp @@ -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); } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp new file mode 100644 index 0000000000000..1abf9436355e0 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp @@ -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 . + */ +/* + 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 +#include +#include +#include +#include +#include +#include +#include +#include + +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(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(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(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 diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h new file mode 100644 index 0000000000000..d9e0832272715 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h @@ -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 . + */ +/* + 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 +#include +#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 diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h index ef9ef52158833..3b2caa2387046 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h @@ -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 diff --git a/libraries/AP_ExternalAHRS/MicroStrain_common.cpp b/libraries/AP_ExternalAHRS/MicroStrain_common.cpp index d5b723cb125c6..66d324df99554 100644 --- a/libraries/AP_ExternalAHRS/MicroStrain_common.cpp +++ b/libraries/AP_ExternalAHRS/MicroStrain_common.cpp @@ -23,35 +23,43 @@ #include "MicroStrain_common.h" #include +// 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 \ No newline at end of file diff --git a/libraries/AP_ExternalAHRS/MicroStrain_common.h b/libraries/AP_ExternalAHRS/MicroStrain_common.h index 37158d7c70dfa..b71027720dd9b 100644 --- a/libraries/AP_ExternalAHRS/MicroStrain_common.h +++ b/libraries/AP_ExternalAHRS/MicroStrain_common.h @@ -40,19 +40,6 @@ class AP_MicroStrain 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 @@ class AP_MicroStrain float pressure; } imu_data; + static constexpr uint8_t NUM_GNSS_INSTANCES = 2; + struct { uint16_t week; uint32_t tow_ms; @@ -77,7 +66,7 @@ class AP_MicroStrain 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 @@ class AP_MicroStrain 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 @@ class AP_MicroStrain 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 diff --git a/libraries/AP_HAL/SIMState.h b/libraries/AP_HAL/SIMState.h index af62741110f2d..41505a6263d90 100644 --- a/libraries/AP_HAL/SIMState.h +++ b/libraries/AP_HAL/SIMState.h @@ -195,6 +195,9 @@ class AP_HAL::SIMState { // simulated MicroStrain Series 5 system SITL::MicroStrain5 *microstrain5; + // simulated MicroStrain Series 7 system + SITL::MicroStrain7 *microstrain7; + #if HAL_SIM_JSON_MASTER_ENABLED // Ride along instances via JSON SITL backend SITL::JSON_Master ride_along; diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index 260a9b28dac6f..718d80537e182 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -269,6 +269,13 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const } microstrain5 = new SITL::MicroStrain5(); return microstrain5; + + } else if (streq(name, "MicroStrain7")) { + if (microstrain7 != nullptr) { + AP_HAL::panic("Only one MicroStrain7 at a time"); + } + microstrain7 = new SITL::MicroStrain7(); + return microstrain7; #if HAL_SIM_AIS_ENABLED } else if (streq(name, "AIS")) { if (ais != nullptr) { @@ -435,6 +442,10 @@ void SITL_State_Common::sim_update(void) microstrain5->update(); } + if (microstrain7 != nullptr) { + microstrain7->update(); + } + #if HAL_SIM_AIS_ENABLED if (ais != nullptr) { ais->update(); diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index e7b82c2c91d4a..3a2d722fc5ebd 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -197,9 +197,12 @@ class HALSITL::SITL_State_Common { // simulated VectorNav system: SITL::VectorNav *vectornav; - // simulated LORD MicroStrain system + // simulated MicroStrain system SITL::MicroStrain5 *microstrain5; + // simulated MicroStrain system + SITL::MicroStrain7 *microstrain7; + #if HAL_SIM_JSON_MASTER_ENABLED // Ride along instances via JSON SITL backend SITL::JSON_Master ride_along; diff --git a/libraries/SITL/SIM_MicroStrain.cpp b/libraries/SITL/SIM_MicroStrain.cpp index ce126f878bd97..f4ec6c78f6881 100644 --- a/libraries/SITL/SIM_MicroStrain.cpp +++ b/libraries/SITL/SIM_MicroStrain.cpp @@ -13,7 +13,7 @@ along with this program. If not, see . */ /* - simulate LORD MicroStrain serial device + simulate MicroStrain GNSS-INS devices */ #include "SIM_MicroStrain.h" #include @@ -24,15 +24,12 @@ using namespace SITL; -MicroStrain5::MicroStrain5() :SerialDevice::SerialDevice() +MicroStrain::MicroStrain() :SerialDevice::SerialDevice() { } -/* - get timeval using simulation time - */ -static void simulation_timeval(struct timeval *tv) +void MicroStrain::simulation_timeval(struct timeval *tv) { uint64_t now = AP_HAL::micros64(); static uint64_t first_usec; @@ -48,7 +45,7 @@ static void simulation_timeval(struct timeval *tv) tv->tv_usec = new_usec % 1000000ULL; } -void MicroStrain5::generate_checksum(MicroStrain_Packet& packet) +void MicroStrain::generate_checksum(MicroStrain_Packet& packet) { uint8_t checksumByte1 = 0; uint8_t checksumByte2 = 0; @@ -67,7 +64,7 @@ void MicroStrain5::generate_checksum(MicroStrain_Packet& packet) packet.checksum[1] = checksumByte2; } -void MicroStrain5::send_packet(MicroStrain_Packet packet) +void MicroStrain::send_packet(MicroStrain_Packet packet) { generate_checksum(packet); @@ -77,7 +74,7 @@ void MicroStrain5::send_packet(MicroStrain_Packet packet) } -void MicroStrain5::send_imu_packet(void) +void MicroStrain::send_imu_packet(void) { const auto &fdm = _sitl->state; MicroStrain_Packet packet; @@ -135,7 +132,6 @@ void MicroStrain5::send_imu_packet(void) send_packet(packet); } - void MicroStrain5::send_gnss_packet(void) { const auto &fdm = _sitl->state; @@ -148,9 +144,9 @@ void MicroStrain5::send_gnss_packet(void) packet.header[1] = 0x65; // Sync Two packet.header[2] = 0x81; // GNSS Descriptor - // Add GPS Time + // Add GPS Timestamp packet.payload[packet.payload_size++] = 0x0E; // GPS Time Field Size - packet.payload[packet.payload_size++] = 0x09; // Descriptor + packet.payload[packet.payload_size++] = 0xD3; // Descriptor put_double(packet, (double) tv.tv_sec); put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL)); put_int(packet, 0); @@ -205,6 +201,8 @@ void MicroStrain5::send_gnss_packet(void) send_packet(packet); } + + void MicroStrain5::send_filter_packet(void) { const auto &fdm = _sitl->state; @@ -217,9 +215,9 @@ void MicroStrain5::send_filter_packet(void) packet.header[1] = 0x65; // Sync Two packet.header[2] = 0x82; // Filter Descriptor - // Add Filter Time - packet.payload[packet.payload_size++] = 0x0E; // Filter Time Field Size - packet.payload[packet.payload_size++] = 0x11; // Descriptor + // Add GPS Timestamp Shared Data + packet.payload[packet.payload_size++] = 0x0E; // GPS Timestamp Field Size + packet.payload[packet.payload_size++] = 0xD3; // Descriptor put_double(packet, (double) tv.tv_usec / 1e6); put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL)); put_int(packet, 0x0001); @@ -256,34 +254,34 @@ void MicroStrain5::send_filter_packet(void) /* send MicroStrain data */ -void MicroStrain5::update(void) +void MicroStrain::update(void) { if (!init_sitl_pointer()) { return; } - uint32_t us_between_imu_packets = 20000; - uint32_t us_between_gnss_packets = 250000; - uint32_t us_between_filter_packets = 100000; + uint32_t ms_between_imu_packets = 40; + uint32_t ms_between_gnss_packets = 500; + uint32_t ms_between_filter_packets = 40; - uint32_t now = AP_HAL::micros(); - if (now - last_imu_pkt_us >= us_between_imu_packets) { - last_imu_pkt_us = now; + uint32_t now = AP_HAL::millis(); + if (now - last_imu_pkt_ms >= ms_between_imu_packets) { + last_imu_pkt_ms = now; send_imu_packet(); } - if (now - last_gnss_pkt_us >= us_between_gnss_packets) { - last_gnss_pkt_us = now; + if (now - last_gnss_pkt_ms >= ms_between_gnss_packets) { + last_gnss_pkt_ms = now; send_gnss_packet(); } - if (now - last_filter_pkt_us >= us_between_filter_packets) { - last_filter_pkt_us = now; + if (now - last_filter_pkt_ms >= ms_between_filter_packets) { + last_filter_pkt_ms = now; send_filter_packet(); } } -void MicroStrain5::put_float(MicroStrain_Packet &packet, float f) +void MicroStrain::put_float(MicroStrain_Packet &packet, float f) { uint32_t fbits = 0; memcpy(&fbits, &f, sizeof(fbits)); @@ -291,7 +289,7 @@ void MicroStrain5::put_float(MicroStrain_Packet &packet, float f) packet.payload_size += sizeof(float); } -void MicroStrain5::put_double(MicroStrain_Packet &packet, double d) +void MicroStrain::put_double(MicroStrain_Packet &packet, double d) { uint64_t dbits = 0; memcpy(&dbits, &d, sizeof(dbits)); @@ -299,7 +297,7 @@ void MicroStrain5::put_double(MicroStrain_Packet &packet, double d) packet.payload_size += sizeof(double); } -void MicroStrain5::put_int(MicroStrain_Packet &packet, uint16_t t) +void MicroStrain::put_int(MicroStrain_Packet &packet, uint16_t t) { put_be16_ptr(&packet.payload[packet.payload_size], t); packet.payload_size += sizeof(uint16_t); diff --git a/libraries/SITL/SIM_MicroStrain.h b/libraries/SITL/SIM_MicroStrain.h index ebed27a4dae81..bb2d3ebe73325 100644 --- a/libraries/SITL/SIM_MicroStrain.h +++ b/libraries/SITL/SIM_MicroStrain.h @@ -4,9 +4,9 @@ //PARAMS: // param set AHRS_EKF_TYPE 11 // param set EAHRS_TYPE 2 -// param set SERIAL4_PROTOCOL 36 -// param set SERIAL4_BAUD 115 -// sim_vehicle.py -v ArduPlane -D --console --map -A --uartE=sim:MicroStrain +// param set SERIAL3_PROTOCOL 36 +// param set SERIAL3_BAUD 115 +// sim_vehicle.py -v Plane -A "--serial3=sim:MicroStrain7" --console --map -DG #pragma once #include "SIM_Aircraft.h" @@ -17,16 +17,17 @@ namespace SITL { -class MicroStrain5 : public SerialDevice +class MicroStrain : public SerialDevice { + // This class implements the common MicroStrain driver support. public: - MicroStrain5(); + MicroStrain(); // update state void update(void); -private: +protected: struct MicroStrain_Packet { uint8_t header[4]; uint8_t payload[256]; @@ -35,23 +36,44 @@ class MicroStrain5 : public SerialDevice size_t payload_size = 0; }; - uint32_t last_imu_pkt_us; - uint32_t last_gnss_pkt_us; - uint32_t last_filter_pkt_us; + uint32_t last_imu_pkt_ms; + uint32_t last_gnss_pkt_ms; + uint32_t last_filter_pkt_ms; void generate_checksum(MicroStrain_Packet&); void send_packet(MicroStrain_Packet); void send_imu_packet(); - void send_gnss_packet(); - void send_filter_packet(); + virtual void send_gnss_packet() = 0; + virtual void send_filter_packet() = 0; void put_float(MicroStrain_Packet&, float); void put_double(MicroStrain_Packet&, double); void put_int(MicroStrain_Packet&, uint16_t); + // get timeval using simulation time + static void simulation_timeval(struct timeval *tv); + uint64_t start_us; }; +class MicroStrain5 : public MicroStrain +{ + // This is a specialization for the 3DM-GX5-GNSS/INS +private: + void send_gnss_packet() override; + void send_filter_packet() override; + +}; + +class MicroStrain7 : public MicroStrain +{ + // This is a specialization for the 3DM-GQ7-GNSS/INS +private: + void send_gnss_packet() override; + void send_filter_packet() override; + +}; + } diff --git a/libraries/SITL/SIM_MicroStrain7.cpp b/libraries/SITL/SIM_MicroStrain7.cpp new file mode 100644 index 0000000000000..4664942049e78 --- /dev/null +++ b/libraries/SITL/SIM_MicroStrain7.cpp @@ -0,0 +1,147 @@ +/* + 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 . + */ +/* + simulate MicroStrain 7-series GNSS-INS devices +*/ + +#include "SIM_MicroStrain.h" + +using namespace SITL; + +void MicroStrain7::send_gnss_packet(void) +{ + const auto &fdm = _sitl->state; + + constexpr uint8_t descriptors[2] = {0x91, 0x92}; + for (uint8_t i = 0; i < ARRAY_SIZE(descriptors); i++) { + MicroStrain_Packet packet; + + struct timeval tv; + simulation_timeval(&tv); + + packet.header[0] = 0x75; // Sync One + packet.header[1] = 0x65; // Sync Two + packet.header[2] = descriptors[i]; // GNSS Descriptor + + // Add GPS Timestamp + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm + packet.payload[packet.payload_size++] = 0x0E; // GPS Time Field Size + packet.payload[packet.payload_size++] = 0xD3; // Descriptor + put_double(packet, (double) tv.tv_sec); + put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL)); + put_int(packet, 0); + + // Add GNSS Fix Information + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_fix_info.htm + packet.payload[packet.payload_size++] = 0x08; // GNSS Fix Field Size + packet.payload[packet.payload_size++] = 0x0B; // Descriptor + packet.payload[packet.payload_size++] = 0x00; // Fix type FIX_3D + packet.payload[packet.payload_size++] = 19; // Sat count + put_int(packet, 0); // Fix flags + put_int(packet, 0); // Valid flags + + // Add GNSS LLH position + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_llh_pos.htm + packet.payload[packet.payload_size++] = 0x2C; // GNSS LLH Field Size + packet.payload[packet.payload_size++] = 0x03; // Descriptor + put_double(packet, fdm.latitude); + put_double(packet, fdm.longitude); + put_double(packet, 0); // Height above ellipsoid - unused + put_double(packet, fdm.altitude); + put_float(packet, 0.5f); // Horizontal accuracy + put_float(packet, 0.5f); // Vertical accuracy + put_int(packet, 31); // Valid flags + + // Add DOP Data + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_dop.htm + packet.payload[packet.payload_size++] = 0x20; // DOP Field Size + packet.payload[packet.payload_size++] = 0x07; // Descriptor + put_float(packet, 0); // GDOP + put_float(packet, 0); // PDOP + put_float(packet, 0); // HDOP + put_float(packet, 0); // VDOP + put_float(packet, 0); // TDOP + put_float(packet, 0); // NDOP + put_float(packet, 0); // EDOP + put_int(packet, 127); + + // Add GNSS NED velocity + packet.payload[packet.payload_size++] = 0x24; // GNSS NED Velocity Field Size + packet.payload[packet.payload_size++] = 0x05; // Descriptor + put_float(packet, fdm.speedN); + put_float(packet, fdm.speedE); + put_float(packet, fdm.speedD); + put_float(packet, 0); //speed - unused + put_float(packet, 0); //ground speed - unused + put_float(packet, 0); //heading - unused + put_float(packet, 0.25f); //speed accuracy + put_float(packet, 0); //heading accuracy - unused + put_int(packet, 31); //valid flags + + packet.header[3] = packet.payload_size; + + send_packet(packet); + } + +} + +void MicroStrain7::send_filter_packet(void) +{ + const auto &fdm = _sitl->state; + MicroStrain_Packet packet; + + struct timeval tv; + simulation_timeval(&tv); + + packet.header[0] = 0x75; // Sync One + packet.header[1] = 0x65; // Sync Two + packet.header[2] = 0x82; // Filter Descriptor + + // Add GPS Timestamp Shared Data + packet.payload[packet.payload_size++] = 0x0E; // GPS Timestamp Field Size + packet.payload[packet.payload_size++] = 0xD3; // Descriptor + put_double(packet, (double) tv.tv_usec / 1e6); + put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL)); + put_int(packet, 0x0001); + + // Add GNSS Filter velocity + packet.payload[packet.payload_size++] = 0x10; // GNSS Velocity Field Size + packet.payload[packet.payload_size++] = 0x02; // Descriptor + put_float(packet, fdm.speedN); + put_float(packet, fdm.speedE); + put_float(packet, fdm.speedD); + put_int(packet, 0x0001); + + // Add Filter LLH position + packet.payload[packet.payload_size++] = 0x1C; // Filter LLH Field Size + packet.payload[packet.payload_size++] = 0x01; // Descriptor + put_double(packet, fdm.latitude); + put_double(packet, fdm.longitude); + put_double(packet, 0); // Height above ellipsoid - unused + put_int(packet, 0x0001); // Valid flags + + // Add Filter State + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_status.htm + packet.payload[packet.payload_size++] = 0x08; // Filter State Field Size + packet.payload[packet.payload_size++] = 0x10; // Descriptor + put_int(packet, 0x04); // Filter state (GQ7_FULL_NAV) + put_int(packet, 0x03); // Dynamics mode (Airborne) + put_int(packet, 0); // Filter flags (None, no warnings) + + packet.header[3] = packet.payload_size; + + + send_packet(packet); +}