/*
  implement OpenDroneID MAVLink and DroneCAN support
 */
/*
  released under GNU GPL v2 or later
 */

#include "options.h"
#include <Arduino.h>
#include "version.h"
#include <math.h>
#include <time.h>
#include <sys/time.h>
#include <opendroneid.h>
#include "cndroneid.h"
#include "mavlink.h"
#include "DroneCAN.h"
#include "WiFi_TX.h"
#include "BLE_TX.h"
#include <esp_wifi.h>
#include <WiFi.h>
#include "parameters.h"
#include "webinterface.h"
#include "check_firmware.h"
#include <esp_ota_ops.h>
#include "efuse.h"
#include "led.h"
// did_GB2025 是GB 46750—2025相关的协议实现
// cn_did     是2023年的试运行协议实现 本质与opendroneid没有太大区别
// 发给mavlink 的消息也会被调试串口转发 一个是心跳 一个是错误状态
#if AP_DRONECAN_ENABLED
static DroneCAN dronecan;
#endif

#if AP_MAVLINK_ENABLED
static MAVLinkSerial mavlink1{Serial1, MAVLINK_COMM_0};
static MAVLinkSerial mavlink2{Serial,  MAVLINK_COMM_1};
#endif

static WiFi_TX wifi;
static BLE_TX ble;

#define DEBUG_BAUDRATE 57600

// OpenDroneID output data structure
ODID_UAS_Data UAS_data;
CNDID_UAS_Data CN_UAS_data; // 国标输出结构体 每个报文的大小都是25字节 走转发
UavIdentificationData GB2025_UAS_data;
String status_reason;
static uint32_t last_location_ms;
static WebInterface webif;

#include "soc/soc.h"
#include "soc/rtc_cntl_reg.h"

static bool arm_check_ok = false; // goes true for LED arm check status
static bool pfst_check_ok = false;

void print_g_struct()
{
    Serial.println("\n========== GLOBAL STRUCTURE g ==========");
    
    // 基础配置
    Serial.printf("LOCK_LEVEL         = %d\n", g.lock_level);
    Serial.printf("CAN_NODE           = %u\n", g.can_node);
#if defined(PIN_CAN_TERM)
    Serial.printf("CAN_TERMINATE      = %u\n", g.can_term);
#endif
    
    // UAS 相关
    Serial.printf("UAS_TYPE           = %u\n", g.ua_type);
    Serial.printf("UAS_ID_TYPE        = %u\n", g.id_type);
    Serial.printf("UAS_ID             = %s\n", g.uas_id);
    Serial.printf("UAS_TYPE_2         = %u\n", g.ua_type_2);
    Serial.printf("UAS_ID_TYPE_2      = %u\n", g.id_type_2);
    Serial.printf("UAS_ID_2           = %s\n", g.uas_id_2);
    
    // 通信配置
    Serial.printf("BAUDRATE           = %lu\n", g.baudrate);
    Serial.printf("WIFI_NAN_RATE      = %.2f\n", g.wifi_nan_rate);
    Serial.printf("WIFI_BEACON_RATE   = %.2f\n", g.wifi_beacon_rate);
    Serial.printf("WIFI_POWER         = %.2f dBm\n", g.wifi_power);
    
    // 蓝牙配置（重点！）
    Serial.printf("BT4_RATE           = %.2f Hz\n", g.bt4_rate);
    Serial.printf("BT4_POWER          = %.2f dBm\n", g.bt4_power);
    Serial.printf("BT5_RATE           = %.2f Hz\n", g.bt5_rate);
    Serial.printf("BT5_POWER          = %.2f dBm\n", g.bt5_power);
    
    // Web 和 WiFi
    Serial.printf("WEBSERVER_EN       = %u\n", g.webserver_enable);
    Serial.printf("WIFI_SSID          = %s\n", g.wifi_ssid);
    Serial.printf("WIFI_PASSWORD      = %s\n", g.wifi_password);  // 注意：生产环境慎打密码
    Serial.printf("WIFI_CHANNEL       = %u\n", g.wifi_channel);
    
    // 其他配置
    Serial.printf("BCAST_POWERUP      = %u\n", g.bcast_powerup);
    Serial.printf("MAVLINK_SYSID      = %u\n", g.mavlink_sysid);
    Serial.printf("OPTIONS            = %u\n", g.options);
    Serial.printf("TO_DEFAULTS        = %u\n", g.to_factory_defaults);
    Serial.printf("DONE_INIT          = %u\n", g.done_init);
    Serial.printf("PROTOCOL           = %u\n", g.protocol);
    Serial.println("==========================================\n");
}

/*
  setup serial ports
 */
void setup()
{
    // delay(2000);

    // disable brownout checking
    WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);

    g.init();
    // g.protocol = 2;
  
    led.set_state(Led::LedState::INIT);
    led.update();

    // Serial for debug printf
    Serial.begin(g.baudrate);

    // print_g_struct();
    // g.protocol = 1;
    // Serial1 for MAVLink
    Serial1.begin(g.baudrate, SERIAL_8N1, PIN_UART_RX, PIN_UART_TX);

    if (g.webserver_enable) {
        // need WiFi for web server
        wifi.init();
    }

    // set all fields to invalid/initial values
    odid_initUasData(&UAS_data);
    cndid_initUasData(&CN_UAS_data);
    uav_identification_data_init(&GB2025_UAS_data);
