From f66815b2000060da71fb92ccf7470e71138adcab Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 28 Sep 2024 17:27:44 +1000 Subject: [PATCH] AP_ExternalAHRS: use AP_GPS_FixType for ExternalAHRS fix type --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 3 ++- libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp | 2 +- libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp | 4 ++-- libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp | 4 ++-- libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp | 4 ++-- 5 files changed, 9 insertions(+), 8 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index 2868c9cede..b617a18e05 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -26,6 +26,7 @@ #include #include #include +#include class AP_ExternalAHRS_backend; @@ -139,7 +140,7 @@ public: typedef struct { uint16_t gps_week; uint32_t ms_tow; - uint8_t fix_type; + AP_GPS_FixType fix_type; uint8_t satellites_in_view; float horizontal_pos_accuracy; float vertical_pos_accuracy; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp index c9b073ab7e..0d2eac1629 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -341,7 +341,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart() } case MessageType::GNSS_EXTENDED_INFO: { CHECK_SIZE(u.gnss_extended_info); - gps_data.fix_type = u.gnss_extended_info.fix_type+1; + gps_data.fix_type = AP_GPS_FixType(u.gnss_extended_info.fix_type+1); gnss_data.spoof_status = u.gnss_extended_info.spoofing_status; break; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp index ed6304429c..a5e4d41c4c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp @@ -169,7 +169,7 @@ void AP_ExternalAHRS_MicroStrain5::post_filter() const AP_ExternalAHRS::gps_data_message_t gps { gps_week: filter_data.week, ms_tow: filter_data.tow_ms, - fix_type: (uint8_t) gnss_data[gnss_instance].fix_type, + fix_type: AP_GPS_FixType(gnss_data[gnss_instance].fix_type), satellites_in_view: gnss_data[gnss_instance].satellites, horizontal_pos_accuracy: gnss_data[gnss_instance].horizontal_position_accuracy, @@ -188,7 +188,7 @@ void AP_ExternalAHRS_MicroStrain5::post_filter() const ned_vel_down: filter_data.ned_velocity_down, }; - if (gps.fix_type >= 3 && !state.have_origin) { + if (gps.fix_type >= AP_GPS_FixType::FIX_3D && !state.have_origin) { WITH_SEMAPHORE(state.sem); state.origin = Location{int32_t(filter_data.lat), int32_t(filter_data.lon), diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp index 4203625a81..43c961a57d 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp @@ -210,7 +210,7 @@ void AP_ExternalAHRS_MicroStrain7::post_filter() const 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, + fix_type: AP_GPS_FixType(gnss_data[instance].fix_type), satellites_in_view: gnss_data[instance].satellites, horizontal_pos_accuracy: gnss_data[instance].horizontal_position_accuracy, @@ -230,7 +230,7 @@ void AP_ExternalAHRS_MicroStrain7::post_filter() const }; // *INDENT-ON* - if (gps.fix_type >= 3 && !state.have_origin) { + if (gps.fix_type >= AP_GPS_FixType::FIX_3D && !state.have_origin) { WITH_SEMAPHORE(state.sem); state.origin = Location{int32_t(gnss_data[instance].lat), int32_t(gnss_data[instance].lon), diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index f1b52aec01..92e4147fe8 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -683,7 +683,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_gnss_packet(const uint8_t *b) { // get ToW in milliseconds gps.gps_week = pkt.timeGps / (AP_MSEC_PER_WEEK * 1000000ULL); gps.ms_tow = (pkt.timeGps / 1000000ULL) % (60 * 60 * 24 * 7 * 1000ULL); - gps.fix_type = pkt.fix1; + gps.fix_type = AP_GPS_FixType(pkt.fix1); gps.satellites_in_view = pkt.numSats1; gps.horizontal_pos_accuracy = pkt.posU1[0]; @@ -701,7 +701,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_gnss_packet(const uint8_t *b) { gps.ned_vel_east = pkt.velNed1[1]; gps.ned_vel_down = pkt.velNed1[2]; - if (!state.have_origin && gps.fix_type >= 3) { + if (!state.have_origin && gps.fix_type >= AP_GPS_FixType::FIX_3D) { WITH_SEMAPHORE(state.sem); state.origin = Location{int32_t(pkt.posLla1[0] * 1.0e7), int32_t(pkt.posLla1[1] * 1.0e7), int32_t(pkt.posLla1[2] * 1.0e2), Location::AltFrame::ABSOLUTE};