From bb28a58bce335d5a8af943122458d42da0efa6ec Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 24 Mar 2021 17:24:01 +1100 Subject: [PATCH] AP_ExternalAHRS: added frontend/backend split allow for multiple backends --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 488 +++--------------- libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 92 ++-- .../AP_ExternalAHRS_VectorNav.cpp | 472 +++++++++++++++++ .../AP_ExternalAHRS_VectorNav.h | 73 +++ .../AP_ExternalAHRS_backend.cpp | 36 ++ .../AP_ExternalAHRS/AP_ExternalAHRS_backend.h | 51 ++ 6 files changed, 734 insertions(+), 478 deletions(-) create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index c87ee06621..cb26a3cb0d 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -16,90 +16,14 @@ suppport for serial connected AHRS systems */ -#define ALLOW_DOUBLE_MATH_FUNCTIONS - #include "AP_ExternalAHRS.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "AP_ExternalAHRS_VectorNav.h" #if HAL_EXTERNAL_AHRS_ENABLED +#include + extern const AP_HAL::HAL &hal; -HAL_Semaphore AP_ExternalAHRS::sem; - -/* - send requested config to the VN - */ -void AP_ExternalAHRS::send_config(void) const -{ - nmea_printf(uart, "$VNWRG,75,3,%u,35,0003,0F2C,0147,0613", unsigned(400/rate.get())); - nmea_printf(uart, "$VNWRG,76,3,80,4E,0002,0010,20B8,2018"); -} - -/* - header for pre-configured 50Hz data - assumes the following config for VN-300: - $VNWRG,75,3,8,35,0003,0F2C,0147,0613*2642 -*/ -static const uint8_t vn_pkt1_header[] { 0x35, 0x03, 0x00, 0x2c, 0x0f, 0x47, 0x01, 0x13, 0x06 }; -#define VN_PKT1_LENGTH 194 // includes header - -struct PACKED VN_packet1 { - uint64_t timeStartup; - uint64_t timeGPS; - float uncompAccel[3]; - float uncompAngRate[3]; - float pressure; - float mag[3]; - float accel[3]; - float gyro[3]; - uint16_t sensSat; - uint16_t AHRSStatus; - float ypr[3]; - float quaternion[4]; - float linAccBody[3]; - float yprU[3]; - uint16_t INSStatus; - double positionLLA[3]; - float velNED[3]; - float posU; - float velU; -}; - -// check packet size for 4 groups -static_assert(sizeof(VN_packet1)+2+4*2+2 == VN_PKT1_LENGTH, "incorrect VN_packet1 length"); - -/* - header for pre-configured 5Hz data - assumes the following VN-300 config: - $VNWRG,76,3,80,4E,0002,0010,20B8,2018*A66B -*/ -static const uint8_t vn_pkt2_header[] { 0x4e, 0x02, 0x00, 0x10, 0x00, 0xb8, 0x20, 0x18, 0x20 }; -#define VN_PKT2_LENGTH 120 // includes header - -struct PACKED VN_packet2 { - uint64_t timeGPS; - float temp; - uint8_t numGPS1Sats; - uint8_t GPS1Fix; - double GPS1posLLA[3]; - float GPS1velNED[3]; - float GPS1DOP[7]; - uint8_t numGPS2Sats; - uint8_t GPS2Fix; - float GPS2DOP[7]; -}; - -// check packet size for 4 groups -static_assert(sizeof(VN_packet2)+2+4*2+2 == VN_PKT2_LENGTH, "incorrect VN_packet2 length"); AP_ExternalAHRS *AP_ExternalAHRS::_singleton; @@ -142,435 +66,139 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { void AP_ExternalAHRS::init(void) { - if (devtype != DevType::VecNav) { - return; - } - auto &sm = AP::serialmanager(); - uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0); - if (!uart) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS no UART"); - return; - } - baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0); - port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); - - bufsize = MAX(VN_PKT1_LENGTH, VN_PKT2_LENGTH); - pktbuf = new uint8_t[bufsize]; - last_pkt1 = new VN_packet1; - last_pkt2 = new VN_packet2; - - if (!pktbuf || !last_pkt1 || !last_pkt2) { - AP_HAL::panic("Failed to allocate ExternalAHRS"); + if (rate.get() < 50) { + // min 50Hz + rate.set(50); } - if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { - AP_HAL::panic("Failed to start ExternalAHRS update thread"); - } - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS initialised"); -} - -/* - check the UART for more data - returns true if the function should be called again straight away - */ -bool AP_ExternalAHRS::check_uart() -{ - if (!port_opened) { - return false; - } - WITH_SEMAPHORE(sem); - - uint32_t n = uart->available(); - if (n == 0) { - return false; - } - if (pktoffset < bufsize) { - ssize_t nread = uart->read(&pktbuf[pktoffset], MIN(n, unsigned(bufsize-pktoffset))); - if (nread <= 0) { - return false; - } - pktoffset += nread; - } - - bool match_header1, match_header2; - - if (pktbuf[0] != 0xFA) { - goto reset; - } - - match_header1 = (0 == memcmp(&pktbuf[1], vn_pkt1_header, MIN(sizeof(vn_pkt1_header), unsigned(pktoffset-1)))); - match_header2 = (0 == memcmp(&pktbuf[1], vn_pkt2_header, MIN(sizeof(vn_pkt2_header), unsigned(pktoffset-1)))); - if (!match_header1 && !match_header2) { - goto reset; - } - - if (match_header1 && pktoffset >= VN_PKT1_LENGTH) { - uint16_t crc = crc16_ccitt(&pktbuf[1], VN_PKT1_LENGTH-1, 0); - if (crc == 0) { - // got pkt1 - process_packet1(&pktbuf[sizeof(vn_pkt1_header)+1]); - memmove(&pktbuf[0], &pktbuf[VN_PKT1_LENGTH], pktoffset-VN_PKT1_LENGTH); - pktoffset -= VN_PKT1_LENGTH; - } else { - goto reset; - } - } else if (match_header2 && pktoffset >= VN_PKT2_LENGTH) { - uint16_t crc = crc16_ccitt(&pktbuf[1], VN_PKT2_LENGTH-1, 0); - if (crc == 0) { - // got pkt2 - process_packet2(&pktbuf[sizeof(vn_pkt2_header)+1]); - memmove(&pktbuf[0], &pktbuf[VN_PKT2_LENGTH], pktoffset-VN_PKT2_LENGTH); - pktoffset -= VN_PKT2_LENGTH; - } else { - goto reset; - } - } - return true; - -reset: - uint8_t *p = (uint8_t *)memchr(&pktbuf[1], (char)0xFA, pktoffset-1); - if (p) { - uint8_t newlen = pktoffset - (p - pktbuf); - memmove(&pktbuf[0], p, newlen); - pktoffset = newlen; - } else { - pktoffset = 0; - } - return true; -} - -void AP_ExternalAHRS::update_thread() -{ - if (!port_opened) { - // open port in the thread - port_opened = true; - uart->begin(baudrate, 1024, 512); - send_config(); - } - - while (true) { - if (!check_uart()) { - hal.scheduler->delay(1); - } + switch (DevType(devtype)) { + case DevType::VecNav: + backend = new AP_ExternalAHRS_VectorNav(this, state); + break; + default: + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unsupported ExternalAHRS type %u", unsigned(devtype)); + break; } } -/* - process packet type 1 - */ -void AP_ExternalAHRS::process_packet1(const uint8_t *b) -{ - const struct VN_packet1 &pkt1 = *(struct VN_packet1 *)b; - const struct VN_packet2 &pkt2 = *last_pkt2; - - last_pkt1_ms = AP_HAL::millis(); - *last_pkt1 = pkt1; - - { - baro_data_message_t baro; - baro.instance = 0; - baro.pressure_pa = pkt1.pressure*1e3; - baro.temperature = pkt2.temp; - - AP::baro().handle_external(baro); - } - - { - mag_data_message_t mag; - mag.field = Vector3f{pkt1.mag[0], pkt1.mag[1], pkt1.mag[2]}; - mag.field *= 1000; // to mGauss - - AP::compass().handle_external(mag); - } - - { - ins_data_message_t ins; - - ins.accel = Vector3f{pkt1.accel[0], pkt1.accel[1], pkt1.accel[2]}; - ins.gyro = Vector3f{pkt1.gyro[0], pkt1.gyro[1], pkt1.gyro[2]}; - ins.temperature = pkt2.temp; - - AP::ins().handle_external(ins); - } - - // @LoggerMessage: EAH1 - // @Description: External AHRS data - // @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 - // @Field: UXY: uncertainty in XY position - // @Field: UV: uncertainty in velocity - // @Field: UR: uncertainty in roll - // @Field: UP: uncertainty in pitch - // @Field: UY: uncertainty in yaw - - AP::logger().Write("EAH1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,UXY,UV,UR,UP,UY", - "sdddnnnDUmmnddd", "F000000GG000000", - "QffffffLLffffff", - AP_HAL::micros64(), - pkt1.ypr[2], pkt1.ypr[1], pkt1.ypr[0], - pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2], - int32_t(pkt1.positionLLA[0]*1.0e7), int32_t(pkt1.positionLLA[1]*1.0e7), - float(pkt1.positionLLA[2]), - pkt1.posU, pkt1.velU, - pkt1.yprU[2], pkt1.yprU[1], pkt1.yprU[0]); -} - -/* - process packet type 2 - */ -void AP_ExternalAHRS::process_packet2(const uint8_t *b) -{ - const struct VN_packet2 &pkt2 = *(struct VN_packet2 *)b; - const struct VN_packet1 &pkt1 = *last_pkt1; - - last_pkt2_ms = AP_HAL::millis(); - *last_pkt2 = pkt2; - - gps_data_message_t gps; - - // get ToW in milliseconds - gps.gps_week = pkt2.timeGPS / (AP_MSEC_PER_WEEK * 1000000ULL); - gps.ms_tow = (pkt2.timeGPS / 1000000ULL) % (60*60*24*7*1000ULL); - gps.fix_type = pkt2.GPS1Fix; - gps.satellites_in_view = pkt2.numGPS1Sats; - - gps.horizontal_pos_accuracy = pkt1.posU; - gps.vertical_pos_accuracy = pkt1.posU; - gps.horizontal_vel_accuracy = pkt1.velU; - - gps.hdop = pkt2.GPS1DOP[4]; - gps.vdop = pkt2.GPS1DOP[3]; - - gps.latitude = pkt2.GPS1posLLA[0] * 1.0e7; - gps.longitude = pkt2.GPS1posLLA[1] * 1.0e7; - gps.msl_altitude = pkt2.GPS1posLLA[2] * 1.0e2; - - gps.ned_vel_north = pkt2.GPS1velNED[0]; - gps.ned_vel_east = pkt2.GPS1velNED[1]; - gps.ned_vel_down = pkt2.GPS1velNED[2]; - - if (gps.fix_type >= 3 && !origin_set) { - origin.lat = gps.latitude; - origin.lng = gps.longitude; - origin.alt = gps.msl_altitude; - origin_set = true; - } - - AP::gps().handle_external(gps); -} - -// get serial port number for the uart +// get serial port number for the uart, or -1 if not applicable int8_t AP_ExternalAHRS::get_port(void) const { - if (!uart) { + if (!backend) { return -1; } - return port_num; + return backend->get_port(); }; // accessors for AP_AHRS bool AP_ExternalAHRS::healthy(void) const { - uint32_t now = AP_HAL::millis(); - return (now - last_pkt1_ms < 40 && now - last_pkt2_ms < 500); + return backend && backend->healthy(); } bool AP_ExternalAHRS::initialised(void) const { - return last_pkt1_ms != 0 && last_pkt2_ms != 0; + return backend && backend->initialised(); } bool AP_ExternalAHRS::get_quaternion(Quaternion &quat) { - if (!last_pkt1) { - return false; + if (state.have_quaternion) { + WITH_SEMAPHORE(state.sem); + quat = state.quat; + return true; } - WITH_SEMAPHORE(sem); - const struct VN_packet1 &pkt1 = *last_pkt1; - quat(pkt1.quaternion[3], pkt1.quaternion[0], pkt1.quaternion[1], pkt1.quaternion[2]); - return true; + return false; } bool AP_ExternalAHRS::get_origin(Location &loc) { - WITH_SEMAPHORE(sem); - loc = origin; - return origin_set; + if (state.have_origin) { + WITH_SEMAPHORE(state.sem); + loc = state.origin; + return true; + } + return false; } bool AP_ExternalAHRS::get_location(Location &loc) { - if (!last_pkt2) { + if (!state.have_location) { return false; } - const struct VN_packet2 &pkt2 = *last_pkt2; - WITH_SEMAPHORE(sem); - loc = Location{int32_t(pkt2.GPS1posLLA[0] * 1.0e7), - int32_t(pkt2.GPS1posLLA[1] * 1.0e7), - int32_t(pkt2.GPS1posLLA[2] * 1.0e2), - Location::AltFrame::ABSOLUTE}; + WITH_SEMAPHORE(state.sem); + loc = state.location; return true; } Vector2f AP_ExternalAHRS::get_groundspeed_vector() { - if (!last_pkt1) { - return Vector2f{}; - } - const struct VN_packet1 &pkt1 = *last_pkt1; - WITH_SEMAPHORE(sem); - return Vector2f{pkt1.velNED[0], pkt1.velNED[1]}; + WITH_SEMAPHORE(state.sem); + Vector2f vec{state.velocity.x, state.velocity.y}; + return vec; } bool AP_ExternalAHRS::get_velocity_NED(Vector3f &vel) { - if (!last_pkt1) { + if (!state.have_velocity) { return false; } - const struct VN_packet1 &pkt1 = *last_pkt1; - WITH_SEMAPHORE(sem); - vel.x = pkt1.velNED[0]; - vel.y = pkt1.velNED[1]; - vel.z = pkt1.velNED[2]; + WITH_SEMAPHORE(state.sem); + vel = state.velocity; return true; } bool AP_ExternalAHRS::get_speed_down(float &speedD) { - if (!last_pkt1) { + if (!state.have_velocity) { return false; } - const struct VN_packet1 &pkt1 = *last_pkt1; - WITH_SEMAPHORE(sem); - speedD = pkt1.velNED[2]; + WITH_SEMAPHORE(state.sem); + speedD = state.velocity.z; return true; } bool AP_ExternalAHRS::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { - if (!healthy()) { - hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav unhealthy"); - return false; - } - if (last_pkt2->GPS1Fix < 3) { - hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS1 lock"); - return false; - } - if (last_pkt2->GPS2Fix < 3) { - hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS2 lock"); - return false; - } - return true; + return backend && backend->pre_arm_check(failure_msg, failure_msg_len); } /* - get filter status. We don't know the meaning of the status bits yet, - so assume all OK if we have GPS lock + get filter status */ void AP_ExternalAHRS::get_filter_status(nav_filter_status &status) const { - memset(&status, 0, sizeof(status)); - if (last_pkt1 && last_pkt2) { - status.flags.initalized = 1; - } - if (healthy() && last_pkt2) { - status.flags.attitude = 1; - status.flags.vert_vel = 1; - status.flags.vert_pos = 1; - - const struct VN_packet2 &pkt2 = *last_pkt2; - if (pkt2.GPS1Fix >= 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; - } + status = {}; + if (backend) { + backend->get_filter_status(status); } } Vector3f AP_ExternalAHRS::get_gyro(void) { - if (!last_pkt1) { - return Vector3f{}; - } - const struct VN_packet1 &pkt1 = *last_pkt1; - WITH_SEMAPHORE(sem); - return Vector3f(pkt1.gyro[0], pkt1.gyro[1], pkt1.gyro[2]); + WITH_SEMAPHORE(state.sem); + return state.gyro; } Vector3f AP_ExternalAHRS::get_accel(void) { - if (!last_pkt1) { - return Vector3f{}; - } - const struct VN_packet1 &pkt1 = *last_pkt1; - WITH_SEMAPHORE(sem); - return Vector3f{pkt1.accel[0], pkt1.accel[1], pkt1.accel[2]}; + WITH_SEMAPHORE(state.sem); + return state.accel; } // send an EKF_STATUS message to GCS void AP_ExternalAHRS::send_status_report(mavlink_channel_t chan) const { - if (!last_pkt1) { - return; - } - // 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; + if (backend) { + backend->send_status_report(chan); } +} - // send message - const struct VN_packet1 &pkt1 = *(struct VN_packet1 *)last_pkt1; - 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(chan, flags, - pkt1.velU/vel_gate, pkt1.posU/pos_gate, pkt1.posU/hgt_gate, - mag_var, 0, 0); +void AP_ExternalAHRS::update(void) +{ + if (backend) { + backend->update(); + } } namespace AP { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index 83d8cba5bf..3a6daedfe9 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -31,9 +31,13 @@ #include +class AP_ExternalAHRS_backend; + class AP_ExternalAHRS { public: + friend class AP_ExternalAHRS_backend; + AP_ExternalAHRS(); void init(void); @@ -49,7 +53,7 @@ public: return _singleton; } - // fixed IMU rate for now + // expected IMU rate in Hz float get_IMU_rate(void) const { return rate.get(); } @@ -57,6 +61,43 @@ public: // get serial port number, -1 for not enabled int8_t get_port(void) const; + struct state_t { + HAL_Semaphore sem; + + Vector3f accel; + Vector3f gyro; + Quaternion quat; + Location location; + Vector3f velocity; + Location origin; + + bool have_quaternion; + bool have_origin; + bool have_location; + bool have_velocity; + } state; + + // accessors for AP_AHRS + bool healthy(void) const; + bool initialised(void) const; + bool get_quaternion(Quaternion &quat); + bool get_origin(Location &loc); + bool get_location(Location &loc); + Vector2f get_groundspeed_vector(); + bool get_velocity_NED(Vector3f &vel); + bool get_speed_down(float &speedD); + bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const; + void get_filter_status(nav_filter_status &status) const; + Vector3f get_gyro(void); + Vector3f get_accel(void); + void send_status_report(mavlink_channel_t chan) const; + + // update backend + void update(); + + /* + structures passed to other subsystems + */ typedef struct { uint8_t instance; float pressure_pa; @@ -90,59 +131,14 @@ public: Vector3f gyro; float temperature; } ins_data_message_t; - - // accessors for AP_AHRS - bool healthy(void) const; - bool initialised(void) const; - bool get_quaternion(Quaternion &quat); - bool get_origin(Location &loc); - bool get_location(Location &loc); - Vector2f get_groundspeed_vector(); - bool get_velocity_NED(Vector3f &vel); - bool get_speed_down(float &speedD); - bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const; - void get_filter_status(nav_filter_status &status) const; - Vector3f get_gyro(void); - Vector3f get_accel(void); - void send_status_report(mavlink_channel_t chan) const; - - // check for new data - void update() { - check_uart(); - } - + private: - AP_HAL::UARTDriver *uart; - int8_t port_num; - bool port_opened; - uint32_t baudrate; - - void update_thread(); - bool check_uart(); - - void process_packet1(const uint8_t *b); - void process_packet2(const uint8_t *b); - void send_config(void) const; + AP_ExternalAHRS_backend *backend; AP_Enum devtype; AP_Int16 rate; static AP_ExternalAHRS *_singleton; - - uint8_t *pktbuf; - uint16_t pktoffset; - uint16_t bufsize; - - struct VN_packet1 *last_pkt1; - struct VN_packet2 *last_pkt2; - - uint32_t last_pkt1_ms; - uint32_t last_pkt2_ms; - - bool origin_set; - Location origin; - - static HAL_Semaphore sem; }; namespace AP { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp new file mode 100644 index 0000000000..648b3904a5 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -0,0 +1,472 @@ +/* + 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 serial connected AHRS systems + */ + +#define ALLOW_DOUBLE_MATH_FUNCTIONS + +#include "AP_ExternalAHRS_VectorNav.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if HAL_EXTERNAL_AHRS_ENABLED + +extern const AP_HAL::HAL &hal; + +/* + send requested config to the VN + */ +void AP_ExternalAHRS_VectorNav::send_config(void) const +{ + nmea_printf(uart, "$VNWRG,75,3,%u,35,0003,0F2C,0147,0613", unsigned(400/get_rate())); + nmea_printf(uart, "$VNWRG,76,3,80,4E,0002,0010,20B8,2018"); +} + +/* + header for pre-configured 50Hz data + assumes the following config for VN-300: + $VNWRG,75,3,8,35,0003,0F2C,0147,0613*2642 +*/ +static const uint8_t vn_pkt1_header[] { 0x35, 0x03, 0x00, 0x2c, 0x0f, 0x47, 0x01, 0x13, 0x06 }; +#define VN_PKT1_LENGTH 194 // includes header + +struct PACKED VN_packet1 { + uint64_t timeStartup; + uint64_t timeGPS; + float uncompAccel[3]; + float uncompAngRate[3]; + float pressure; + float mag[3]; + float accel[3]; + float gyro[3]; + uint16_t sensSat; + uint16_t AHRSStatus; + float ypr[3]; + float quaternion[4]; + float linAccBody[3]; + float yprU[3]; + uint16_t INSStatus; + double positionLLA[3]; + float velNED[3]; + float posU; + float velU; +}; + +// check packet size for 4 groups +static_assert(sizeof(VN_packet1)+2+4*2+2 == VN_PKT1_LENGTH, "incorrect VN_packet1 length"); + +/* + header for pre-configured 5Hz data + assumes the following VN-300 config: + $VNWRG,76,3,80,4E,0002,0010,20B8,2018*A66B +*/ +static const uint8_t vn_pkt2_header[] { 0x4e, 0x02, 0x00, 0x10, 0x00, 0xb8, 0x20, 0x18, 0x20 }; +#define VN_PKT2_LENGTH 120 // includes header + +struct PACKED VN_packet2 { + uint64_t timeGPS; + float temp; + uint8_t numGPS1Sats; + uint8_t GPS1Fix; + double GPS1posLLA[3]; + float GPS1velNED[3]; + float GPS1DOP[7]; + uint8_t numGPS2Sats; + uint8_t GPS2Fix; + float GPS2DOP[7]; +}; + +// check packet size for 4 groups +static_assert(sizeof(VN_packet2)+2+4*2+2 == VN_PKT2_LENGTH, "incorrect VN_packet2 length"); + +// constructor +AP_ExternalAHRS_VectorNav::AP_ExternalAHRS_VectorNav(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, "ExternalAHRS no UART"); + return; + } + baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0); + port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); + + bufsize = MAX(VN_PKT1_LENGTH, VN_PKT2_LENGTH); + pktbuf = new uint8_t[bufsize]; + last_pkt1 = new VN_packet1; + last_pkt2 = new VN_packet2; + + if (!pktbuf || !last_pkt1 || !last_pkt2) { + AP_HAL::panic("Failed to allocate ExternalAHRS"); + } + + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_VectorNav::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { + AP_HAL::panic("Failed to start ExternalAHRS update thread"); + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS initialised"); +} + +/* + check the UART for more data + returns true if the function should be called again straight away + */ +bool AP_ExternalAHRS_VectorNav::check_uart() +{ + if (!port_opened) { + return false; + } + WITH_SEMAPHORE(state.sem); + + uint32_t n = uart->available(); + if (n == 0) { + return false; + } + if (pktoffset < bufsize) { + ssize_t nread = uart->read(&pktbuf[pktoffset], MIN(n, unsigned(bufsize-pktoffset))); + if (nread <= 0) { + return false; + } + pktoffset += nread; + } + + bool match_header1, match_header2; + + if (pktbuf[0] != 0xFA) { + goto reset; + } + + match_header1 = (0 == memcmp(&pktbuf[1], vn_pkt1_header, MIN(sizeof(vn_pkt1_header), unsigned(pktoffset-1)))); + match_header2 = (0 == memcmp(&pktbuf[1], vn_pkt2_header, MIN(sizeof(vn_pkt2_header), unsigned(pktoffset-1)))); + if (!match_header1 && !match_header2) { + goto reset; + } + + if (match_header1 && pktoffset >= VN_PKT1_LENGTH) { + uint16_t crc = crc16_ccitt(&pktbuf[1], VN_PKT1_LENGTH-1, 0); + if (crc == 0) { + // got pkt1 + process_packet1(&pktbuf[sizeof(vn_pkt1_header)+1]); + memmove(&pktbuf[0], &pktbuf[VN_PKT1_LENGTH], pktoffset-VN_PKT1_LENGTH); + pktoffset -= VN_PKT1_LENGTH; + } else { + goto reset; + } + } else if (match_header2 && pktoffset >= VN_PKT2_LENGTH) { + uint16_t crc = crc16_ccitt(&pktbuf[1], VN_PKT2_LENGTH-1, 0); + if (crc == 0) { + // got pkt2 + process_packet2(&pktbuf[sizeof(vn_pkt2_header)+1]); + memmove(&pktbuf[0], &pktbuf[VN_PKT2_LENGTH], pktoffset-VN_PKT2_LENGTH); + pktoffset -= VN_PKT2_LENGTH; + } else { + goto reset; + } + } + return true; + +reset: + uint8_t *p = (uint8_t *)memchr(&pktbuf[1], (char)0xFA, pktoffset-1); + if (p) { + uint8_t newlen = pktoffset - (p - pktbuf); + memmove(&pktbuf[0], p, newlen); + pktoffset = newlen; + } else { + pktoffset = 0; + } + return true; +} + +void AP_ExternalAHRS_VectorNav::update_thread() +{ + if (!port_opened) { + // open port in the thread + port_opened = true; + uart->begin(baudrate, 1024, 512); + send_config(); + } + + while (true) { + if (!check_uart()) { + hal.scheduler->delay(1); + } + } +} + +/* + process packet type 1 + */ +void AP_ExternalAHRS_VectorNav::process_packet1(const uint8_t *b) +{ + const struct VN_packet1 &pkt1 = *(struct VN_packet1 *)b; + const struct VN_packet2 &pkt2 = *last_pkt2; + + last_pkt1_ms = AP_HAL::millis(); + *last_pkt1 = pkt1; + + { + WITH_SEMAPHORE(state.sem); + state.accel = Vector3f{pkt1.accel[0], pkt1.accel[1], pkt1.accel[2]}; + state.gyro = Vector3f{pkt1.gyro[0], pkt1.gyro[1], pkt1.gyro[2]}; + + state.quat = Quaternion{pkt1.quaternion[3], pkt1.quaternion[0], pkt1.quaternion[1], pkt1.quaternion[2]}; + state.have_quaternion = true; + + state.velocity = Vector3f{pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2]}; + state.have_velocity = true; + + state.location = Location{int32_t(pkt1.positionLLA[0] * 1.0e7), + int32_t(pkt1.positionLLA[1] * 1.0e7), + int32_t(pkt1.positionLLA[2] * 1.0e2), + Location::AltFrame::ABSOLUTE}; + state.have_location = true; + } + + { + AP_ExternalAHRS::baro_data_message_t baro; + baro.instance = 0; + baro.pressure_pa = pkt1.pressure*1e3; + baro.temperature = pkt2.temp; + + AP::baro().handle_external(baro); + } + + { + AP_ExternalAHRS::mag_data_message_t mag; + mag.field = Vector3f{pkt1.mag[0], pkt1.mag[1], pkt1.mag[2]}; + mag.field *= 1000; // to mGauss + + AP::compass().handle_external(mag); + } + + { + AP_ExternalAHRS::ins_data_message_t ins; + + ins.accel = state.accel; + ins.gyro = state.gyro; + ins.temperature = pkt2.temp; + + AP::ins().handle_external(ins); + } + + + // @LoggerMessage: EAH1 + // @Description: External AHRS data + // @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 + // @Field: UXY: uncertainty in XY position + // @Field: UV: uncertainty in velocity + // @Field: UR: uncertainty in roll + // @Field: UP: uncertainty in pitch + // @Field: UY: uncertainty in yaw + + AP::logger().Write("EAH1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,UXY,UV,UR,UP,UY", + "sdddnnnDUmmnddd", "F000000GG000000", + "QffffffLLffffff", + AP_HAL::micros64(), + pkt1.ypr[2], pkt1.ypr[1], pkt1.ypr[0], + pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2], + int32_t(pkt1.positionLLA[0]*1.0e7), int32_t(pkt1.positionLLA[1]*1.0e7), + float(pkt1.positionLLA[2]), + pkt1.posU, pkt1.velU, + pkt1.yprU[2], pkt1.yprU[1], pkt1.yprU[0]); +} + +/* + process packet type 2 + */ +void AP_ExternalAHRS_VectorNav::process_packet2(const uint8_t *b) +{ + const struct VN_packet2 &pkt2 = *(struct VN_packet2 *)b; + const struct VN_packet1 &pkt1 = *last_pkt1; + + last_pkt2_ms = AP_HAL::millis(); + *last_pkt2 = pkt2; + + AP_ExternalAHRS::gps_data_message_t gps; + + // get ToW in milliseconds + gps.gps_week = pkt2.timeGPS / (AP_MSEC_PER_WEEK * 1000000ULL); + gps.ms_tow = (pkt2.timeGPS / 1000000ULL) % (60*60*24*7*1000ULL); + gps.fix_type = pkt2.GPS1Fix; + gps.satellites_in_view = pkt2.numGPS1Sats; + + gps.horizontal_pos_accuracy = pkt1.posU; + gps.vertical_pos_accuracy = pkt1.posU; + gps.horizontal_vel_accuracy = pkt1.velU; + + gps.hdop = pkt2.GPS1DOP[4]; + gps.vdop = pkt2.GPS1DOP[3]; + + gps.latitude = pkt2.GPS1posLLA[0] * 1.0e7; + gps.longitude = pkt2.GPS1posLLA[1] * 1.0e7; + gps.msl_altitude = pkt2.GPS1posLLA[2] * 1.0e2; + + gps.ned_vel_north = pkt2.GPS1velNED[0]; + gps.ned_vel_east = pkt2.GPS1velNED[1]; + gps.ned_vel_down = pkt2.GPS1velNED[2]; + + if (gps.fix_type >= 3 && !state.have_origin) { + WITH_SEMAPHORE(state.sem); + state.origin = Location{int32_t(pkt2.GPS1posLLA[0] * 1.0e7), + int32_t(pkt2.GPS1posLLA[1] * 1.0e7), + int32_t(pkt2.GPS1posLLA[2] * 1.0e2), + Location::AltFrame::ABSOLUTE}; + state.have_origin = true; + } + + AP::gps().handle_external(gps); +} + +// get serial port number for the uart +int8_t AP_ExternalAHRS_VectorNav::get_port(void) const +{ + if (!uart) { + return -1; + } + return port_num; +}; + +// accessors for AP_AHRS +bool AP_ExternalAHRS_VectorNav::healthy(void) const +{ + uint32_t now = AP_HAL::millis(); + return (now - last_pkt1_ms < 40 && now - last_pkt2_ms < 500); +} + +bool AP_ExternalAHRS_VectorNav::initialised(void) const +{ + return last_pkt1_ms != 0 && last_pkt2_ms != 0; +} + +bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const +{ + if (!healthy()) { + hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav unhealthy"); + return false; + } + if (last_pkt2->GPS1Fix < 3) { + hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS1 lock"); + return false; + } + if (last_pkt2->GPS2Fix < 3) { + hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS2 lock"); + 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_VectorNav::get_filter_status(nav_filter_status &status) const +{ + memset(&status, 0, sizeof(status)); + if (last_pkt1 && last_pkt2) { + status.flags.initalized = 1; + } + if (healthy() && last_pkt2) { + status.flags.attitude = 1; + status.flags.vert_vel = 1; + status.flags.vert_pos = 1; + + const struct VN_packet2 &pkt2 = *last_pkt2; + if (pkt2.GPS1Fix >= 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; + } + } +} + +// send an EKF_STATUS message to GCS +void AP_ExternalAHRS_VectorNav::send_status_report(mavlink_channel_t chan) const +{ + if (!last_pkt1) { + return; + } + // 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 struct VN_packet1 &pkt1 = *(struct VN_packet1 *)last_pkt1; + 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(chan, flags, + pkt1.velU/vel_gate, pkt1.posU/pos_gate, pkt1.posU/hgt_gate, + mag_var, 0, 0); +} + +#endif // HAL_EXTERNAL_AHRS_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h new file mode 100644 index 0000000000..2ca9971771 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.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 serial connected AHRS systems + */ + +#pragma once + +#include "AP_ExternalAHRS_backend.h" + +#if HAL_EXTERNAL_AHRS_ENABLED + +#include + +class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { + +public: + AP_ExternalAHRS_VectorNav(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(mavlink_channel_t chan) const override; + + // check for new data + void update() override { + check_uart(); + } + +private: + AP_HAL::UARTDriver *uart; + int8_t port_num; + bool port_opened; + uint32_t baudrate; + uint16_t rate; + + void update_thread(); + bool check_uart(); + + void process_packet1(const uint8_t *b); + void process_packet2(const uint8_t *b); + void send_config(void) const; + + uint8_t *pktbuf; + uint16_t pktoffset; + uint16_t bufsize; + + struct VN_packet1 *last_pkt1; + struct VN_packet2 *last_pkt2; + + uint32_t last_pkt1_ms; + uint32_t last_pkt2_ms; +}; + +#endif // HAL_EXTERNAL_AHRS_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp new file mode 100644 index 0000000000..f544238bc4 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp @@ -0,0 +1,36 @@ +/* + 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 . + */ +/* + parent class for ExternalAHRS backends + */ + +#include "AP_ExternalAHRS_backend.h" + +#if HAL_EXTERNAL_AHRS_ENABLED + +AP_ExternalAHRS_backend::AP_ExternalAHRS_backend(AP_ExternalAHRS *_frontend, + AP_ExternalAHRS::state_t &_state) : + frontend(*_frontend), + state(_state) +{} + + +uint16_t AP_ExternalAHRS_backend::get_rate(void) const +{ + return frontend.get_IMU_rate(); +} + +#endif // HAL_EXTERNAL_AHRS_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h new file mode 100644 index 0000000000..fe0edbe132 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h @@ -0,0 +1,51 @@ +/* + 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 . + */ +/* + parent class for ExternalAHRS backends + */ + +#pragma once + +#include "AP_ExternalAHRS.h" + +#if HAL_EXTERNAL_AHRS_ENABLED + +class AP_ExternalAHRS_backend { +public: + AP_ExternalAHRS_backend(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state); + + // get serial port number, -1 for not enabled + virtual int8_t get_port(void) const { return -1; } + + // accessors for AP_AHRS + virtual bool healthy(void) const = 0; + virtual bool initialised(void) const = 0; + virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const = 0; + virtual void get_filter_status(nav_filter_status &status) const {} + virtual void send_status_report(mavlink_channel_t chan) const {} + + // check for new data + virtual void update() = 0; + +protected: + AP_ExternalAHRS::state_t &state; + uint16_t get_rate(void) const; + +private: + AP_ExternalAHRS &frontend; +}; + +#endif // HAL_EXTERNAL_AHRS_ENABLED +