#if AP_MAVLINK_ENABLED
    mavlink1.init();
    mavlink2.init();
#endif
#if AP_DRONECAN_ENABLED
    dronecan.init();
#endif

    set_efuses();

    CheckFirmware::check_OTA_running();

#if defined(PIN_CAN_EN)
    // optional CAN enable pin
    pinMode(PIN_CAN_EN, OUTPUT);
    digitalWrite(PIN_CAN_EN, HIGH);
#endif

#if defined(PIN_CAN_nSILENT)
    // disable silent pin
    pinMode(PIN_CAN_nSILENT, OUTPUT);
    digitalWrite(PIN_CAN_nSILENT, HIGH);
#endif

#if defined(PIN_CAN_TERM)
#if !defined(CAN_TERM_EN)
#define CAN_TERM_EN HIGH
#endif

    // optional CAN termination control
    pinMode(PIN_CAN_TERM, OUTPUT);

    if (g.can_term == 1) {
        digitalWrite(PIN_CAN_TERM, CAN_TERM_EN);
    } else {
        digitalWrite(PIN_CAN_TERM, !CAN_TERM_EN);
    }
#endif

#if defined(BUZZER_PIN)
    //set BuZZER OUTPUT ACTIVE, just to show it works
    pinMode(GPIO_NUM_39, OUTPUT);
    digitalWrite(GPIO_NUM_39, HIGH);
#endif

    pfst_check_ok = true;   // note - this will need to be expanded to better capture PFST test status

    // initially set LED for fail
    led.set_state(Led::LedState::ARM_FAIL);

    esp_log_level_set("*", ESP_LOG_DEBUG);

    esp_ota_mark_app_valid_cancel_rollback();
}
#define IMIN(x,y) ((x)<(y)?(x):(y))
#define ODID_COPY_STR(to, from) strncpy(to, (const char*)from, IMIN(sizeof(to), sizeof(from)))
/**
 * mavlink 到 GB2025 转换
 */
