/*
   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 <http://www.gnu.org/licenses/>.
 */
/*
  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 7
    param set GPS1_TYPE 21
    param set GPS2_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 <AP_Baro/AP_Baro.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <cstring>  // For memcpy

static const char* LOG_FMT = "%s ExternalAHRS: %s";

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, LOG_FMT, get_name(), "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 ExternalAHRS failed to allocate ExternalAHRS update thread");
//     }

//     // don't offer IMU by default, at 100Hz 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));

//     hal.scheduler->delay(5000);
//     if (!initialised()) {
//         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_FMT, get_name(), "failed to initialise.");
//     }
// }

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, LOG_FMT, get_name(), "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 ExternalAHRS failed to allocate ExternalAHRS update thread");
    }

    // Send the "Get Device Info" command
    uint8_t command[2] = {2, 0x03}; // Field Length = 2, Descriptor = 0x03
    uart->write(command, sizeof(command));

    // Wait for the response
    uint8_t response[256];
    uint16_t response_len = uart->read(response, sizeof(response));

    // Validate the response length
    if (response_len < 2 || response[1] != 0x81) { // Response Descriptor = 0x81
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_FMT, get_name(), "Device detection failed: Invalid response");
        return;
    }

  // Parse the device information
    uint8_t device_info[response[0]]; // Response Length at response[0]
    memcpy(device_info, &response[2], response[0]); // Copy device info starting from byte 2

      // Example: Check for CV7 or GQ7
    if (device_info[0] == static_cast<uint8_t>(DeviceInfo::CV7_IDENTIFIER)) { // Example identifier
        is_cv7 = true;
        GCS_SEND_TEXT(MAV_SEVERITY_INFO, LOG_FMT, get_name(), "Detected device: 3DM-CV7");
    } else if (device_info[0] == static_cast<uint8_t>(DeviceInfo::GQ7_IDENTIFIER)) { // Example identifier
        is_cv7 = false;
        GCS_SEND_TEXT(MAV_SEVERITY_INFO, LOG_FMT, get_name(), "Detected device: 3DM-GQ7");
    } else {
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_FMT, get_name(), "Device detection failed: Unknown model");
        return;
    }

    // Ensure required packets are available
    if ((is_cv7 && last_imu_pkt == 0) ||
        (!is_cv7 && (last_imu_pkt == 0 || last_gps_pkt == 0 || last_filter_pkt == 0))) {
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_FMT, get_name(), "failed to initialise: Missing required packets.");
        return;
    }

    // don't offer IMU by default, at 100Hz 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));

    GCS_SEND_TEXT(MAV_SEVERITY_INFO, LOG_FMT, get_name(), "initialised.");
}

bool AP_ExternalAHRS_MicroStrain7::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const  //No changes
{
    if (!initialised()) {
        hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "not initialised");
        return false;
    }
    if (!times_healthy()) {
        hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "data is stale");
        return false;
    }
    if (!filter_healthy()) {
        hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "filter is unhealthy");
        return false;
    }
    if (!healthy()) {
        hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "unhealthy");
        return false;
    }
    static_assert(NUM_GNSS_INSTANCES == 2, "This check only works if there are two GPS types.");
    if (gnss_data[0].fix_type < GPS_FIX_TYPE_3D_FIX && gnss_data[1].fix_type < GPS_FIX_TYPE_3D_FIX) {
        hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "missing 3D GPS fix on either GPS");
        return false;
    }
    return true;
}

void AP_ExternalAHRS_MicroStrain7::update_thread(void) //No changes
{
    if (!port_open) {
        port_open = true;
        uart->begin(baudrate);
    }

    while (true) {
        build_packet();
        hal.scheduler->delay_microseconds(100);
        check_initialise_state();
    }
}

void AP_ExternalAHRS_MicroStrain7::check_initialise_state(void)  //No changes
{
    const auto new_init_state = initialised();
    // Only send the message after fully booted up, otherwise it gets dropped.
    if (!last_init_state && new_init_state && AP_HAL::millis() > 5000) {
        GCS_SEND_TEXT(MAV_SEVERITY_INFO, LOG_FMT, get_name(), "initialised.");
        last_init_state = new_init_state;
    }
}

// bool AP_ExternalAHRS_MicroStrain7::initialised(void) const
// {
//     // Send the "Get Device Info" command
//     uint8_t command[2] = {0x01, 0x03}; // Example command for "Get Device Info"
//     uart->write(command, sizeof(command));

//     // Wait for the response
//     uint8_t response[256];
//     uint16_t response_len = uart->read(response, sizeof(response));

//     // Validate the response length
//     if (response_len < static_cast<uint16_t>(DeviceInfo::MIN_RESPONSE_LENGTH)) {
//         hal.console->printf("Device initialization failed: Insufficient response\n");
//         return false;
//     }

//     // Extract the model number from the response
//     uint16_t model_number = 0;
//     memcpy(&model_number, &response[static_cast<uint8_t>(DeviceInfo::MODEL_NUMBER_OFFSET)], sizeof(model_number));

//     // Check if the model number matches CV7 or GQ7
//     if (model_number == static_cast<uint16_t>(DeviceInfo::CV7_MODEL_NUMBER)) {
//         hal.console->printf("Detected device: 3DM-CV7\n");
//         is_cv7 = true;
//         return last_imu_pkt != 0; // CV7 requires only IMU packets
//     } else if (model_number == static_cast<uint16_t>(DeviceInfo::GQ7_MODEL_NUMBER)) {
//         hal.console->printf("Detected device: 3DM-GQ7\n");
//         is_cv7 = false;
//         return last_imu_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0; // GQ7 requires all packets
//     } else {
//         hal.console->printf("Device initialization failed: Unknown model number (%u)\n", model_number);
//         return false;
//     }
// }

bool AP_ExternalAHRS_MicroStrain7::initialised(void) const
{
    // Check packets based on device type
    if (is_cv7) {
        // CV7 requires only IMU packets
        return last_imu_pkt != 0;
    } else {
        // GQ7 requires IMU, GPS, and Filter packets
        const bool got_packets =    (last_imu_pkt != 0) &&
                                    (last_gps_pkt != 0) &&
                                    (last_filter_pkt != 0);
        return got_packets;
    }
}


// bool AP_ExternalAHRS_MicroStrain7::detect_device(void) const// new method to distinguish between cv7 and gq7
// {
//     // Send the "Get Device Info" command
//     uint8_t command[2] = {0x01, 0x03}; // Command for "Get Device Info"
//     uart->write(command, sizeof(command));

//     // Wait for response
//     uint8_t response[256];
//     uint16_t response_len = uart->read(response, sizeof(response));

//     // Validate the response length
//     if (response_len < static_cast<uint16_t>(DeviceInfo::MIN_RESPONSE_LENGTH)) {
//         hal.console->printf("Device detection failed: Insufficient response\n");
//         return false;
//     }

//     // Extract the model number from the response (assuming it is a 16-character string)
//     char model_number[17] = {0}; // 16 characters + null terminator
//     memcpy(model_number, &response[static_cast<int>(DeviceInfo::MODEL_NUMBER_OFFSET)], 16);

//     // Compare the model number to known values
//     if (strcmp(model_number, "6286") == 0) {  // CV7 model number as string
//         is_cv7 = true;
//         hal.console->printf("Detected device: 3DM-CV7\n");
//     } else if (strcmp(model_number, "6284") == 0) {  // GQ7 model number as string
//         is_cv7 = false;
//         hal.console->printf("Detected device: 3DM-GQ7\n");
//     } else {
//         hal.console->printf("Device detection failed: Unknown model number\n");
//         return false;
//     }
//     return true;
// }



// 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  // updated method for 3dm-cv7 imu (sheteshreyash)
{
    {
        WITH_SEMAPHORE(state.sem);
        state.accel = imu_data.accel;
        state.gyro = imu_data.gyro;
    }

    {
        // *INDENT-OFF*
        AP_ExternalAHRS::ins_data_message_t ins {
            accel: imu_data.accel,
            gyro: imu_data.gyro,
            temperature: -300 // Placeholder for temperature
        };
        // *INDENT-ON*
        AP::ins().handle_external(ins);
    }

#if AP_COMPASS_EXTERNALAHRS_ENABLED
    {
    // Check if magnetometer data is valid and handle it for both CV7 and GQ7
    AP_ExternalAHRS::mag_data_message_t mag {
        field: imu_data.mag
    };
    AP::compass().handle_external(mag);
    }
#endif

#if AP_BARO_EXTERNALAHRS_ENABLED
    if (!is_cv7) { // Only handle barometer data for GQ7
        // *INDENT-OFF*
        const AP_ExternalAHRS::baro_data_message_t baro {
            instance: 0,
            pressure_pa: imu_data.pressure,
            temperature: 25, // Default temperature for barometer
        };
        // *INDENT-ON*
        AP::baro().handle_external(baro);
    }
#endif
}

// void AP_ExternalAHRS_MicroStrain7::post_filter() const  //sheteshreyash
// {
//     {
//         WITH_SEMAPHORE(state.sem);

//         // Update quaternion from filter data
//         state.quat = filter_data.attitude_quat;
//         state.have_quaternion = true;

//         // Update velocity from filter data
//         state.velocity = Vector3f{
//             filter_data.ned_velocity_north,
//             filter_data.ned_velocity_east,
//             filter_data.ned_velocity_down

//         };
//         state.have_velocity = true;

//         // Update location from filter data (if lat/lon/alt are valid)
//         if (filter_data.lat != 0 && filter_data.lon != 0) {
//             state.location = Location{
//                 filter_data.lat,
//                 filter_data.lon,
//                 filter_data.hae_altitude, // Using HAE altitude for height above ellipsoid
//                 Location::AltFrame::ABSOLUTE
//             };
//             state.have_location = true;
//         } else {
//             state.location = Location{}; // Set to invalid if lat/lon are zero
//             state.have_location = false;
//         }
//     }

//     // AHRS-specific functionality
//     // Process AHRS specific data (e.g., attitude, velocity, location, etc.)
//     {
//         // If the system has a valid origin, set the location to the origin when the first valid GNSS fix is received
//         if (state.have_origin) {
//             WITH_SEMAPHORE(state.sem);
//             // Check if the GNSS fix is 3D (good enough for origin)
//             if (!state.have_origin) {
//                 state.origin = Location{
//                     int32_t(filter_data.lat),
//                     int32_t(filter_data.lon),
//                     int32_t(filter_data.hae_altitude), // HAE altitude used
//                     Location::AltFrame::ABSOLUTE
//                 };
//                 state.have_origin = true;
//             }
//         }

//         // Example AHRS specific handling (attitude and velocity data)
//         // Assuming some functions or data structures are already defined to handle the IMU data:
//         // Process the AHRS attitude and velocity for output
//         Vector3f ahrs_velocity{filter_data.ned_velocity_north, filter_data.ned_velocity_east, filter_data.ned_velocity_down};
//         Vector3f ahrs_orientation{filter_data.attitude_quat.q1, filter_data.attitude_quat.q2, filter_data.attitude_quat.q3};
//         // Update other AHRS-specific data if necessary
//         // For example, if there are specific flags or calculations, you can do that here.
//         // Some custom logic specific to 3DM CV7 can go here (if needed).
//     }

//     // Handle GPS and GNSS data if available
//     for (int instance = 0; instance < NUM_GNSS_INSTANCES; instance++) {
//         // Process GNSS data as needed (assuming gnss_data and filter_data are properly updated)
//         AP_ExternalAHRS::gps_data_message_t gps{
//             gps_week: filter_data.week,
//             ms_tow: filter_data.tow_ms,
//             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,
//             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: gnss_data[instance].lon,
//             latitude: gnss_data[instance].lat,
//             msl_altitude: gnss_data[instance].msl_altitude,

//             ned_vel_north: gnss_data[instance].ned_velocity_north,
//             ned_vel_east: gnss_data[instance].ned_velocity_east,
//             ned_vel_down: gnss_data[instance].ned_velocity_down,
//         };

//         // Handle external GPS data in Ardupilot
//         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),
//                                     int32_t(gnss_data[instance].msl_altitude),
//                                     Location::AltFrame::ABSOLUTE};
//             state.have_origin = true;
//         }
//         AP::gps().handle_external(gps, instance);
//     }
// }


void AP_ExternalAHRS_MicroStrain7::post_filter() const  //sheteshreyash
{
    {
        WITH_SEMAPHORE(state.sem);
        // Publish filter velocity and quaternion data
        state.velocity = Vector3f{filter_data.ned_velocity_north, filter_data.ned_velocity_east, filter_data.ned_velocity_down};
        state.have_velocity = true;

        state.quat = filter_data.attitude_quat;
        state.have_quaternion = true;

        // Handle location data
        if (!is_cv7) {
            // For GQ7, use GNSS data for location
            state.location = Location{filter_data.lat, filter_data.lon, gnss_data[0].msl_altitude, Location::AltFrame::ABSOLUTE};
            state.have_location = true;
        } else {
            // For CV7, use filter data or set location as invalid
            state.location = Location{};
            state.have_location = false;
        }
    }

    // Publish GNSS data only for GQ7
    if (!is_cv7) {
        for (int instance = 0; instance < NUM_GNSS_INSTANCES; instance++) {
            // Prepare GNSS message
            AP_ExternalAHRS::gps_data_message_t gps{
                gps_week: filter_data.week,
                ms_tow: filter_data.tow_ms,
                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,
                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: gnss_data[instance].lon,
                latitude: gnss_data[instance].lat,
                msl_altitude: gnss_data[instance].msl_altitude,

                ned_vel_north: gnss_data[instance].ned_velocity_north,
                ned_vel_east: gnss_data[instance].ned_velocity_east,
                ned_vel_down: gnss_data[instance].ned_velocity_down,
            };

            // Update origin if not set and fix type is 3D or better
            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),
                                        int32_t(gnss_data[instance].msl_altitude),
                                        Location::AltFrame::ABSOLUTE};
                state.have_origin = true;
            }
            // Send GPS data to the AP framework
            AP::gps().handle_external(gps, instance);
        }
    }
}


int8_t AP_ExternalAHRS_MicroStrain7::get_port(void) const  //No changes
{
    if (!uart) {
        return -1;
    }
    return port_num;
};

// Get model/type name
const char* AP_ExternalAHRS_MicroStrain7::get_name() const  //No changes
{
    return "MicroStrain7";
}

bool AP_ExternalAHRS_MicroStrain7::healthy(void) const //No changes
{
    return times_healthy() && filter_healthy();
}

void AP_ExternalAHRS_MicroStrain7::get_filter_status(nav_filter_status &status) const  //No changes
{
    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<FilterState>(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;
        }
    }
}

// get variances
bool AP_ExternalAHRS_MicroStrain7::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const  //No changes
{
    velVar = filter_data.ned_velocity_uncertainty.length() * vel_gate_scale;
    posVar = filter_data.ned_position_uncertainty.xy().length() * pos_gate_scale;
    hgtVar = filter_data.ned_position_uncertainty.z * hgt_gate_scale;
    tasVar = 0;
    return true;
}

bool AP_ExternalAHRS_MicroStrain7::times_healthy() const   //No changes
{
    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);

    return times_healthy;
}

bool AP_ExternalAHRS_MicroStrain7::filter_healthy() const   //No changes
{
    const auto filter_state = static_cast<FilterState>(filter_status.state);
    const bool filter_healthy = filter_state_healthy(filter_state);
    return filter_healthy;
}

bool AP_ExternalAHRS_MicroStrain7::filter_state_healthy(FilterState state)    //No changes
{
    switch (state) {
    case FilterState::GQ7_FULL_NAV:
    case FilterState::GQ7_AHRS:
        return true;
    default:
        return false;
    }
}

#endif // AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED