From 35a71f60d6389d524272ccc5a427512cb68926a2 Mon Sep 17 00:00:00 2001 From: BLash Date: Wed, 17 Jul 2024 17:04:51 -0500 Subject: [PATCH] AP_ExternalAHRS_VectorNav: Fix build Minor changes to fix build and use due to bench testing --- .../AP_ExternalAHRS_VectorNav.cpp | 58 ++++++++----------- 1 file changed, 24 insertions(+), 34 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index c1e9070d7e2686..aeefe9ff54d45b 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -41,7 +41,7 @@ extern const AP_HAL::HAL &hal; /* -Type::AHRS configures 2 packets: high-rate IMU and mid-rate EKF +TYPE::VN_AHRS configures 2 packets: high-rate IMU and mid-rate EKF Header for IMU packet $VNWRG,75,3,16,01,0721*D415 Common group (Group 1) @@ -85,7 +85,7 @@ constexpr uint8_t VN_AHRS_ekf_packet::header[]; constexpr uint8_t VN_AHRS_EKF_LENGTH = sizeof(VN_AHRS_ekf_packet) + sizeof(VN_AHRS_ekf_packet::header) + 1 + 2; // Includes sync byte and CRC /* -Type::INS configures 3 packets: high-rate IMU, mid-rate EKF, and 5Hz GNSS +TYPE::VN_INS configures 3 packets: high-rate IMU, mid-rate EKF, and 5Hz GNSS Header for IMU packet $VNWRG,75,3,16,01,0721*D415 Common group (Group 1) @@ -111,7 +111,7 @@ Header for EKF packet Header for GNSS packet $VNWRG,77,1,160,49,0003,26B8,0018*4FD9 Common group (Group 1) - timeStartup + TimeStartup TimeGps Gnss1 group (Group 3) NumSats @@ -231,22 +231,20 @@ bool AP_ExternalAHRS_VectorNav::check_uart() bool match_header2 = false; bool match_header3 = false; bool match_header4 = false; - bool match_header5 = false; if (pktbuf[0] != SYNC_BYTE) { goto reset; } + match_header1 = (0 == memcmp(&pktbuf[1], VN_IMU_packet::header, MIN(sizeof(VN_IMU_packet::header), unsigned(pktoffset - 1)))); if (type == TYPE::VN_AHRS) { - match_header1 = (0 == memcmp(&pktbuf[1], VN_IMU_packet::header, MIN(sizeof(VN_IMU_packet::header), unsigned(pktoffset - 1)))); match_header2 = (0 == memcmp(&pktbuf[1], VN_AHRS_ekf_packet::header, MIN(sizeof(VN_AHRS_ekf_packet::header), unsigned(pktoffset - 1)))); } else { - match_header3 = (0 == memcmp(&pktbuf[1], VN_IMU_packet::header, MIN(sizeof(VN_IMU_packet::header), unsigned(pktoffset - 1)))); - match_header4 = (0 == memcmp(&pktbuf[1], VN_INS_ekf_packet::header, MIN(sizeof(VN_INS_ekf_packet::header), unsigned(pktoffset - 1)))); - match_header5 = (0 == memcmp(&pktbuf[1], VN_INS_gnss_packet::header, MIN(sizeof(VN_INS_gnss_packet::header), unsigned(pktoffset - 1)))); + match_header3 = (0 == memcmp(&pktbuf[1], VN_INS_ekf_packet::header, MIN(sizeof(VN_INS_ekf_packet::header), unsigned(pktoffset - 1)))); + match_header4 = (0 == memcmp(&pktbuf[1], VN_INS_gnss_packet::header, MIN(sizeof(VN_INS_gnss_packet::header), unsigned(pktoffset - 1)))); } - if (!match_header1 && !match_header2 && !match_header3 && !match_header4 && !match_header5) { + if (!match_header1 && !match_header2 && !match_header3 && !match_header4) { goto reset; } @@ -268,17 +266,7 @@ bool AP_ExternalAHRS_VectorNav::check_uart() } else { goto reset; } - } else if (match_header3 && pktoffset >= VN_IMU_LENGTH) { - uint16_t crc = crc16_ccitt(&pktbuf[1], VN_IMU_LENGTH - 1, 0); - if (crc == 0) { - // got pkt1 - process_imu_packet(&pktbuf[sizeof(VN_IMU_packet::header) + 1]); - memmove(&pktbuf[0], &pktbuf[VN_IMU_LENGTH], pktoffset - VN_IMU_LENGTH); - pktoffset -= VN_IMU_LENGTH; - } else { - goto reset; - } - } else if (match_header4 && pktoffset >= VN_INS_EKF_LENGTH) { + } else if (match_header3 && pktoffset >= VN_INS_EKF_LENGTH) { uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_EKF_LENGTH - 1, 0); if (crc == 0) { process_ins_ekf_packet(&pktbuf[sizeof(VN_INS_ekf_packet::header) + 1]); @@ -287,7 +275,7 @@ bool AP_ExternalAHRS_VectorNav::check_uart() } else { goto reset; } - } else if (match_header5 && pktoffset >= VN_INS_GNSS_LENGTH) { + } else if (match_header4 && pktoffset >= VN_INS_GNSS_LENGTH) { uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_GNSS_LENGTH - 1, 0); if (crc == 0) { process_ins_gnss_packet(&pktbuf[sizeof(VN_INS_gnss_packet::header) + 1]); @@ -420,8 +408,8 @@ bool AP_ExternalAHRS_VectorNav::decode_latest_term() case 1: if (nmea.error_response) { GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "VectorNav received VNERR code: %s", nmea.term); - } else if (strncmp(nmea.term, message_to_send + 6, - nmea.term_offset) != 0) { // Start after "VNXXX," + } else if (strlen(message_to_send) > 6 && + strncmp(nmea.term, &message_to_send[6], nmea.term_offset != 0)) { // Start after "VNXXX," return false; } return true; @@ -458,7 +446,7 @@ void AP_ExternalAHRS_VectorNav::initialize() { // These assumes unit is still configured at its default rate of 800hz run_command("VNWRG,75,3,%u,01,0721", unsigned(800 / get_rate())); - run_command("VNWRG,76,3,16,11,0001,0104"); + run_command("VNWRG,76,3,16,11,0001,0106"); } else { // Default to setup for sensors other than VN-100 or VN-110 // This assumes unit is still configured at its default IMU rate of 400hz for VN-300, 800hz for others @@ -470,7 +458,7 @@ void AP_ExternalAHRS_VectorNav::initialize() { has_dual_gnss = true; } run_command("VNWRG,75,3,%u,01,0721", unsigned(imu_rate / get_rate())); - run_command("VNWRG,76,3,%u,31,0001,0104,0613", unsigned(imu_rate / 50)); + run_command("VNWRG,76,3,%u,31,0001,0106,0613", unsigned(imu_rate / 50)); run_command("VNWRG,77,3,%u,49,0003,26B8,0018", unsigned(imu_rate / 5)); } @@ -548,7 +536,7 @@ void AP_ExternalAHRS_VectorNav::process_imu_packet(const uint8_t *b) } #if HAL_LOGGING_ENABLED - // @LoggerMessage: EAHIM + // @LoggerMessage: EAHI // @Description: External AHRS IMU data // @Field: TimeUS: Time since system startup // @Field: Temp: Temprature @@ -585,7 +573,7 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_ekf_packet(const uint8_t *b) { state.have_quaternion = true; #if HAL_LOGGING_ENABLED - // @LoggerMessage: EAHAT + // @LoggerMessage: EAHA // @Description: External AHRS Attitude data // @Field: TimeUS: Time since system startup // @Field: Q1: Attitude quaternion 1 @@ -629,8 +617,8 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) { #if HAL_LOGGING_ENABLED auto now = AP_HAL::micros64(); - // @LoggerMessage: EAHAT - // @Description: External AHRS Kalman data + // @LoggerMessage: EAHA + // @Description: External AHRS Attitude data // @Field: TimeUS: Time since system startup // @Field: Q1: Attitude quaternion 1 // @Field: Q2: Attitude quaternion 2 @@ -651,7 +639,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) { pkt.ypr[0], pkt.ypr[1], pkt.ypr[2], pkt.yprU[0], pkt.yprU[1], pkt.yprU[2]); - // @LoggerMessage: EAHKF + // @LoggerMessage: EAHK // @Description: External AHRS INS Kalman Filter data // @Field: TimeUS: Time since system startup // @Field: InsStatus: VectorNav INS health status @@ -665,11 +653,11 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) { // @Field: VelU: Filter estimated Velocity uncertainty AP::logger().WriteStreaming("EAHKF", "TimeUS,InsStatus,Lat,Lon,Alt,VelN,VelE,VelD,PosU,VelU", - "s-ddmnnndd", "F00000000", + "s-ddmnnndn", "F00000000", "QHdddfffff", now, pkt.insStatus, - pkt.lla[0], pkt.lla[1], pkt.lla[2], + pkt.posLla[0], pkt.posLla[1], pkt.posLla[2], pkt.velNed[0], pkt.velNed[1], pkt.velNed[2], pkt.posU, pkt.velU); #endif // HAL_LOGGING_ENABLED @@ -680,7 +668,9 @@ void AP_ExternalAHRS_VectorNav::process_ins_gnss_packet(const uint8_t *b) { const struct VN_INS_gnss_packet &pkt = *(struct VN_INS_gnss_packet *)b; AP_ExternalAHRS::gps_data_message_t gps; + last_pkt3_ms = AP_HAL::millis(); + latest_ins_gnss_packet = &pkt; // get ToW in milliseconds gps.gps_week = pkt.timeGps / (AP_MSEC_PER_WEEK * 1000000ULL); @@ -736,7 +726,7 @@ bool AP_ExternalAHRS_VectorNav::initialised(void) const if (!setup_complete) { return false; } - return last_pkt1_ms != 0 && last_pkt2_ms != 0 && (type == TYPE::VN_AHRS ? true : last_pkt3_ms != 0); + return last_pkt1_ms != UINT32_MAX && last_pkt2_ms != UINT32_MAX && (type == TYPE::VN_AHRS ? true : last_pkt3_ms != UINT32_MAX); } bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const @@ -750,7 +740,7 @@ bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure return false; } if (type == TYPE::VN_INS) { - if (latest_ins_gnss_packet->fix2 < 3) { + if (latest_ins_gnss_packet->fix1 < 3) { hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS1 lock"); return false; }