int odid_to_gb2025_safe(const ODID_UAS_Data *odid, UavIdentificationData *gb) {
    if (!odid || !gb) return -1;
    
    uav_identification_data_init(gb);
    gb->coord_type = COORD_WGS84;
    
    /* ========== 001-唯一产品识别码 ========== */
    if (odid->BasicIDValid[0]) {
        // BasicID总是有值的，即使无效也有默认值
        memcpy(gb->unique_code, odid->BasicID[0].UASID, 20);
        // 不需要有效性标志，因为这是固定字段
    }
    
    /* ========== 002-实名登记标志 ========== */
    if (odid->OperatorIDValid) {
        char last_8[9] = {0};
        size_t actual_len = strlen(odid->OperatorID.OperatorId);

        if (actual_len >= 8) {
            strcpy(last_8, &odid->OperatorID.OperatorId[actual_len - 8]);
        }
        // 当长度<8时，last_8全为0，复制过去就是8个0x00
        memcpy(gb->register_id, last_8, 8);
    }
    
    /* ========== 003-运行类别 ========== */
    if (odid->SystemValid) {
        // 检查枚举值范围
        if (odid->System.CategoryEU >= 0 && odid->System.CategoryEU < 4) {
            gb->run_class = (UavRunClass)odid->System.CategoryEU;
            gb->flag_run_class = true;
        } else {
            gb->run_class = UAV_RUN_UNDEFINED;
            gb->flag_run_class = false;  // 标记为未收到有效值
        }
    }
    
    /* ========== 004-无人机分类 ========== */
    if (odid->BasicIDValid[0]) {
        // 检查UAType是否在有效范围内
        if (odid->BasicID[0].UAType >= 0 && (UavType)odid->BasicID[0].UAType < 5) {
            gb->uav_type = (UavType)odid->BasicID[0].UAType;
        } else {
            gb->uav_type = UAV_TYPE_RESERVED;  // 设为预留值
        }
    }
    
    /* ========== 006/008-经纬度处理 ========== */
    if (odid->LocationValid) {
        // 检查经纬度是否有效
        bool lat_valid = (odid->Location.Latitude >= -90 && 
                          odid->Location.Latitude <= 90 &&
                          odid->Location.Latitude != 0);  // 0通常是无效值
        
        bool lon_valid = (odid->Location.Longitude >= -180 && 
                          odid->Location.Longitude <= 180 &&
                          odid->Location.Longitude != 0);
        
        if (lat_valid && lon_valid) {
            gb->uav_lat = odid->Location.Latitude;
            gb->uav_lon = odid->Location.Longitude;
            gb->is_uav_pos_valid = true;
        } else {
            // 非法经纬度：标记为无效，编码时会自动处理
            gb->uav_lat = 0;
            gb->uav_lon = 0;
            gb->is_uav_pos_valid = false;
        }
    }
    
    /* ========== 009-航迹角 ========== */
    if (odid->LocationValid) {
        // 航迹角有效范围：0-360，且不是无效值361
        if (odid->Location.Direction >= 0 && 
            odid->Location.Direction <= 360 && 
            odid->Location.Direction != INV_DIR) {
            gb->track_angle = odid->Location.Direction;
            gb->is_track_angle_valid = true;
        } else {
            gb->track_angle = 0;
            gb->is_track_angle_valid = false;
        }
    }
    
    /* ========== 010-地速 ========== */
    if (odid->LocationValid) {
        // 地速有效范围：0-254.25，且不是无效值255
        if (odid->Location.SpeedHorizontal >= 0 && 
            odid->Location.SpeedHorizontal <= MAX_SPEED_H &&
            odid->Location.SpeedHorizontal != INV_SPEED_H) {
            gb->ground_speed = odid->Location.SpeedHorizontal;
            gb->is_ground_speed_valid = true;
        } else {
            gb->ground_speed = 0;
            gb->is_ground_speed_valid = false;
        }
    }
    
    /* ========== 012-垂直速度 ========== */
    if (odid->LocationValid) {
        // 垂直速度有效范围：-62 到 62，且不是无效值63
        if (odid->Location.SpeedVertical >= MIN_SPEED_V && 
            odid->Location.SpeedVertical <= MAX_SPEED_V &&
            odid->Location.SpeedVertical != INV_SPEED_V) {
            gb->uav_vert_speed = odid->Location.SpeedVertical;
            gb->is_vert_speed_valid = true;
            gb->flag_vert_speed = true;
        } else {
            gb->uav_vert_speed = 0;
            gb->is_vert_speed_valid = false;
            gb->flag_vert_speed = false;  // 标记为未收到
        }
    }
    
    /* ========== 高度类处理（013/014/007/011） ========== */

    if (odid->LocationValid) {
        // Serial.printf("Geo altitude %f \r\n", odid->Location.AltitudeGeo);
        // Serial.printf("Rel Height %f \r\n", odid->Location.Height);
        // Serial.printf("Baro altitude %f \r\n", odid->Location.AltitudeBaro);
        // Serial.printf("time stamp %f \r\n", odid->Location.TimeStamp);
        // 大地高度
        if (odid->Location.AltitudeGeo > MIN_ALT && 
            odid->Location.AltitudeGeo <= MAX_ALT) {
            
            gb->uav_geo_alt = odid->Location.AltitudeGeo;
            gb->is_uav_geo_alt_valid = true;
        } else {
            gb->uav_geo_alt = 0;
            gb->is_uav_geo_alt_valid = false;
        }
        
        // 气压高度
       
        if (odid->Location.AltitudeBaro > MIN_ALT && 
            odid->Location.AltitudeBaro <= MAX_ALT) {
            
            gb->uav_press_alt = odid->Location.AltitudeBaro;
            gb->is_press_alt_valid = true;
            gb->flag_press_alt = true;
        } else {
            gb->uav_press_alt = 0;
            gb->is_press_alt_valid = false;
            gb->flag_press_alt = false;
        }
        
        // 相对高度
      
        if (odid->Location.Height > MIN_ALT && 
            odid->Location.Height <= MAX_ALT) {
            gb->uav_rel_alt = odid->Location.Height;
           
            gb->is_rel_alt_valid = true;
            gb->flag_rel_alt = true;
        } else {
            gb->uav_rel_alt = 0;
            gb->is_rel_alt_valid = false;
            gb->flag_rel_alt = false;
        }
    }
    
    /* ========== 015-运行状态 ========== */
    if (odid->LocationValid) {
        // 检查状态值范围
        if (odid->Location.Status >= 0 && odid->Location.Status < 6) {
            gb->run_state = (UavRunState)odid->Location.Status;
        } else {
            gb->run_state = UAV_STATE_UNREPORTED;
        }
    }
    
    /* ========== 精度枚举值处理 ========== */
    if (odid->LocationValid) {
        // 水平精度（0-15有效）
        gb->hori_accuracy = (odid->Location.HorizAccuracy >= 0 && odid->Location.HorizAccuracy <= 12) ? 
                             (UavHorizontalAccuracy_t)odid->Location.HorizAccuracy : UAV_HOR_ACC_UNKNOWN;
        
        // 垂直精度（0-6有效，7-15预留）
        gb->vert_accuracy = (odid->Location.VertAccuracy >=0 && odid->Location.VertAccuracy <= 6) ? 
                             (UavVerticalAccuracy_t)odid->Location.VertAccuracy : UAV_VER_ACC_UNKNOWN;
        
        // 速度精度（0-4有效，5-15预留）
        gb->speed_accuracy = (odid->Location.SpeedAccuracy >=0 && odid->Location.SpeedAccuracy <= 4) ? 
                              (UAVSpeedAccuracy_t)odid->Location.SpeedAccuracy : UAV_SPEED_ACC_UNKNOWN;
        
        // 时间戳精度（0-8都有效）
        gb->time_accuracy = (odid->Location.TSAccuracy >= 0 && odid->Location.TSAccuracy <= 8)?
                                (UavTimestampAccuracy_t)odid->Location.TSAccuracy : UAV_TIME_ACC_UNKNOWN;
    }
    
    /* ========== 遥控站位置 时间戳========== */
    if (odid->SystemValid) {
        // 检查遥控站经纬度
        bool sta_lat_valid = (odid->System.OperatorLatitude >= -90 && 
                               odid->System.OperatorLatitude <= 90);
        bool sta_lon_valid = (odid->System.OperatorLongitude >= -180 && 
                               odid->System.OperatorLongitude <= 180);
        
        if (sta_lat_valid && sta_lon_valid && 
            (odid->System.OperatorLatitude != 0 || odid->System.OperatorLongitude != 0)) {
            gb->station_lat = odid->System.OperatorLatitude;
            gb->station_lon = odid->System.OperatorLongitude;
            gb->is_station_pos_valid = true;
        } else {
            gb->station_lat = 0;
            gb->station_lon = 0;
            gb->is_station_pos_valid = false;
        }
        
        // 遥控站高度
        if (odid->System.OperatorAltitudeGeo > MIN_ALT && 
            odid->System.OperatorAltitudeGeo <= MAX_ALT) {
            gb->station_geo_alt = odid->System.OperatorAltitudeGeo;
            gb->is_station_alt_valid = true;
        } else {
            gb->station_geo_alt = 0;
            gb->is_station_alt_valid = false;
        }

        if (odid->System.Timestamp < 0)
        {
            gb->timestamp = 0;
            gb->is_timestamp_valid = false;
           
        } else {
            gb->timestamp = (uint64_t)(odid->System.Timestamp * 1000); // 使用unix时间 单位ms
            gb->is_timestamp_valid = true;
        }

    }
    
    return 0;
}
/*
  check parsing of UAS_data, this checks ranges of values to ensure we
  will produce a valid pack
  returns nullptr on no error, or a string error
  检查UAS数据的解析情况，这会检查值的范围以确保我们能生成一个有效的数据包。
  无错误时返回空指针，否则返回字符串形式的错误信息。
 */
