feature-remoteid/RemoteIDModule/BLE_TX.cpp

/*
  BLE TX driver

  Many thanks to Roel Schiphorst from BlueMark for his assistance with the Bluetooth code

  Also many thanks to chegewara for his sample code:
  https://github.com/Tinyu-Zhao/Arduino-Borads/blob/b584d00a81e4ac6d7b72697c674ca1bbcb8aae69/libraries/BLE/examples/BLE5_multi_advertising/BLE5_multi_advertising.ino
 */

#include "BLE_TX.h"
#include "options.h"
#include <esp_system.h>

#include <BLEDevice.h>
#include <BLEAdvertising.h>
#include "parameters.h"



//interval min/max are configured for 1 Hz update rate. Somehow dynamic setting of these fields fails
//shorter intervals lead to more BLE transmissions. This would result in increased power consumption and can lead to more interference to other radio systems.
static esp_ble_gap_ext_adv_params_t legacy_adv_params = {
    .type = ESP_BLE_GAP_SET_EXT_ADV_PROP_LEGACY_NONCONN,
    .interval_min = 192,
    .interval_max = 267,
    .channel_map = ADV_CHNL_ALL,
    .own_addr_type = BLE_ADDR_TYPE_RANDOM,
    .filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_WLST, // we want unicast non-connectable transmission
    .tx_power = 0,
    .primary_phy = ESP_BLE_GAP_PHY_1M,
    .max_skip = 0,
    .secondary_phy = ESP_BLE_GAP_PHY_1M,
    .sid = 0,
    .scan_req_notif = false,
};

static esp_ble_gap_ext_adv_params_t ext_adv_params_coded = {
    .type = ESP_BLE_GAP_SET_EXT_ADV_PROP_NONCONN_NONSCANNABLE_UNDIRECTED, //set to unicast advertising
    .interval_min = 1200,
    .interval_max = 1600,
    .channel_map = ADV_CHNL_ALL,
    .own_addr_type = BLE_ADDR_TYPE_RANDOM,
    .filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_WLST, // we want unicast non-connectable transmission
    .tx_power = 0,
    .primary_phy = ESP_BLE_GAP_PHY_CODED,
    .max_skip = 0,
    .secondary_phy = ESP_BLE_GAP_PHY_CODED,
    .sid = 1,
    .scan_req_notif = false,
};

/*
  map dBm to a TX power
 */
uint8_t BLE_TX::dBm_to_tx_power(float dBm) const
{
    static const struct {
        uint8_t level;
        float dBm;
    } dBm_table[] = {
        { ESP_PWR_LVL_N27,-27 },
        { ESP_PWR_LVL_N24,-24 },
        { ESP_PWR_LVL_N21,-21 },
        { ESP_PWR_LVL_N18,-18 },
        { ESP_PWR_LVL_N15,-15 },
        { ESP_PWR_LVL_N12,-12 },
        { ESP_PWR_LVL_N9,  -9 },
        { ESP_PWR_LVL_N6,  -6 },
        { ESP_PWR_LVL_N3,  -3 },
        { ESP_PWR_LVL_N0,   0 },
        { ESP_PWR_LVL_P3,   3 },
        { ESP_PWR_LVL_P6,   6 },
        { ESP_PWR_LVL_P9,   9 },
        { ESP_PWR_LVL_P12, 12 },
        { ESP_PWR_LVL_P15, 15 },
        { ESP_PWR_LVL_P18, 18 },
    };
    for (const auto &t : dBm_table) {
        if (dBm <= t.dBm) {
            return t.level;
        }
    }
    return ESP_PWR_LVL_P18;
}


static BLEMultiAdvertising advert(2);

bool BLE_TX::init(void)
{
    if (initialised) {
        return true;
    }
    initialised = true;
    BLEDevice::init("");

    // setup power levels
    legacy_adv_params.tx_power = dBm_to_tx_power(g.bt4_power);
    ext_adv_params_coded.tx_power = dBm_to_tx_power(g.bt5_power);
    
    // set min/max interval based on output rate    
    legacy_adv_params.interval_max =  (1000/(g.bt4_rate*7))/0.625;
    legacy_adv_params.interval_min = 0.75*legacy_adv_params.interval_max;
    ext_adv_params_coded.interval_max =  (1000/(g.bt5_rate))/0.625;
    ext_adv_params_coded.interval_min = 0.75*ext_adv_params_coded.interval_max;
    // 调试抓包
    // generate random mac address
    uint8_t mac_addr[6];
    generate_random_mac(mac_addr);
    mac_addr[0] |= 0xc0;

    Serial.printf("Setting BT MAC: %02X:%02X:%02X:%02X:%02X:%02X\n",
                mac_addr[0], mac_addr[1], mac_addr[2],
                mac_addr[3], mac_addr[4], mac_addr[5]);

    advert.setAdvertisingParams(0, &legacy_adv_params);
    advert.setInstanceAddress(0, mac_addr);
    advert.setDuration(0);

    advert.setAdvertisingParams(1, &ext_adv_params_coded);
    advert.setDuration(1);
    advert.setInstanceAddress(1, mac_addr);

    // prefer S8 coding
    if (esp_ble_gap_set_prefered_default_phy(ESP_BLE_GAP_PHY_OPTIONS_PREF_S8_CODING, ESP_BLE_GAP_PHY_OPTIONS_PREF_S8_CODING) != ESP_OK) {
        Serial.printf("Failed to setup S8 coding\n");
    }

    memset(&msg_counters_CNDID,0, sizeof(msg_counters_CNDID));
    memset(&msg_counters_ODID,0, sizeof(msg_counters_ODID));
    msg_counters_GB2025 = 0;
    return true;
}

#define IMIN(a,b) ((a)<(b)?(a):(b))

bool BLE_TX::transmit_longrange(ODID_UAS_Data &UAS_data)
{
    init();
    // create a packed UAS data message
    uint8_t payload[250];
    int length = odid_message_build_pack(&UAS_data, payload, 255);
    if (length <= 0) {
        return false;
    }
    // Serial.printf("ble4 tx power %d\n", legacy_adv_params.tx_power);
    // Serial.printf("ble5 tx power %d\n", ext_adv_params_coded.tx_power);
    // setup ASTM header
    const uint8_t header[] { uint8_t(length+5), 0x16, 0xfa, 0xff, 0x0d, uint8_t(msg_counters_ODID[ODID_MSG_COUNTER_PACKED]++) };

    // combine header with payload
    memcpy(longrange_payload, header, sizeof(header));
    memcpy(&longrange_payload[sizeof(header)], payload, length);
    int longrange_length = sizeof(header) + length;

    advert.setAdvertisingData(1, longrange_length, longrange_payload);

    // we start advertising when we have the first lot of data to send
    Serial.printf("started is %d \r\n", started);
    Serial.printf("count is %d \r\n", msg_counters_ODID[ODID_MSG_COUNTER_PACKED]);
    for(uint8_t i =0 ;i < longrange_length; i++ )
        Serial.printf(" %x ", longrange_payload[i]);
    Serial.printf("end \r\n");
    if (!started) {
        advert.start();
    }
    started = true;

    return true;
}

bool BLE_TX::transmit_legacy(ODID_UAS_Data &UAS_data)
{
    init();
    // Serial.printf("ble4 tx power %d\n", legacy_adv_params.tx_power);
    // Serial.printf("ble5 tx power %d\n", ext_adv_params_coded.tx_power);
    static uint8_t legacy_phase = 0;
    int legacy_length = 0;
    // setup ASTM header
    const uint8_t header[] { 0x1e, 0x16, 0xfa, 0xff, 0x0d }; //exclude the message counter
    // combine header with payload
    memset(legacy_payload, 0, sizeof(legacy_payload));
    memcpy(legacy_payload, header, sizeof(header));
    legacy_length = sizeof(header);

    switch (legacy_phase)
    {
    case  0: {
        if (UAS_data.LocationValid) {
            ODID_Location_encoded location_encoded;
            memset(&location_encoded, 0, sizeof(location_encoded));
            if (encodeLocationMessage(&location_encoded, &UAS_data.Location) != ODID_SUCCESS) {
                break;
            }

            memcpy(&legacy_payload[sizeof(header)], &msg_counters_ODID[ODID_MSG_COUNTER_LOCATION], 1); //set packet counter
            msg_counters_ODID[ODID_MSG_COUNTER_LOCATION]++;
            //msg_counters_ODID[ODID_MSG_COUNTER_LOCATION] %= 256; //likely not be needed as it is defined as unint_8

            memcpy(&legacy_payload[sizeof(header) + 1], &location_encoded, sizeof(location_encoded));
            legacy_length = sizeof(header) + 1 + sizeof(location_encoded);
        }
        break;
    }

    case  1: {
        if (UAS_data.BasicIDValid[0]) {
            ODID_BasicID_encoded basicid_encoded;
            memset(&basicid_encoded, 0, sizeof(basicid_encoded));
            if (encodeBasicIDMessage(&basicid_encoded, &UAS_data.BasicID[0]) != ODID_SUCCESS) {
                break;
            }

            memcpy(&legacy_payload[sizeof(header)], &msg_counters_ODID[ODID_MSG_COUNTER_BASIC_ID], 1); //set packet counter
            msg_counters_ODID[ODID_MSG_COUNTER_BASIC_ID]++;
            //msg_counters_ODID[ODID_MSG_COUNTER_BASIC_ID] %= 256; //likely not be needed as it is defined as unint_8

            memcpy(&legacy_payload[sizeof(header) + 1], &basicid_encoded, sizeof(basicid_encoded));
            legacy_length = sizeof(header) + 1 + sizeof(basicid_encoded);
        }
        break;
    }

    case  2: {
        if (UAS_data.SelfIDValid) {
            ODID_SelfID_encoded selfid_encoded;
            memset(&selfid_encoded, 0, sizeof(selfid_encoded));
            if (encodeSelfIDMessage(&selfid_encoded, &UAS_data.SelfID) != ODID_SUCCESS) {
                break;
            }

            memcpy(&legacy_payload[sizeof(header)], &msg_counters_ODID[ODID_MSG_COUNTER_SELF_ID], 1); //set packet counter
            msg_counters_ODID[ODID_MSG_COUNTER_SELF_ID]++;
            //msg_counters_ODID[ODID_MSG_COUNTER_SELF_ID] %= 256; //likely not be needed as it is defined as uint_8

            memcpy(&legacy_payload[sizeof(header) + 1], &selfid_encoded, sizeof(selfid_encoded));
            legacy_length = sizeof(header) + 1 + sizeof(selfid_encoded);
        }
        break;
    }

    case  3: {
        if (UAS_data.SystemValid) {
            ODID_System_encoded system_encoded;
            memset(&system_encoded, 0, sizeof(system_encoded));
            if (encodeSystemMessage(&system_encoded, &UAS_data.System) != ODID_SUCCESS) {
                break;
            }

            memcpy(&legacy_payload[sizeof(header)], &msg_counters_ODID[ODID_MSG_COUNTER_SYSTEM], 1); //set packet counter
            msg_counters_ODID[ODID_MSG_COUNTER_SYSTEM]++;
            //msg_counters_ODID[ODID_MSG_COUNTER_SYSTEM] %= 256; //likely not be needed as it is defined as uint_8

            memcpy(&legacy_payload[sizeof(header) + 1], &system_encoded, sizeof(system_encoded));
            legacy_length = sizeof(header) + 1 + sizeof(system_encoded);
        }
        break;
    }

    case  4: {
        if (UAS_data.OperatorIDValid) {
            ODID_OperatorID_encoded operatorid_encoded;
            memset(&operatorid_encoded, 0, sizeof(operatorid_encoded));
            if (encodeOperatorIDMessage(&operatorid_encoded, &UAS_data.OperatorID) != ODID_SUCCESS) {
                break;
            }

            memcpy(&legacy_payload[sizeof(header)], &msg_counters_ODID[ODID_MSG_COUNTER_OPERATOR_ID], 1); //set packet counter
            msg_counters_ODID[ODID_MSG_COUNTER_OPERATOR_ID]++;
            //msg_counters_ODID[ODID_MSG_COUNTER_OPERATOR_ID] %= 256; //likely not be needed as it is defined as uint_8

            memcpy(&legacy_payload[sizeof(header) + 1], &operatorid_encoded, sizeof(operatorid_encoded));
            legacy_length = sizeof(header) + 1 + sizeof(operatorid_encoded);
        }
        break;
    }

    case  5: //in case of dual basic ID
        if (UAS_data.BasicIDValid[1]) {
            ODID_BasicID_encoded basicid2_encoded;
            memset(&basicid2_encoded, 0, sizeof(basicid2_encoded));
            if (encodeBasicIDMessage(&basicid2_encoded, &UAS_data.BasicID[1]) != ODID_SUCCESS) {
                break;
            }

            memcpy(&legacy_payload[sizeof(header)], &msg_counters_ODID[ODID_MSG_COUNTER_BASIC_ID], 1); //set packet counter
            msg_counters_ODID[ODID_MSG_COUNTER_BASIC_ID]++;
            //msg_counters_ODID[ODID_MSG_COUNTER_BASIC_ID] %= 256; //likely not be needed as it is defined as unint_8

            memcpy(&legacy_payload[sizeof(header) + 1], &basicid2_encoded, sizeof(basicid2_encoded));
            legacy_length = sizeof(header) + 1 + sizeof(basicid2_encoded);
        }
        break;

    case  6: {
        //set BLE name
        char legacy_name[28] {};
        const char *UAS_ID = (const char *)UAS_data.BasicID[0].UASID;
        const uint8_t ID_len = strlen(UAS_ID);
        const uint8_t ID_tail = IMIN(4, ID_len); 
        snprintf(legacy_name, sizeof(legacy_name), "ArduRemoteID_%s", &UAS_ID[ID_len-ID_tail]);

        memset(legacy_payload, 0, sizeof(legacy_payload));
        const uint8_t legacy_name_header[] { 0x02, 0x01, 0x06, uint8_t(strlen(legacy_name)+1), ESP_BLE_AD_TYPE_NAME_SHORT};

        memcpy(legacy_payload, legacy_name_header, sizeof(legacy_name_header));
        memcpy(&legacy_payload[sizeof(legacy_name_header)], legacy_name, strlen(legacy_name) + 1);

        legacy_length = sizeof(legacy_name_header) + strlen(legacy_name) + 1; //add extra char for \0
        break;
    }
    }

    legacy_phase++;

    if (UAS_data.BasicIDValid[1]) {
        legacy_phase %= 7;
    } else {
        legacy_phase %= 6;
    }

    advert.setAdvertisingData(0, legacy_length, legacy_payload);
    // Serial.printf("started is %d\n", started);
    // Serial.printf("legacy is %s\n", legacy_payload);
    if (!started) {
        advert.start();
    }
    started = true;

    return true;
}


// bt5 蓝牙发送国标数据
bool BLE_TX::transmit_cn_longrange(CNDID_UAS_Data &UAS_data)
{
     init();
    // create a packed UAS data message
    uint8_t payload[250];
    int length = cndid_message_build_pack(&UAS_data, payload, 255);
 
    if (length <= 0) {
        return false;
    }

    // setup ASTM header American Society for Testing and Materials
    const uint8_t header[] { uint8_t(length+5),   // 头字段1：总长度（payload长度 + 5个固定头字节）
        0x16,  // 头字段2：ASTM协议固定标识（Remote ID相关）
        0xfa,  // 头字段3：厂商/类型标识（自定义或标准值）
        0xff,  // 头字段4：保留/扩展位
        0x0d,  // 头字段5：消息类型（如打包的UAS数据类型）
        uint8_t(msg_counters_CNDID[CNDID_MSG_COUNTER_PACKED]++) }; // 头字段6：消息计数器（自增，防丢包/重放）

    // // 拼接协议头和payload到最终发送缓冲区longrange_payload
    memcpy(longrange_payload, header, sizeof(header)); // 先拷贝协议头
    memcpy(&longrange_payload[sizeof(header)], payload, length);  // 再拷贝payload
    int longrange_length = sizeof(header) + length;  // 计算最终数据包总长度
    // 设置BLE广播数据（参数：广播类型1，数据长度，数据缓冲区）
    advert.setAdvertisingData(1, longrange_length, longrange_payload);
    // 首次发送时启动BLE广播（避免重复启动）    
    // we start advertising when we have the first lot of data to send
    Serial.printf("started is %d \r\n", started);
    Serial.printf("count is %d \r\n", msg_counters_CNDID[CNDID_MSG_COUNTER_PACKED]);
    for(uint8_t i =0 ;i < longrange_length; i++ )
        Serial.printf(" %x ", longrange_payload[i]);
    Serial.printf("end \r\n");
    if (!started) {
        advert.start();
    }
    started = true;

    return true;
}

