From b4bf4efe3ef78b4bd11b7e938cf8efb1a13125a7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Dec 2023 09:00:30 +1100 Subject: [PATCH] AP_ExternalAHRS: added InertialLabs backend --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 6 + libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 9 +- .../AP_ExternalAHRS_InertialLabs.cpp | 659 ++++++++++++++++++ .../AP_ExternalAHRS_InertialLabs.h | 226 ++++++ .../AP_ExternalAHRS/AP_ExternalAHRS_config.h | 4 + 5 files changed, 903 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index d7368fd8c39055..109ada6995bd01 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_InertialLabs.h" #include #include @@ -103,6 +104,11 @@ void AP_ExternalAHRS::init(void) case DevType::MicroStrain5: backend = new AP_ExternalAHRS_MicroStrain5(this, state); return; +#endif +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + case DevType::InertialLabs: + backend = new AP_ExternalAHRS_InertialLabs(this, state); + return; #endif } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index 1c20985fb2bb04..ab5506952cc097 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -48,10 +48,12 @@ class AP_ExternalAHRS { #endif #if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED MicroStrain5 = 2, +#endif +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + InertialLabs = 5, #endif // 3 reserved for AdNav // 4 reserved for CINS - // 5 reserved for InertialLabs // 6 reserved for Trimble // 7 reserved for MicroStrain7 // 8 reserved for SBG @@ -154,6 +156,11 @@ class AP_ExternalAHRS { float temperature; } ins_data_message_t; + typedef struct { + float differential_pressure; // Pa + float temperature; // degC + } airspeed_data_message_t; + protected: enum class OPTIONS { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp new file mode 100644 index 00000000000000..8149e62adf0e99 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -0,0 +1,659 @@ +/* + 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 serial connected InertialLabs INS + */ + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + +#include "AP_ExternalAHRS_InertialLabs.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL &hal; + +// constructor +AP_ExternalAHRS_InertialLabs::AP_ExternalAHRS_InertialLabs(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); + if (!uart) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs ExternalAHRS no UART"); + return; + } + baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0); + port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); + + // don't offer IMU by default, at 200Hz it is too slow for many aircraft + set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::GPS) | + uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) | + uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS)); + + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_InertialLabs::update_thread, void), "ILabs", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { + AP_HAL::panic("InertialLabs Failed to start ExternalAHRS update thread"); + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs ExternalAHRS initialised"); +} + +/* + re-sync buffer on parse failure + */ +void AP_ExternalAHRS_InertialLabs::re_sync(void) +{ + if (buffer_ofs > 3) { + /* + look for the 2 byte header and try to sync to that + */ + const uint16_t header = 0x55AA; + const uint8_t *p = (const uint8_t *)memmem(&buffer[1], buffer_ofs-3, &header, sizeof(header)); + if (p != nullptr) { + const uint16_t n = p - &buffer[0]; + memmove(&buffer[0], p, buffer_ofs - n); + buffer_ofs -= n; + } else { + buffer_ofs = 0; + } + } else { + buffer_ofs = 0; + } +} + +// macro for checking we don't run past end of message buffer +#define CHECK_SIZE(d) need_re_sync = (message_ofs + (msg_len=sizeof(d)) > buffer_end); if (need_re_sync) break + +// lookup a message in the msg_types bitmask to see if we received it in this packet +#define GOT_MSG(mtype) msg_types.get(unsigned(MessageType::mtype)) + +/* + check header is valid + */ +bool AP_ExternalAHRS_InertialLabs::check_header(const ILabsHeader *h) const +{ + return h->magic == 0x55AA && + h->msg_type == 1 && + h->msg_id == 0x95 && + h->msg_len <= sizeof(buffer)-2; +} + +/* + check the UART for more data + returns true if we have consumed potentially valid bytes + */ +bool AP_ExternalAHRS_InertialLabs::check_uart() +{ + WITH_SEMAPHORE(state.sem); + + if (!setup_complete) { + return false; + } + uint32_t n = uart->available(); + if (n == 0) { + return false; + } + if (n + buffer_ofs > sizeof(buffer)) { + n = sizeof(buffer) - buffer_ofs; + } + const ILabsHeader *h = (ILabsHeader *)&buffer[0]; + + if (buffer_ofs < sizeof(ILabsHeader)) { + n = MIN(n, sizeof(ILabsHeader)-buffer_ofs); + } else { + if (!check_header(h)) { + re_sync(); + return false; + } + if (buffer_ofs > h->msg_len+8) { + re_sync(); + return false; + } + n = MIN(n, uint32_t(h->msg_len + 2 - buffer_ofs)); + } + + const ssize_t nread = uart->read(&buffer[buffer_ofs], n); + if (nread != ssize_t(n)) { + re_sync(); + return false; + } + buffer_ofs += n; + + if (buffer_ofs < sizeof(ILabsHeader)) { + return true; + } + + if (!check_header(h)) { + re_sync(); + return false; + } + + if (buffer_ofs < h->msg_len+2) { + /* + see if we can read the rest immediately + */ + const uint16_t needed = h->msg_len+2 - buffer_ofs; + if (uart->available() < needed) { + // need more data + return true; + } + const ssize_t nread2 = uart->read(&buffer[buffer_ofs], needed); + if (nread2 != needed) { + re_sync(); + return false; + } + buffer_ofs += nread2; + } + + // check checksum + const uint16_t crc1 = crc_sum_of_bytes_16(&buffer[2], buffer_ofs-4); + const uint16_t crc2 = le16toh_ptr(&buffer[buffer_ofs-2]); + if (crc1 != crc2) { + re_sync(); + return false; + } + + const uint8_t *buffer_end = &buffer[buffer_ofs]; + const uint16_t payload_size = h->msg_len - 6; + const uint8_t *payload = &buffer[6]; + if (payload_size < 3) { + re_sync(); + return false; + } + const uint8_t num_messages = payload[0]; + if (num_messages == 0 || + num_messages > payload_size-1) { + re_sync(); + return false; + } + const uint8_t *message_ofs = &payload[num_messages+1]; + bool need_re_sync = false; + + // bitmask for what types we get + Bitmask<256> msg_types; + uint32_t now_ms = AP_HAL::millis(); + + for (uint8_t i=0; i= buffer_end) { + re_sync(); + return false; + } + MessageType mtype = (MessageType)payload[1+i]; + ILabsData &u = *(ILabsData*)message_ofs; + uint8_t msg_len = 0; + + msg_types.set(unsigned(mtype)); + + switch (mtype) { + case MessageType::GPS_INS_TIME_MS: { + // this is the GPS tow timestamp in ms for when the IMU data was sampled + CHECK_SIZE(u.gps_time_ms); + state2.gnss_ins_time_ms = u.gps_time_ms; + break; + } + case MessageType::GPS_WEEK: { + CHECK_SIZE(u.gps_week); + gps_data.gps_week = u.gps_week; + break; + } + case MessageType::ACCEL_DATA_HR: { + CHECK_SIZE(u.accel_data_hr); + ins_data.accel = u.accel_data_hr.tofloat().rfu_to_frd()*GRAVITY_MSS*1.0e-6; + break; + } + case MessageType::GYRO_DATA_HR: { + CHECK_SIZE(u.gyro_data_hr); + ins_data.gyro = u.gyro_data_hr.tofloat().rfu_to_frd()*DEG_TO_RAD*1.0e-5; + break; + } + case MessageType::BARO_DATA: { + CHECK_SIZE(u.baro_data); + baro_data.pressure_pa = u.baro_data.pressure_pa2*2; + break; + } + case MessageType::MAG_DATA: { + CHECK_SIZE(u.mag_data); + mag_data.field = u.mag_data.tofloat().rfu_to_frd()*(10*NTESLA_TO_MGAUSS); + break; + } + case MessageType::ORIENTATION_ANGLES: { + CHECK_SIZE(u.orientation_angles); + state.quat.from_euler(radians(u.orientation_angles.roll*0.01), + radians(u.orientation_angles.pitch*0.01), + radians(u.orientation_angles.yaw*0.01)); + state.have_quaternion = true; + if (last_att_ms == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: got link"); + } + last_att_ms = now_ms; + break; + } + case MessageType::VELOCITIES: { + CHECK_SIZE(u.velocity); + state.velocity = u.velocity.tofloat().rfu_to_frd()*0.01; + state.have_velocity = true; + last_vel_ms = now_ms; + break; + } + case MessageType::POSITION: { + CHECK_SIZE(u.position); + state.location.lat = u.position.lat; + state.location.lng = u.position.lon; + state.location.alt = u.position.alt; + state.have_location = true; + state.last_location_update_us = AP_HAL::micros(); + last_pos_ms = now_ms; + break; + } + case MessageType::KF_VEL_COVARIANCE: { + CHECK_SIZE(u.kf_vel_covariance); + state2.kf_vel_covariance = u.kf_vel_covariance.tofloat() * 0.001; + break; + } + case MessageType::KF_POS_COVARIANCE: { + CHECK_SIZE(u.kf_pos_covariance); + state2.kf_pos_covariance = u.kf_pos_covariance.tofloat() * 0.001; + break; + } + case MessageType::UNIT_STATUS: { + CHECK_SIZE(u.unit_status); + state2.unit_status = u.unit_status; + break; + } + case MessageType::GNSS_EXTENDED_INFO: { + CHECK_SIZE(u.gnss_extended_info); + gps_data.fix_type = u.gnss_extended_info.fix_type+1; + state2.gnss_extended_info = u.gnss_extended_info; + break; + } + case MessageType::NUM_SATS: { + CHECK_SIZE(u.num_sats); + gps_data.satellites_in_view = u.num_sats; + break; + } + case MessageType::GNSS_POSITION: { + CHECK_SIZE(u.position); + gps_data.latitude = u.position.lat; + gps_data.longitude = u.position.lon; + gps_data.msl_altitude = u.position.alt; + break; + } + case MessageType::GNSS_VEL_TRACK: { + CHECK_SIZE(u.gnss_vel_track); + Vector2f velxy; + velxy.offset_bearing(u.gnss_vel_track.track_over_ground*0.01, u.gnss_vel_track.hor_speed*0.01); + gps_data.ned_vel_north = velxy.x; + gps_data.ned_vel_east = velxy.y; + gps_data.ned_vel_down = -u.gnss_vel_track.ver_speed*0.01; + break; + } + case MessageType::GNSS_POS_TIMESTAMP: { + CHECK_SIZE(u.gnss_pos_timestamp); + gps_data.ms_tow = u.gnss_pos_timestamp; + break; + } + case MessageType::GNSS_INFO_SHORT: { + CHECK_SIZE(u.gnss_info_short); + state2.gnss_info_short = u.gnss_info_short; + break; + } + case MessageType::GNSS_NEW_DATA: { + CHECK_SIZE(u.gnss_new_data); + state2.gnss_new_data = u.gnss_new_data; + break; + } + case MessageType::GNSS_JAM_STATUS: { + CHECK_SIZE(u.gnss_jam_status); + state2.gnss_jam_status = u.gnss_jam_status; + break; + } + case MessageType::DIFFERENTIAL_PRESSURE: { + CHECK_SIZE(u.differential_pressure); + airspeed_data.differential_pressure = u.differential_pressure*1.0e-4; + break; + } + case MessageType::TRUE_AIRSPEED: { + CHECK_SIZE(u.true_airspeed); + state2.true_airspeed = u.true_airspeed*0.01; + break; + } + case MessageType::WIND_SPEED: { + CHECK_SIZE(u.wind_speed); + state2.wind_speed = u.wind_speed.tofloat().rfu_to_frd()*0.01; + break; + } + case MessageType::AIR_DATA_STATUS: { + CHECK_SIZE(u.air_data_status); + state2.air_data_status = u.air_data_status; + break; + } + case MessageType::SUPPLY_VOLTAGE: { + CHECK_SIZE(u.supply_voltage); + state2.supply_voltage = u.supply_voltage*0.01; + break; + } + case MessageType::TEMPERATURE: { + CHECK_SIZE(u.temperature); + // assume same temperature for baro and airspeed + baro_data.temperature = u.temperature*0.1; + airspeed_data.temperature = u.temperature*0.1; + break; + } + case MessageType::UNIT_STATUS2: { + CHECK_SIZE(u.unit_status2); + state2.unit_status2 = u.unit_status2; + break; + } + } + + if (msg_len == 0) { + // got an unknown message + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: unknown msg 0x%02x", unsigned(mtype)); + buffer_ofs = 0; + return false; + } + message_ofs += msg_len; + + if (msg_len == 0 || need_re_sync) { + re_sync(); + return false; + } + } + + if (h->msg_len != message_ofs-buffer) { + // we had stray bytes at the end of the message + re_sync(); + return false; + } + + if (GOT_MSG(ACCEL_DATA_HR) && + GOT_MSG(GYRO_DATA_HR)) { + AP::ins().handle_external(ins_data); + } + if (GOT_MSG(GPS_INS_TIME_MS) && + GOT_MSG(NUM_SATS) && + GOT_MSG(GNSS_POSITION) && + GOT_MSG(GNSS_NEW_DATA) && + GOT_MSG(GNSS_EXTENDED_INFO) && + state2.gnss_new_data != 0) { + uint8_t instance; + if (AP::gps().get_first_external_instance(instance)) { + AP::gps().handle_external(gps_data, instance); + } + if (gps_data.satellites_in_view > 3) { + if (last_gps_ms == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: got GPS lock"); + if (!state.have_origin) { + state.origin = Location{ + gps_data.latitude, + gps_data.longitude, + gps_data.msl_altitude, + Location::AltFrame::ABSOLUTE}; + state.have_origin = true; + } + } + last_gps_ms = now_ms; + } + } + if (GOT_MSG(BARO_DATA) && + GOT_MSG(TEMPERATURE)) { + AP::baro().handle_external(baro_data); + } + if (GOT_MSG(MAG_DATA)) { + AP::compass().handle_external(mag_data); + } +#if AP_AIRSPEED_EXTERNAL_ENABLED && (APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)) + // only on plane and copter as others do not link AP_Airspeed + if (GOT_MSG(DIFFERENTIAL_PRESSURE) && + GOT_MSG(TEMPERATURE)) { + auto *arsp = AP::airspeed(); + if (arsp != nullptr) { + arsp->handle_external(airspeed_data); + } + } +#endif // AP_AIRSPEED_EXTERNAL_ENABLED + buffer_ofs = 0; + + if (GOT_MSG(POSITION) && + GOT_MSG(ORIENTATION_ANGLES) && + GOT_MSG(VELOCITIES)) { + + float roll, pitch, yaw; + state.quat.to_euler(roll, pitch, yaw); + uint64_t now_us = AP_HAL::micros64(); + + // @LoggerMessage: ILB1 + // @Description: InertialLabs AHRS data1 + // @Field: TimeUS: Time since system startup + // @Field: Roll: euler roll + // @Field: Pitch: euler pitch + // @Field: Yaw: euler yaw + // @Field: VN: velocity north + // @Field: VE: velocity east + // @Field: VD: velocity down + // @Field: Lat: latitude + // @Field: Lon: longitude + // @Field: Alt: altitude AMSL + + AP::logger().WriteStreaming("ILB1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt", + "sdddnnnDUm", + "F000000GG0", + "QffffffLLf", + now_us, + degrees(roll), degrees(pitch), degrees(yaw), + state.velocity.x, state.velocity.y, state.velocity.z, + state.location.lat, state.location.lng, state.location.alt*0.01); + + // @LoggerMessage: ILB2 + // @Description: InertialLabs AHRS data2 + // @Field: TimeUS: Time since system startup + // @Field: PosVarN: position variance north + // @Field: PosVarE: position variance east + // @Field: PosVarD: position variance down + // @Field: VelVarN: velocity variance north + // @Field: VelVarE: velocity variance east + // @Field: VelVarD: velocity variance down + + AP::logger().WriteStreaming("ILB2", "TimeUS,PosVarN,PosVarE,PosVarD,VelVarN,VelVarE,VelVarD", + "smmmnnn", + "F000000", + "Qffffff", + now_us, + state2.kf_pos_covariance.x, state2.kf_pos_covariance.x, state2.kf_pos_covariance.z, + state2.kf_vel_covariance.x, state2.kf_vel_covariance.x, state2.kf_vel_covariance.z); + + // @LoggerMessage: ILB3 + // @Description: InertialLabs AHRS data3 + // @Field: TimeUS: Time since system startup + // @Field: Stat1: unit status1 + // @Field: Stat2: unit status2 + // @Field: FType: fix type + // @Field: SpStat: spoofing status + // @Field: GI1: GNSS Info1 + // @Field: GI2: GNSS Info2 + // @Field: GJS: GNSS jamming status + // @Field: TAS: true airspeed + // @Field: WVN: Wind velocity north + // @Field: WVE: Wind velocity east + // @Field: WVD: Wind velocity down + + AP::logger().WriteStreaming("ILB3", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD", + "s-----------", + "F-----------", + "QHHBBBBBffff", + now_us, + state2.unit_status, state2.unit_status2, + state2.gnss_extended_info.fix_type, state2.gnss_extended_info.spoofing_status, + state2.gnss_info_short.info1, state2.gnss_info_short.info2, + state2.gnss_jam_status, + state2.true_airspeed, + state2.wind_speed.x, state2.wind_speed.y, state2.wind_speed.z); + } + + return true; +} + +void AP_ExternalAHRS_InertialLabs::update_thread() +{ + // Open port in the thread + uart->begin(baudrate, 1024, 512); + + /* + we assume the user has already configured the device + */ + + setup_complete = true; + while (true) { + if (!check_uart()) { + hal.scheduler->delay_microseconds(250); + } + } +} + +// get serial port number for the uart +int8_t AP_ExternalAHRS_InertialLabs::get_port(void) const +{ + if (!uart) { + return -1; + } + return port_num; +}; + +// accessors for AP_AHRS +bool AP_ExternalAHRS_InertialLabs::healthy(void) const +{ + WITH_SEMAPHORE(state.sem); + return AP_HAL::millis() - last_att_ms < 100; +} + +bool AP_ExternalAHRS_InertialLabs::initialised(void) const +{ + if (!setup_complete) { + return false; + } + return true; +} + +bool AP_ExternalAHRS_InertialLabs::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const +{ + if (!setup_complete) { + hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs setup failed"); + return false; + } + if (!healthy()) { + hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs unhealthy"); + return false; + } + WITH_SEMAPHORE(state.sem); + uint32_t now = AP_HAL::millis(); + if (now - last_att_ms > 10 || + now - last_pos_ms > 10 || + now - last_vel_ms > 10) { + hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs not up to date"); + return false; + } + return true; +} + +/* + get filter status. We don't know the meaning of the status bits yet, + so assume all OK if we have GPS lock + */ +void AP_ExternalAHRS_InertialLabs::get_filter_status(nav_filter_status &status) const +{ + WITH_SEMAPHORE(state.sem); + uint32_t now = AP_HAL::millis(); + const uint32_t dt_limit = 200; + const uint32_t dt_limit_gps = 500; + memset(&status, 0, sizeof(status)); + status.flags.initalized = last_att_ms != 0; + status.flags.attitude = now - last_att_ms < dt_limit; + status.flags.vert_vel = now - last_vel_ms < dt_limit; + status.flags.vert_pos = now - last_pos_ms < dt_limit; + status.flags.horiz_vel = status.flags.vert_vel; + status.flags.horiz_pos_abs = status.flags.vert_pos; + status.flags.horiz_pos_rel = status.flags.horiz_pos_abs; + status.flags.pred_horiz_pos_rel = status.flags.horiz_pos_abs; + status.flags.pred_horiz_pos_abs = status.flags.horiz_pos_abs; + status.flags.using_gps = now - last_gps_ms < dt_limit_gps; +} + +// send an EKF_STATUS message to GCS +void AP_ExternalAHRS_InertialLabs::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 = 5; + const float pos_gate = 5; + const float hgt_gate = 5; + const float mag_var = 0; + mavlink_msg_ekf_status_report_send(link.get_chan(), flags, + state2.kf_vel_covariance.length()/vel_gate, + state2.kf_pos_covariance.xy().length()/pos_gate, + state2.kf_pos_covariance.z/hgt_gate, + mag_var, 0, 0); +} + +#endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h new file mode 100644 index 00000000000000..1071eda49bf29d --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h @@ -0,0 +1,226 @@ +/* + 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 serial connected InertialLabs INS system + */ + +#pragma once + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + +#include "AP_ExternalAHRS_backend.h" + +class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { + +public: + AP_ExternalAHRS_InertialLabs(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state); + + // get serial port number, -1 for not enabled + int8_t get_port(void) 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 { + check_uart(); + } + + // Get model/type name + const char* get_name() const override { + return "ILabs"; + } + + enum class MessageType : uint8_t { + GPS_INS_TIME_MS = 0x01, + GPS_WEEK = 0x3C, + ACCEL_DATA_HR = 0x23, + GYRO_DATA_HR = 0x21, + BARO_DATA = 0x25, + MAG_DATA = 0x24, + ORIENTATION_ANGLES = 0x07, + VELOCITIES = 0x12, + POSITION = 0x10, + KF_VEL_COVARIANCE = 0x58, + KF_POS_COVARIANCE = 0x57, + UNIT_STATUS = 0x53, + GNSS_EXTENDED_INFO = 0x4A, + NUM_SATS = 0x3B, + GNSS_POSITION = 0x30, + GNSS_VEL_TRACK = 0x32, + GNSS_POS_TIMESTAMP = 0x3E, + GNSS_INFO_SHORT = 0x36, + GNSS_NEW_DATA = 0x41, + GNSS_JAM_STATUS = 0xC0, + DIFFERENTIAL_PRESSURE = 0x28, + TRUE_AIRSPEED = 0x86, + WIND_SPEED = 0x8A, + AIR_DATA_STATUS = 0x8D, + SUPPLY_VOLTAGE = 0x50, + TEMPERATURE = 0x52, + UNIT_STATUS2 = 0x5A, + }; + + /* + packets consist of: + ILabsHeader + list of MessageType + sequence of ILabsData + checksum + */ + struct PACKED ILabsHeader { + uint16_t magic; // 0x55AA + uint8_t msg_type; // always 1 for INS data + uint8_t msg_id; // always 0x95 + uint16_t msg_len; // msg_len+2 is total packet length + }; + + struct PACKED vec3_16_t { + int16_t x,y,z; + Vector3f tofloat(void) { + return Vector3f(x,y,z); + } + }; + struct PACKED vec3_32_t { + int32_t x,y,z; + Vector3f tofloat(void) { + return Vector3f(x,y,z); + } + }; + struct PACKED vec3_u8_t { + uint8_t x,y,z; + Vector3f tofloat(void) { + return Vector3f(x,y,z); + } + }; + struct PACKED vec3_u16_t { + uint16_t x,y,z; + Vector3f tofloat(void) { + return Vector3f(x,y,z); + } + }; + + struct gnss_extended_info_t { + uint8_t fix_type; + uint8_t spoofing_status; + }; + + struct gnss_info_short_t { + uint8_t info1; + uint8_t info2; + }; + + union PACKED ILabsData { + uint32_t gps_time_ms; // ms since start of GPS week + uint16_t gps_week; + vec3_32_t accel_data_hr; // g * 1e6 + vec3_32_t gyro_data_hr; // deg/s * 1e5 + struct PACKED { + uint16_t pressure_pa2; // Pascals/2 + int32_t baro_alt; // meters*100 + } baro_data; + vec3_16_t mag_data; // nT/10 + struct PACKED { + int16_t yaw; // deg*100 + int16_t pitch; // deg*100 + int16_t roll; // deg*100 + } orientation_angles; // 321 euler order? + vec3_32_t velocity; // m/s * 100 + struct PACKED { + int32_t lat; // deg*1e7 + int32_t lon; // deg*1e7 + int32_t alt; // m*100, AMSL + } position; + vec3_u8_t kf_vel_covariance; // mm/s + vec3_u16_t kf_pos_covariance; + uint16_t unit_status; + gnss_extended_info_t gnss_extended_info; + uint8_t num_sats; + struct PACKED { + int32_t hor_speed; // m/s*100 + uint16_t track_over_ground; // deg*100 + int32_t ver_speed; // m/s*100 + } gnss_vel_track; + uint32_t gnss_pos_timestamp; // ms + gnss_info_short_t gnss_info_short; + uint8_t gnss_new_data; + uint8_t gnss_jam_status; + int32_t differential_pressure; // mbar*1e4 + int16_t true_airspeed; // m/s*100 + vec3_16_t wind_speed; // m/s*100 + uint16_t air_data_status; + uint16_t supply_voltage; // V*100 + int16_t temperature; // degC*10 + uint16_t unit_status2; + }; + + AP_ExternalAHRS::gps_data_message_t gps_data; + AP_ExternalAHRS::mag_data_message_t mag_data; + AP_ExternalAHRS::baro_data_message_t baro_data; + AP_ExternalAHRS::ins_data_message_t ins_data; + AP_ExternalAHRS::airspeed_data_message_t airspeed_data; + + uint16_t buffer_ofs; + uint8_t buffer[256]; // max for normal message set is 167+8 + +private: + AP_HAL::UARTDriver *uart; + int8_t port_num; + uint32_t baudrate; + bool setup_complete; + + void update_thread(); + bool check_uart(); + bool check_header(const ILabsHeader *h) const; + + // re-sync on header bytes + void re_sync(void); + + static const struct MessageLength { + MessageType mtype; + uint8_t length; + } message_lengths[]; + + struct { + Vector3f kf_vel_covariance; + Vector3f kf_pos_covariance; + uint32_t gnss_ins_time_ms; + uint16_t unit_status; + uint16_t unit_status2; + gnss_extended_info_t gnss_extended_info; + gnss_info_short_t gnss_info_short; + uint8_t gnss_new_data; + uint8_t gnss_jam_status; + float differential_pressure; + float true_airspeed; + Vector3f wind_speed; + uint16_t air_data_status; + float supply_voltage; + } state2; + + uint32_t last_att_ms; + uint32_t last_vel_ms; + uint32_t last_pos_ms; + uint32_t last_gps_ms; +}; + +#endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h index ef9ef52158833b..2ab10874d8ffed 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h @@ -21,3 +21,7 @@ #ifndef AP_EXTERNAL_AHRS_VECTORNAV_ENABLED #define AP_EXTERNAL_AHRS_VECTORNAV_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED #endif + +#ifndef AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED +#define AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED +#endif