static const char *check_parse(void)
{
    String ret = "";
    
    {
        ODID_Location_encoded encoded {};
        if (encodeLocationMessage(&encoded, &UAS_data.Location) != ODID_SUCCESS) {
            ret += "LOC ";
        }
    }
    {
        ODID_System_encoded encoded {};
        if (encodeSystemMessage(&encoded, &UAS_data.System) != ODID_SUCCESS) {
            ret += "SYS ";
        }
    }
    {
        ODID_BasicID_encoded encoded {};
        if (UAS_data.BasicIDValid[0] == 1) {
            if (encodeBasicIDMessage(&encoded, &UAS_data.BasicID[0]) != ODID_SUCCESS) {
                ret += "ID_1 ";
            }
        }
        memset(&encoded, 0, sizeof(encoded));
        if (UAS_data.BasicIDValid[1] == 1) {
            if (encodeBasicIDMessage(&encoded, &UAS_data.BasicID[1]) != ODID_SUCCESS) {
                ret += "ID_2 ";
            }
        }
    }
    {
        ODID_SelfID_encoded encoded {};
        if (encodeSelfIDMessage(&encoded, &UAS_data.SelfID) != ODID_SUCCESS) {
            ret += "SELF_ID ";
        }
    }
    {
        ODID_OperatorID_encoded encoded {};
        if (encodeOperatorIDMessage(&encoded, &UAS_data.OperatorID) != ODID_SUCCESS) {
            ret += "OP_ID ";
        }
    }
       
    if (ret.length() > 0) {
        // if all errors would occur in this function, it will fit in
        // 50 chars that is also the max for the arm status message
        // 如果所有错误都发生在这个函数中，它会刚好容纳
        // 50个字符，这也是臂状态消息的最大长度
        static char return_string[50];
        memset(return_string, 0, sizeof(return_string));
        snprintf(return_string, sizeof(return_string)-1, "bad %s data", ret.c_str());
        Serial.printf("error %s\n", return_string);
        return return_string;
    }
    return nullptr;
}

/*
  fill in UAS_data from MAVLink packets
 */