// bt4 蓝牙发送国标数据
bool BLE_TX::transmit_cn_legacy(CNDID_UAS_Data &UAS_data)
{
    init();
    static uint8_t legacy_phase = 0;
    int legacy_length = 0;
    // 设置ASTM标头 这个标头可能要根据国标修改
    const uint8_t header[] { 0x1e, 0x16, 0xfa, 0xff, 0x0d }; // 排除消息计数器
    // 将头部与有效载荷结合起来
    memset(legacy_payload, 0, sizeof(legacy_payload));
    memcpy(legacy_payload, header, sizeof(header));
    legacy_length = sizeof(header);

    switch (legacy_phase)
    {
    case  0: {
        if (UAS_data.LocationValid) {
            CNDID_Location_encoded location_encoded;
            memset(&location_encoded, 0, sizeof(location_encoded));
            if (encodeCNLocationMessage(&location_encoded, &UAS_data.Location) != CNDID_SUCCESS) {
                break;
            }

            memcpy(&legacy_payload[sizeof(header)], &msg_counters_CNDID[CNDID_MSG_COUNTER_LOCATION], 1); //set packet counter
            msg_counters_CNDID[CNDID_MSG_COUNTER_LOCATION]++;
            //msg_counters_CNDID[CNDID_MSG_COUNTER_LOCATION] %= 256; //likely not be needed as it is defined as unint_8

            memcpy(&legacy_payload[sizeof(header) + 1], &location_encoded, sizeof(location_encoded));
            legacy_length = sizeof(header) + 1 + sizeof(location_encoded);
        }
        break;
    }

    case  1: {
        if (UAS_data.BasicIDValid[0]) {
            CNDID_BasicID_encoded basicid_encoded;
            memset(&basicid_encoded, 0, sizeof(basicid_encoded));
            if (encodeCNBasicIDMessage(&basicid_encoded, &UAS_data.BasicID[0]) != CNDID_SUCCESS) {
                break;
            }

            memcpy(&legacy_payload[sizeof(header)], &msg_counters_CNDID[CNDID_MSG_COUNTER_BASIC_ID], 1); //set packet counter
            msg_counters_CNDID[CNDID_MSG_COUNTER_BASIC_ID]++;
            //msg_counters_CNDID[CNDID_MSG_COUNTER_BASIC_ID] %= 256; //likely not be needed as it is defined as unint_8

            memcpy(&legacy_payload[sizeof(header) + 1], &basicid_encoded, sizeof(basicid_encoded));
            legacy_length = sizeof(header) + 1 + sizeof(basicid_encoded);
        }
        break;
    }

    case  2: {
        if (UAS_data.SelfIDValid) {
            CNDID_SelfID_encoded selfid_encoded;
            memset(&selfid_encoded, 0, sizeof(selfid_encoded));
            if (encodeCNSelfIDMessage(&selfid_encoded, &UAS_data.SelfID) != CNDID_SUCCESS) {
                break;
            }

            memcpy(&legacy_payload[sizeof(header)], &msg_counters_CNDID[CNDID_MSG_COUNTER_SELF_ID], 1); //set packet counter
            msg_counters_CNDID[CNDID_MSG_COUNTER_SELF_ID]++;
            //msg_counters_CNDID[CNDID_MSG_COUNTER_SELF_ID] %= 256; //likely not be needed as it is defined as uint_8

            memcpy(&legacy_payload[sizeof(header) + 1], &selfid_encoded, sizeof(selfid_encoded));
            legacy_length = sizeof(header) + 1 + sizeof(selfid_encoded);
        }
        break;
    }

    case  3: {
        if (UAS_data.SystemValid) {
            CNDID_System_encoded system_encoded;
            memset(&system_encoded, 0, sizeof(system_encoded));
            if (encodeCNSystemMessage(&system_encoded, &UAS_data.System) != CNDID_SUCCESS) {
                break;
            }

            memcpy(&legacy_payload[sizeof(header)], &msg_counters_CNDID[CNDID_MSG_COUNTER_SYSTEM], 1); //set packet counter
            msg_counters_CNDID[CNDID_MSG_COUNTER_SYSTEM]++;
            //msg_counters_CNDID[CNDID_MSG_COUNTER_SYSTEM] %= 256; //likely not be needed as it is defined as uint_8

            memcpy(&legacy_payload[sizeof(header) + 1], &system_encoded, sizeof(system_encoded));
            legacy_length = sizeof(header) + 1 + sizeof(system_encoded);
        }
        break;
    }

    case  4: //in case of dual basic ID 在存在双重基本身份标识的情况下
        if (UAS_data.BasicIDValid[1]) {
            CNDID_BasicID_encoded basicid2_encoded;
            memset(&basicid2_encoded, 0, sizeof(basicid2_encoded));
            if (encodeCNBasicIDMessage(&basicid2_encoded, &UAS_data.BasicID[1]) != CNDID_SUCCESS) {
                break;
            }

            memcpy(&legacy_payload[sizeof(header)], &msg_counters_CNDID[CNDID_MSG_COUNTER_BASIC_ID], 1); //set packet counter
            msg_counters_CNDID[CNDID_MSG_COUNTER_BASIC_ID]++;
            //msg_counters_CNDID[CNDID_MSG_COUNTER_BASIC_ID] %= 256; //可能不需要，因为它被定义为unint_8

            memcpy(&legacy_payload[sizeof(header) + 1], &basicid2_encoded, sizeof(basicid2_encoded));
            legacy_length = sizeof(header) + 1 + sizeof(basicid2_encoded);
        }
        break;

    case  5: {
        //set BLE name  构造BLE设备名称：CN_RemoteID_XXXX（XXXX是无人机ID后4位）
        char legacy_name[28] {};
        const char *UAS_ID = (const char *)UAS_data.BasicID[0].UASID;
        const uint8_t ID_len = strlen(UAS_ID);
        const uint8_t ID_tail = IMIN(4, ID_len);
        snprintf(legacy_name, sizeof(legacy_name), "ArduRemoteID_%s", &UAS_ID[ID_len-ID_tail]);
         // 清空缓冲区，构造BLE名称广播标头（符合BLE AD规范）
        memset(legacy_payload, 0, sizeof(legacy_payload));
        const uint8_t legacy_name_header[] { 0x02, 0x01, 0x06, uint8_t(strlen(legacy_name)+1), ESP_BLE_AD_TYPE_NAME_SHORT};
         // 拷贝名称标头 + 名称内容
        memcpy(legacy_payload, legacy_name_header, sizeof(legacy_name_header));
        memcpy(&legacy_payload[sizeof(legacy_name_header)], legacy_name, strlen(legacy_name) + 1);
        // 更新广播长度（标头+名称+结束符）
        legacy_length = sizeof(legacy_name_header) + strlen(legacy_name) + 1; //add extra char for \0
        break;
    }
    }

    legacy_phase++; // 每次发送后阶段自增

    if (UAS_data.BasicIDValid[1]) { // 根据是否有双重基础ID，控制阶段循环范围
        legacy_phase %= 6; 
    } else {
        legacy_phase %= 5;
    }
    // 设置BLE广播数据（长度+内容）
    advert.setAdvertisingData(0, legacy_length, legacy_payload);
    // 若广播未启动则启动，标记为已启动
    // Serial.printf("started is %d\n", started);
    // Serial.printf("legacy is %s\n", legacy_payload);
    if (!started) {
        advert.start();
    }
    started = true;

    return true;
}
// 以下数据包不再准寻报文加集合报文的形式 无法再进行legacy这种分报文发送的形式
bool BLE_TX::transmit_GB2025_longrange(UavIdentificationData &UAS_data)
{
    init();
    // create a packed UAS data message
    uint8_t payload[250];
    int length = did_GB2025_message_build_pack(&UAS_data, payload, 255);
    Serial.printf("bt5 gb data len %d\r\n", length);
    if (length <= 0) {
        return false;
    }

    // setup ASTM header American Society for Testing and Materials
    const uint8_t header[] { 
        uint8_t(length+5),   // 头字段1：总长度（payload长度 + 5个固定头字节）
        0x16,  // 头字段2：ASTM协议固定标识（Remote ID相关）
        0xfa,  // 头字段3：厂商/类型标识（自定义或标准值）
        0xff,  // 头字段4：保留/扩展位
        0x0d,  // 头字段5：消息类型（如打包的UAS数据类型）
        uint8_t(msg_counters_GB2025++) }; // 头字段6：消息计数器（自增，防丢包/重放）

    // // 拼接协议头和payload到最终发送缓冲区longrange_payload
    memcpy(longrange_payload, header, sizeof(header)); // 先拷贝协议头
    memcpy(&longrange_payload[sizeof(header)], payload, length);  // 再拷贝payload
    int longrange_length = sizeof(header) + length;  // 计算最终数据包总长度
    // 设置BLE广播数据（参数：广播类型1，数据长度，数据缓冲区）
    advert.setAdvertisingData(1, longrange_length, longrange_payload);
    // 首次发送时启动BLE广播（避免重复启动）    
    // we start advertising when we have the first lot of data to send
    Serial.printf("started is %d \r\n", started);
    Serial.printf("count is %d \r\n", msg_counters_GB2025);
    for(uint8_t i =0 ;i < longrange_length; i++ )
        Serial.printf(" %x ", longrange_payload[i]);
    Serial.printf("end \r\n");
    if (!started) {
        advert.start();
    }
    started = true;

    return true;
}