static void set_data(Transport &t) // 两种类型都获取 广播按照需要的进行广播
{   // 从mavlink获取数据包
    const auto &operator_id = t.get_operator_id(); 
    const auto &basic_id = t.get_basic_id();
    const auto &system = t.get_system();
    const auto &self_id = t.get_self_id();
    const auto &location = t.get_location();

    odid_initUasData(&UAS_data);
    cndid_initUasData(&CN_UAS_data); // 复位UAS数据结构体
    uav_identification_data_init(&GB2025_UAS_data);
    /*
      if we don't have BasicID info from parameters and we have it
      from the DroneCAN or MAVLink transport then copy it to the
      parameters to persist it. This makes it possible to set the
      UAS_ID string via a MAVLink BASIC_ID message and also offers a
      migration path from the old approach of GCS setting these values
      to having them as parameters

      BasicID 2 can be set in parameters, or provided via mavlink We
      don't persist the BasicID2 if provided via mavlink to allow
      users to change BasicID2 on different days
     */
    if (!g.have_basic_id_info() && !(g.options & OPTIONS_DONT_SAVE_BASIC_ID_TO_PARAMETERS)) {
        if (basic_id.ua_type != 0 &&
            basic_id.id_type != 0 &&
            strnlen((const char *)basic_id.uas_id, 20) > 0) {
            g.set_by_name_uint8("UAS_TYPE", basic_id.ua_type);
            g.set_by_name_uint8("UAS_ID_TYPE", basic_id.id_type);
            char uas_id[21] {};
            ODID_COPY_STR(uas_id, basic_id.uas_id);
            g.set_by_name_string("UAS_ID", uas_id);
        }
    } // 是否配置了保存基本ID的选项 == 存到eeprom 可以用国际的协议存储BASE ID
    
    // BasicID
    if (g.have_basic_id_info() && !(g.options & OPTIONS_DONT_SAVE_BASIC_ID_TO_PARAMETERS)) {
        // from parameters
        UAS_data.BasicID[0].UAType = (ODID_uatype_t)g.ua_type;
        UAS_data.BasicID[0].IDType = (ODID_idtype_t)g.id_type;
        // 国际国标 的 基础ID参数是一样的 拷贝
        CN_UAS_data.BasicID[0].UAType = (CNDID_UAType_t)g.ua_type;
        CN_UAS_data.BasicID[0].IDType = (CNDID_IDType_t)g.id_type;

        ODID_COPY_STR(UAS_data.BasicID[0].UASID, g.uas_id);
        ODID_COPY_STR(CN_UAS_data.BasicID[0].UASID, g.uas_id); // 拷贝

        UAS_data.BasicIDValid[0] = 1;
        CN_UAS_data.BasicIDValid[0] = 1; // 拷贝

        // BasicID 2
        if (g.have_basic_id_2_info()) {
            // from parameters
            UAS_data.BasicID[1].UAType = (ODID_uatype_t)g.ua_type_2;
            UAS_data.BasicID[1].IDType = (ODID_idtype_t)g.id_type_2;
            
            // 国际国标 的 基础ID参数是一样的 拷贝
            CN_UAS_data.BasicID[1].UAType = (CNDID_UAType_t)g.ua_type;
            CN_UAS_data.BasicID[1].IDType = (CNDID_IDType_t)g.id_type;

            ODID_COPY_STR(UAS_data.BasicID[1].UASID, g.uas_id_2);
            ODID_COPY_STR(CN_UAS_data.BasicID[1].UASID, g.uas_id_2); // 拷贝


            UAS_data.BasicIDValid[1] = 1;
            CN_UAS_data.BasicIDValid[1] = 1; // 拷贝
        } else if (strcmp((const char*)g.uas_id, (const char*)basic_id.uas_id) != 0) {
            /*
              no BasicID 2 in the parameters, if one is provided on MAVLink
              and it is a different uas_id from the basicID1 then use it as BasicID2
            */
            if (basic_id.ua_type != 0 &&
                basic_id.id_type != 0 &&
                strnlen((const char *)basic_id.uas_id, 20) > 0) {
                UAS_data.BasicID[1].UAType = (ODID_uatype_t)basic_id.ua_type;
                UAS_data.BasicID[1].IDType = (ODID_idtype_t)basic_id.id_type;
                ODID_COPY_STR(UAS_data.BasicID[1].UASID, basic_id.uas_id);
                UAS_data.BasicIDValid[1] = 1;
                // 拷贝
                CN_UAS_data.BasicID[1].UAType = (CNDID_UAType_t)basic_id.ua_type;
                CN_UAS_data.BasicID[1].IDType = (CNDID_IDType_t)basic_id.id_type;
                ODID_COPY_STR(CN_UAS_data.BasicID[1].UASID, basic_id.uas_id);
                CN_UAS_data.BasicIDValid[1] = 1;
            }
        }
    }

    if (g.options & OPTIONS_DONT_SAVE_BASIC_ID_TO_PARAMETERS) {
        if (basic_id.ua_type != 0 &&
        basic_id.id_type != 0 &&
        strnlen((const char *)basic_id.uas_id, 20) > 0) {
            if (strcmp((const char*)UAS_data.BasicID[0].UASID, (const char*)basic_id.uas_id) != 0 && strnlen((const char *)basic_id.uas_id, 20) > 0) {
                UAS_data.BasicID[1].UAType = (ODID_uatype_t)basic_id.ua_type;
                UAS_data.BasicID[1].IDType = (ODID_idtype_t)basic_id.id_type;
                ODID_COPY_STR(UAS_data.BasicID[1].UASID, basic_id.uas_id);
                UAS_data.BasicIDValid[1] = 1;

                 // 拷贝
                CN_UAS_data.BasicID[1].UAType = (CNDID_UAType_t)basic_id.ua_type;
                CN_UAS_data.BasicID[1].IDType = (CNDID_IDType_t)basic_id.id_type;
                ODID_COPY_STR(CN_UAS_data.BasicID[1].UASID, basic_id.uas_id);
                CN_UAS_data.BasicIDValid[1] = 1;
            } else {
                UAS_data.BasicID[0].UAType = (ODID_uatype_t)basic_id.ua_type;
                UAS_data.BasicID[0].IDType = (ODID_idtype_t)basic_id.id_type;
                ODID_COPY_STR(UAS_data.BasicID[0].UASID, basic_id.uas_id);
                UAS_data.BasicIDValid[0] = 1;
                // 拷贝
                CN_UAS_data.BasicID[0].UAType = (CNDID_UAType_t)basic_id.ua_type;
                CN_UAS_data.BasicID[0].IDType = (CNDID_IDType_t)basic_id.id_type;
                ODID_COPY_STR(CN_UAS_data.BasicID[0].UASID, basic_id.uas_id); // 拷贝
                CN_UAS_data.BasicIDValid[0] = 1; // 拷贝
            }
        }
    }
    
    // OperatorID
    if (strlen(operator_id.operator_id) > 0) {
        UAS_data.OperatorID.OperatorIdType = (ODID_operatorIdType_t)operator_id.operator_id_type;
        ODID_COPY_STR(UAS_data.OperatorID.OperatorId, operator_id.operator_id);
        UAS_data.OperatorIDValid = 1;
    }
    // 
    // SelfID
    if (strlen(self_id.description) > 0) {
        UAS_data.SelfID.DescType = (ODID_desctype_t)self_id.description_type;
        ODID_COPY_STR(UAS_data.SelfID.Desc, self_id.description);
        UAS_data.SelfIDValid = 1;
    }
    // SelfID CN
    if (strlen(self_id.description) > 0) {
        CN_UAS_data.SelfID.DescType= (CNDID_DescType_t)self_id.description_type;
        ODID_COPY_STR(CN_UAS_data.SelfID.Desc, self_id.description); // 拷贝函数通用
        CN_UAS_data.SelfIDValid = 1;
    }

    // System
    if (system.timestamp != 0) {
        UAS_data.System.OperatorLocationType = (ODID_operator_location_type_t)system.operator_location_type;
        UAS_data.System.ClassificationType = (ODID_classification_type_t)system.classification_type;
        UAS_data.System.OperatorLatitude = system.operator_latitude * 1.0e-7;
        UAS_data.System.OperatorLongitude = system.operator_longitude * 1.0e-7;
        UAS_data.System.AreaCount = system.area_count;
        UAS_data.System.AreaRadius = system.area_radius;
        UAS_data.System.AreaCeiling = system.area_ceiling;
        UAS_data.System.AreaFloor = system.area_floor;
        UAS_data.System.CategoryEU = (ODID_category_EU_t)system.category_eu;
        UAS_data.System.ClassEU = (ODID_class_EU_t)system.class_eu;
        UAS_data.System.OperatorAltitudeGeo = system.operator_altitude_geo;
        UAS_data.System.Timestamp = system.timestamp;
        UAS_data.SystemValid = 1;
    }
    // System CN
    if (system.timestamp != 0) {

        CN_UAS_data.System.Coord_Type = (CNDID_CoordType_t)CN_COORD_TYPE; // 坐标系建议写死
        CN_UAS_data.System.OperatorLocationType = (CNDID_operator_location_type_t)system.operator_location_type;
        CN_UAS_data.System.Classification_Type = (CNDID_classification_type_t)system.classification_type;
        CN_UAS_data.System.OperatorLatitude = system.operator_latitude * 1.0e-7;
        CN_UAS_data.System.OperatorLongitude = system.operator_longitude * 1.0e-7;
        CN_UAS_data.System.AreaCount = system.area_count;
        CN_UAS_data.System.AreaRadius = system.area_radius;
        CN_UAS_data.System.AreaCeiling = system.area_ceiling;
        CN_UAS_data.System.AreaFloor = system.area_floor;
        CN_UAS_data.System.CategoryCN = (CNDID_category_CN_t)system.category_eu; // 右边是mvlink自动生成的结构体成员不能更改
        CN_UAS_data.System.ClassCN = (CNDID_class_CN_t)system.class_eu; // 右边是mvlink自动生成的结构体成员不能更改 

        CN_UAS_data.System.OperatorAltitudeGeo = system.operator_altitude_geo;
        CN_UAS_data.System.Timestamp = system.timestamp;
        CN_UAS_data.SystemValid = 1;
    }
    // Location
    if (location.timestamp != 0) {
        UAS_data.Location.Status = (ODID_status_t)location.status;
        UAS_data.Location.Direction = location.direction*0.01;
        UAS_data.Location.SpeedHorizontal = location.speed_horizontal*0.01;
        UAS_data.Location.SpeedVertical = location.speed_vertical*0.01;
        UAS_data.Location.Latitude = location.latitude*1.0e-7;
        UAS_data.Location.Longitude = location.longitude*1.0e-7;
        UAS_data.Location.AltitudeBaro = location.altitude_barometric;
        UAS_data.Location.AltitudeGeo = location.altitude_geodetic;
        UAS_data.Location.HeightType = (ODID_Height_reference_t)location.height_reference;
        UAS_data.Location.Height = location.height;
        UAS_data.Location.HorizAccuracy = (ODID_Horizontal_accuracy_t)location.horizontal_accuracy;
        UAS_data.Location.VertAccuracy = (ODID_Vertical_accuracy_t)location.vertical_accuracy;
        UAS_data.Location.BaroAccuracy = (ODID_Vertical_accuracy_t)location.barometer_accuracy;
        UAS_data.Location.SpeedAccuracy = (ODID_Speed_accuracy_t)location.speed_accuracy;
        UAS_data.Location.TSAccuracy = (ODID_Timestamp_accuracy_t)location.timestamp_accuracy;
        UAS_data.Location.TimeStamp = location.timestamp;
        UAS_data.LocationValid = 1;
    }

       // Location CN
    if (location.timestamp != 0) {
        CN_UAS_data.Location.Status = (CNDID_Status_t)location.status;
        CN_UAS_data.Location.Direction = location.direction*0.01;
        CN_UAS_data.Location.SpeedHorizontal = location.speed_horizontal*0.01;
        CN_UAS_data.Location.SpeedVertical = location.speed_vertical*0.01;
        CN_UAS_data.Location.Latitude = location.latitude*1.0e-7;
        CN_UAS_data.Location.Longitude = location.longitude*1.0e-7;
        CN_UAS_data.Location.AltitudeBaro = location.altitude_barometric;
        CN_UAS_data.Location.AltitudeGeo = location.altitude_geodetic;
        CN_UAS_data.Location.HeightType = (CNDID_HeightReference_t)location.height_reference;
        CN_UAS_data.Location.Height = location.height;
        CN_UAS_data.Location.HorizAccuracy = (CNDID_HorizontalAccuracy_t)location.horizontal_accuracy;
        CN_UAS_data.Location.VertAccuracy = (CNDID_VerticalAccuracy_t)location.vertical_accuracy;
        CN_UAS_data.Location.BaroAccuracy = (CNDID_VerticalAccuracy_t)location.barometer_accuracy;
        CN_UAS_data.Location.SpeedAccuracy = (CNDID_SpeedAccuracy_t)location.speed_accuracy;
        CN_UAS_data.Location.TSAccuracy = (CNDID_TimeStampAccuracy_t)location.timestamp_accuracy;
        CN_UAS_data.Location.TimeStamp = location.timestamp;
        CN_UAS_data.LocationValid = 1;
    }
   // GB2025 数据转换 
   odid_to_gb2025_safe(&UAS_data, &GB2025_UAS_data);
    // memcpy(GB2025_UAS_data.unique_code, basic_id.uas_id, 20);
    // memcpy(GB2025_UAS_data.register_id, operator_id.operator_id, 8); // 从操作员ID里复用
    // GB2025_UAS_data.uav_type = basic_id.ua_type;
    // GB2025_UAS_data.run_class = system.category_eu;
    // GB2025_UAS_data.station_type = system.operator_location_type;
    // GB2025_UAS_data.station_lat = system.operator_latitude * 1.0e-7;
    // GB2025_UAS_data.station_lon = system.operator_longitude * 1.0e-7;
    // GB2025_UAS_data.station_geo_alt = system.operator_altitude_geo;

    // GB2025_UAS_data.uav_lat = location.latitude*1.0e-7;
    // GB2025_UAS_data.uav_lon =  location.longitude*1.0e-7;
    // GB2025_UAS_data.track_angle = location.direction*0.01;
    // GB2025_UAS_data.ground_speed = location.speed_horizontal*0.01;
    // GB2025_UAS_data.uav_rel_alt = location.height_reference;
    // GB2025_UAS_data.uav_vert_speed = location.speed_vertical*0.01;
    // GB2025_UAS_data.uav_geo_alt = location.altitude_geodetic;
    // GB2025_UAS_data.uav_press_alt = location.altitude_barometric;

    // GB2025_UAS_data.run_state = location.status;
    // GB2025_UAS_datacoord_type = COORD_WGS84;
    // GB2025_UAS_data.hori_accuracy = location.horizontal_accuracy;
    // GB2025_UAS_data.vert_accuracy = location.vertical_accuracy;
    // GB2025_UAS_data.speed_accuracy = location.speed_accuracy;
    // GB2025_UAS_data.timestamp = (uint64_t)(odid->Location.TimeStamp * 1000);;
    // GB2025_UAS_data.time_accuracy = location.timestamp_accuracy;

    const char *reason = check_parse(); // 内部以区分国标 国际
    t.arm_status_check(reason);         // 内部以区分国标 国际
    t.set_parse_fail(reason);           // 设置错误原因 

    arm_check_ok = (reason==nullptr);
    
    if (g.options & OPTIONS_FORCE_ARM_OK) { // 设置强制绿灯？
        arm_check_ok = true;
    }
    led.set_state(pfst_check_ok && arm_check_ok? Led::LedState::ARM_OK : Led::LedState::ARM_FAIL);

    uint32_t now_ms = millis();
    uint32_t location_age_ms = now_ms - t.get_last_location_ms();
    uint32_t last_location_age_ms = now_ms - last_location_ms;
    if (location_age_ms < last_location_age_ms) {
        last_location_ms = t.get_last_location_ms();
    }
}

static uint8_t loop_counter = 0;

Transport& get_transport() {
    if (g.com_port == CAN_PORT) {
        return dronecan;      
    } else if (g.com_port == UART_PORT) {
        return mavlink1;      
    }else{
        return dronecan; 
    }
}

void loop()
{
#if AP_MAVLINK_ENABLED
    mavlink1.update(); // mavlink实际使用该串口
    mavlink2.update(); 
#endif
#if AP_DRONECAN_ENABLED
    dronecan.update(); // 开启了drone can 
#endif

    const uint32_t now_ms = millis();

    // the transports have common static data, so we can just use the
    // first for status
    // 这些传输工具具有共同的静态数据，所以我们只需使用第一个来获取状态
//  #if AP_MAVLINK_ENABLED
//      auto &transport = mavlink1; // 默认使用串口接收 mavlink数据
//  #elif AP_DRONECAN_ENABLED
//      auto &transport = dronecan;
//  #else
//      #error "Must enable DroneCAN or MAVLink"
//  #endif


    // 使用
    Transport &transport = get_transport();

    bool have_location = false;
    // - last_location_ms：最后一次收到有效位置数据的时间戳（毫秒）
    // - last_system_ms：最后一次收到飞控系统消息的时间戳（毫秒）
    const uint32_t last_location_ms = transport.get_last_location_ms();
    const uint32_t last_system_ms = transport.get_last_system_ms();

    led.update();

    status_reason = ""; //  初始化故障原因描述（默认空）

    if (last_location_ms == 0 ||
        now_ms - last_location_ms > 5000) { 
       
        UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE; // 相同的故障ID 4 // web页面可视
        CN_UAS_data.Location.Status = CNDID_STATUS_REMOTE_ID_SYSTEM_FAILURE; 
    }

    if (last_system_ms == 0 ||
        now_ms - last_system_ms > 5000) { 
       
        UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
        CN_UAS_data.Location.Status = CNDID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
    }
    //  判定条件3：数据解析失败（飞控消息格式错误/解析出错）
    if (transport.get_parse_fail() != nullptr) {
        UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
        CN_UAS_data.Location.Status = CNDID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
       
        status_reason = String(transport.get_parse_fail()); // 状态原因上传web页面
    } 

    // web update has to happen after we update Status above
    // 网络更新必须在我们更新上述状态之后进行
    if (g.webserver_enable) {
        webif.update();
    } 

    if (g.bcast_powerup) { // true 上电即广播"模式
        // if we are broadcasting on powerup we always mark location valid
        // so the location with default data is sent 
        // 如果我们在启动时进行广播，我们总是会标记位置有效
        // 这样就会发送带有默认数据的位置信息
        if (!UAS_data.LocationValid) {
            UAS_data.Location.Status = ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE;
            UAS_data.LocationValid = 1;
        }
    } else {
        // only broadcast if we have received a location at least once
        // 仅在我们至少接收过一次位置信息时才进行广播
        if (last_location_ms == 0) {
            delay(1); // 没接收到位置信息直接退出该次loop 执行完毕后还会再次进入
            return;
        }
    }

    set_data(transport);

    // Serial.printf("/***********************************/\n");
    // Serial.printf("ID %s %s\n",UAS_data.BasicID[0].UASID, UAS_data.BasicID[1].UASID);
    // Serial.printf("OP ID %s\n",UAS_data.OperatorID.OperatorId);
    // Serial.printf("sys timestamp %d %d\n",UAS_data.System.Timestamp, UAS_data.System.OperatorLocationType);
    // Serial.printf("loc timestamp %d %d\n",UAS_data.Location.TimeStamp, UAS_data.Location);
    // Serial.printf("/***********************************/\n");

    static uint32_t last_update_wifi_nan_ms; // wifi_nan_rate发送频率HZ 达到对应的时间了就启动RemoteID WIFI_NAN发送
    if (g.wifi_nan_rate > 0 &&
        now_ms - last_update_wifi_nan_ms > 1000/g.wifi_nan_rate) {
        last_update_wifi_nan_ms = now_ms;
        if(g.protocol == PROTOCOL_EU)
            wifi.transmit_nan(UAS_data);
        else if(g.protocol == PROTOCOL_CN2023)
            wifi.transmit_cn_nan(CN_UAS_data); // 这个可要可不要 国标只要求发送wifi beacon 信标帧
        else if(g.protocol == PROTOCOL_GB2025)
            wifi.transmit_GB2025_nan(GB2025_UAS_data);
        // Serial.printf("update_wifi_nan\n");
    }

    static uint32_t last_update_wifi_beacon_ms; // wifi_beacon_rate发送频率HZ 达到对应的时间了就启动RemoteID WIFI_BEACON发送
    if (g.wifi_beacon_rate > 0 &&
        now_ms - last_update_wifi_beacon_ms > 1000/g.wifi_beacon_rate) {
        last_update_wifi_beacon_ms = now_ms;
        if(g.protocol == PROTOCOL_EU)
            wifi.transmit_beacon(UAS_data);
        else if(g.protocol == PROTOCOL_CN2023)
            wifi.transmit_cn_beacon(CN_UAS_data);
        else if(g.protocol == PROTOCOL_GB2025)
            wifi.transmit_GB2025_beacon(GB2025_UAS_data);
        // Serial.printf("update_wifi_beacon\n");
    }

    static uint32_t last_update_bt5_ms; // BLE 远端蓝牙发送频率 bt5_rate 达到对应的时间了就启动RemoteID BLE5发送
    if (g.bt5_rate > 0 &&
        now_ms - last_update_bt5_ms > 1000/g.bt5_rate) {
        last_update_bt5_ms = now_ms;
        if(g.protocol == PROTOCOL_EU)
            ble.transmit_longrange(UAS_data);
        else if(g.protocol == PROTOCOL_CN2023)
            ble.transmit_cn_longrange(CN_UAS_data);
        else if(g.protocol == PROTOCOL_GB2025)
            ble.transmit_GB2025_longrange(GB2025_UAS_data); // 国标只要求bt5
        Serial.printf("update_bt5\n");
    }

    static uint32_t last_update_bt4_ms; // BLE 近端蓝牙发送频率 bt4_rate 达到对应的时间了就启动RemoteID BLE4发送
    int bt4_states = 0;
    if(g.protocol == PROTOCOL_EU)
        bt4_states = UAS_data.BasicIDValid[1] ? 7 : 6;
    else if(g.protocol == PROTOCOL_CN2023)
        bt4_states = CN_UAS_data.BasicIDValid[1] ? 7 : 6; // CN 最多只发6条其实 为了间隔上同步
    // Serial.printf("bt4 state %d\n", bt4_states);   
    if (g.bt4_rate > 0 &&
        now_ms - last_update_bt4_ms > (1000.0f/bt4_states)/g.bt4_rate) {
        last_update_bt4_ms = now_ms;

        if(g.protocol == PROTOCOL_EU)
            ble.transmit_legacy(UAS_data);
        else if(g.protocol  == PROTOCOL_CN2023)
            ble.transmit_cn_legacy(CN_UAS_data);
        // Serial.printf("update_bt4\n");   
    }

    // sleep for a bit for power saving 节能 
    delay(1);
}