feature-remoteid/RemoteIDModule/mavlink.cpp

/*
  mavlink class for handling OpenDroneID messages
 */
#include <Arduino.h>
#include "mavlink.h"
#include "board_config.h"
#include "version.h"
#include "parameters.h"

#define SERIAL_BAUD 115200

static HardwareSerial *serial_ports[MAVLINK_COMM_NUM_BUFFERS];

#include <generated/mavlink_helpers.h>

mavlink_system_t mavlink_system = {0, MAV_COMP_ID_ODID_TXRX_1};

/*
  send a buffer out a MAVLink channel
  函数作用：把字节缓冲区通过指定MAVLink通道发送出去
 */
void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
{
    if (chan >= MAVLINK_COMM_NUM_BUFFERS || serial_ports[uint8_t(chan)] == nullptr) {
        return; // 通道号超出支持范围  通道对应的串口未绑定（空指针）
    }
    auto &serial = *serial_ports[uint8_t(chan)]; // 从全局数组中获取通道对应的物理串口引用
    serial.write(buf, len); // 把字节缓冲区写入串口（核心：完成物理发送）
}

/*
  abstraction for MAVLink on a serial port
 */
MAVLinkSerial::MAVLinkSerial(HardwareSerial &_serial, mavlink_channel_t _chan) :
    serial(_serial), // 绑定物理串口（如Serial1）到类成员serial
    chan(_chan) // 绑定MAVLink通道号（如MAVLINK_COMM_0）到类成员chan
{
    serial_ports[uint8_t(_chan - MAVLINK_COMM_0)] = &serial; // 函数体：将串口指针存入全局数组serial_ports
}

void MAVLinkSerial::init(void)
{
    // print banner at startup
    serial.printf("ArduRemoteID version %u.%u %08x\n",
                  FW_VERSION_MAJOR, FW_VERSION_MINOR, GIT_VERSION);
    mavlink_system.sysid = g.mavlink_sysid; // 初始化MAVLink系统ID：从全局配置g中读取预设的mavlink_sysid
}
// 地面站（如 Mission Planner）会向 RemoteID 模块发送 PARAM_REQUEST_LIST 消息，
// 请求获取所有参数（如 UIN、运营人 ID、发送频率等），
// 这段代码就是按 50ms 间隔逐个回复参数值，方便地面站配置 / 查看 RemoteID 模块的参数。
void MAVLinkSerial::update(void)
{
    const uint32_t now_ms = millis();

    if (mavlink_system.sysid != 0) {  // 分支1：如果MAVLink系统ID已初始化（非0）
        update_send(); // 执行MAVLink消息发送逻辑
    } else if (g.mavlink_sysid != 0) { // 分支2：如果系统ID未初始化，但配置了g.mavlink_sysid（外部配置）
        mavlink_system.sysid = g.mavlink_sysid;  // 用配置值初始化系统ID
    } else if (now_ms - last_hb_warn_ms >= 2000) {  // 分支3：系统ID未初始化，且无配置 → 打印心跳等待提示（每2秒一次）
        last_hb_warn_ms = millis();
        serial.printf("Waiting for heartbeat\n"); // 串口打印：等待飞控心跳
        // serial.printf("等待心跳包\n");
    }
    update_receive(); // 接收串口数据，解析飞控发来的MAVLink消息
    // 如果参数请求未完成，且距离上次发送超过50ms
    if (param_request_last_ms != 0 && now_ms - param_request_last_ms > 50) {
        param_request_last_ms = now_ms;
        float value;
        if (param_next->get_as_float(value)) { // 读取下一个参数的浮点值（如UIN、运营人ID转的浮点/字符串）
            // 发送MAVLink PARAM_VALUE消息（回复地面站的参数查询）
            mavlink_msg_param_value_send(chan, // MAVLink通道
                                         param_next->name, value, // 参数名（如"UIN"、"OPERATOR_ID"） // 参数值
                                         MAV_PARAM_TYPE_REAL32, // 参数类型（浮点型）
                                         g.param_count_float(), // 浮点参数总数
                                         g.param_index_float(param_next)); // 当前参数索引
        }
        param_next++; // 指向下一个参数
        if (param_next->ptype == Parameters::ParamType::NONE) { // 如果参数遍历完成 → 重置状态
            param_next = nullptr;
            param_request_last_ms = 0;
        }
    }
}
/*
 RemoteID 模块每秒向飞控 / 地面站发送 MAVLink 心跳包（标识自身为 ODID 设备），
 并附带发送无人机解锁状态—— 让飞控 / 地面站识别 
 “这是一个 RemoteID 设备”，并知晓无人机的解锁 / 上锁状态
*/
void MAVLinkSerial::update_send(void)
{
    uint32_t now_ms = millis();
    if (now_ms - last_hb_ms >= 1000) {
        last_hb_ms = now_ms;
        mavlink_msg_heartbeat_send(
            chan,
            MAV_TYPE_ODID,
            MAV_AUTOPILOT_INVALID,
            0,
            0,
            0);

        // send arming status
        arm_status_send();
    }
}

void MAVLinkSerial::update_receive(void) // 流式处理 一次处理缓冲区所有数据 然后解析
{
    // receive new packets
    mavlink_message_t msg;    // 1. 定义MAVLink消息结构体（存储解析后的完整消息）
    mavlink_status_t status;  // 2. 定义MAVLink状态结构体（记录解析状态，如丢包、同步等）
    status.packet_rx_drop_count = 0;   // 3. 重置丢包计数（初始化解析状态）

    const uint16_t nbytes = serial.available(); // 4. 获取串口缓冲区中待读取的字节数
    for (uint16_t i=0; i<nbytes; i++) { // 5. 遍历读取所有字节，逐字节解析
        const uint8_t c = (uint8_t)serial.read();  // 6. 读取1个字节（飞控发来的MAVLink数据）
        // Try to get a new message
        if (mavlink_parse_char(chan, c, &msg, &status)) { // 7. 核心：逐字节解析MAVLink消息
            process_packet(status, msg); // 8. 解析出完整消息后，交给process_packet()处理（提取位置/状态等核心数据）
        }
    }
}

/*
  printf via MAVLink STATUSTEXT for debugging
  传统串口打印需要现场接电脑调试，而 MAVLink STATUSTEXT 消息可通过飞控转发到地面站，
  即使无人机在空中，也能实时看到 RemoteID 模块的调试日志；
 */
void MAVLinkSerial::mav_printf(uint8_t severity, const char *fmt, ...)
{
    va_list arg_list;  // 1. 定义可变参数列表（处理printf风格的可变参数）
    char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] {}; // 2. 定义日志文本缓冲区：长度=STATUSTEXT消息的text字段长度+1（+1是字符串结束符）
    va_start(arg_list, fmt); // 3. 初始化可变参数列表，指向fmt后的第一个参数
    vsnprintf(text, sizeof(text), fmt, arg_list);  // 4. 格式化字符串：把fmt+可变参数写入text缓冲区，限制长度为缓冲区大小
    va_end(arg_list); // 5. 结束可变参数列表
    // 6. 发送MAVLink STATUSTEXT消息
    mavlink_msg_statustext_send(chan,  // MAVLink通道号（对应飞控串口
                                severity, // 日志级别
                                text, // 格式化后的日志文本
                                0, 0); // 扩展参数（MAVLink 2.0新增，暂用0）
}
/*
专门针对 OpenDroneID（ODID）标准的各类消息做解析，同时处理参数交互、安全指令等辅助逻辑
*/
void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &msg)
{
    const uint32_t now_ms = millis();
    switch (msg.msgid) {
    case MAVLINK_MSG_ID_HEARTBEAT: { // 心跳消息处理
        mavlink_heartbeat_t hb;
        if (mavlink_system.sysid == 0) { // 系统ID未初始化时
            mavlink_msg_heartbeat_decode(&msg, &hb); // 仅接收飞控（非地面站GCS）的心跳，初始化模块sysid
            if (msg.sysid > 0 && hb.type != MAV_TYPE_GCS) {
                mavlink_system.sysid = msg.sysid; // 过滤地面站（GCS）心跳：只从飞控获取 sysid，防止地面站干扰
            }
        }
        break;
    }
    case MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION: { // 位置消息 无人机经纬度、高度、速度、timestamp（时间戳）等位置信息
        mavlink_msg_open_drone_id_location_decode(&msg, &location);
        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
            Serial.printf("MAVLink: got Location\n");
        }
        if (last_location_timestamp != location.timestamp) {
            //only update the timestamp if we receive information with a different timestamp
            last_location_ms = millis();
            last_location_timestamp = location.timestamp;
        }
        last_location_ms = now_ms;
        break;
    }
    case MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID: { // 基础 ID 消息
        mavlink_open_drone_id_basic_id_t basic_id_tmp;
        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
            Serial.printf("MAVLink: got BasicID\n");
         
        } // 仅接收有效数据：UAS_ID非空 + ID类型合法（1~MAV_ODID_ID_TYPE_SPECIFIC_SESSION_ID）
        mavlink_msg_open_drone_id_basic_id_decode(&msg, &basic_id_tmp);
        if ((strlen((const char*) basic_id_tmp.uas_id) > 0) && (basic_id_tmp.id_type > 0) && (basic_id_tmp.id_type <= MAV_ODID_ID_TYPE_SPECIFIC_SESSION_ID)) {
            //only update if we receive valid data
            basic_id = basic_id_tmp; // 更新本地缓存
            last_basic_id_ms = now_ms; // 记录最后接收时间
        }
        break;
    }
    case MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION: { // 其他 ODID 消息（认证 / 自描述 / 系统 / 运营商 ID）
        mavlink_msg_open_drone_id_authentication_decode(&msg, &authentication);
        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
            Serial.printf("MAVLink: got Auth\n");
         
        }
        break;
    }
    case MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID: { // 无人机自描述信息（如机型、用途）
        mavlink_msg_open_drone_id_self_id_decode(&msg, &self_id);
        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
            Serial.printf("MAVLink: got SelfID\n");
          
        }
        last_self_id_ms = now_ms;
        break;
    }
    case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM: { // 系统信息（运营人位置、高度基准）
        mavlink_msg_open_drone_id_system_decode(&msg, &system);
        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
            Serial.printf("MAVLink: got System\n");
          
        }
        if ((last_system_timestamp != system.timestamp) || (system.timestamp == 0)) {
            //only update the timestamp if we receive information with a different timestamp
            last_system_ms = millis();
            last_system_timestamp = system.timestamp;
        }
        break;
    }
    case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE: { // 运营人位置更新
        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
            Serial.printf("MAVLink: got System update\n");
          
        }
        mavlink_open_drone_id_system_update_t pkt_system_update;
        mavlink_msg_open_drone_id_system_update_decode(&msg, &pkt_system_update);
        system.operator_latitude = pkt_system_update.operator_latitude;
        system.operator_longitude = pkt_system_update.operator_longitude;
        system.operator_altitude_geo = pkt_system_update.operator_altitude_geo;
        system.timestamp = pkt_system_update.timestamp;
        if (last_system_ms != 0) {
            // we can only mark system as updated if we have the other
            // information already
            if ((last_system_timestamp != system.timestamp) || (pkt_system_update.timestamp == 0)) {
                //only update the timestamp if we receive information with a different timestamp
                last_system_ms = millis();
                last_system_timestamp = pkt_system_update.timestamp;
            }
            last_system_ms = now_ms;
        }
        break;
    }
    case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID: { // 运营人 ID（如企业名称、联系方式）
        mavlink_msg_open_drone_id_operator_id_decode(&msg, &operator_id);
        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
            Serial.printf("MAVLink: got OperatorID\n");
           
        }
        last_operator_id_ms = now_ms;
        break;
    }
    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { // 参数列表请求
        param_next = g.find_by_index_float(0);
        param_request_last_ms = millis();
        break;
    };
    case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { // 参数读取请求
        mavlink_param_request_read_t pkt;
        mavlink_msg_param_request_read_decode(&msg, &pkt);
        const Parameters::Param *p;
        if (pkt.param_index < 0) {
            p = g.find(pkt.param_id);
        } else {
            p = g.find_by_index_float(pkt.param_index);
        }
        float value;
        if (!p || !p->get_as_float(value)) {
            return;
        }
        if (p != nullptr && (p->flags & PARAM_FLAG_HIDDEN)) {
            return;
        }
        mavlink_msg_param_value_send(chan,
                                     p->name, value,
                                     MAV_PARAM_TYPE_REAL32,
                                     g.param_count_float(),
                                     g.param_index_float(p));
        break;
    }
    case MAVLINK_MSG_ID_PARAM_SET: { //  参数设置
        /*
          解析地面站发来的参数设置请求 → 校验参数类型 / 有效性 → 执行锁定安全逻辑（仅允许提高锁定级别）
          → 执行参数修改（或拒绝）→ 回复修改后的参数值给地面站确认。
        */
        mavlink_param_set_t pkt;
        mavlink_msg_param_set_decode(&msg, &pkt);
        auto *p = g.find(pkt.param_id);
        float value;
        if (pkt.param_type != MAV_PARAM_TYPE_REAL32 ||
            !p || !p->get_as_float(value)) {
            return;
        }
        p->get_as_float(value);
        /*
        当模块锁定（lock_level>0）时，只要满足「修改的不是 LOCK_LEVEL 参数」OR「修改 LOCK_LEVEL 但新值≤当前值」
        其中一个条件，就拒绝修改；
        只有「修改 LOCK_LEVEL 且新值 > 当前值」才允许。
        */
        if (g.lock_level > 0 && 
            (strcmp(p->name, "LOCK_LEVEL") != 0 || 
             uint8_t(pkt.param_value) <= uint8_t(value))) {  
            // only param set allowed is to increase lock level 
            mav_printf(MAV_SEVERITY_ERROR, "Parameters locked");
        } else { // 不满足条件 → 允许修改（仅两种情况：1.未锁定；2.锁定但提高LOCK_LEVEL）
            p->set_as_float(pkt.param_value);
        }
        p->get_as_float(value);
        mavlink_msg_param_value_send(chan,
                                     p->name, value,
                                     MAV_PARAM_TYPE_REAL32,
                                     g.param_count_float(),
                                     g.param_index_float(p));
        break;
    }
    case MAVLINK_MSG_ID_SECURE_COMMAND: // 安全指令处理
    case MAVLINK_MSG_ID_SECURE_COMMAND_REPLY: {
        mavlink_secure_command_t pkt;
        mavlink_msg_secure_command_decode(&msg, &pkt);
        handle_secure_command(pkt);
        break;
    }
    default: // 默认分支
        // we don't care about other packets
        break;
    }
}

void MAVLinkSerial::arm_status_send(void)
{   // 1. 判定解锁状态码：
    const uint8_t status = parse_fail==nullptr?MAV_ODID_ARM_STATUS_GOOD_TO_ARM:MAV_ODID_ARM_STATUS_PRE_ARM_FAIL_GENERIC;
    const char *reason = parse_fail==nullptr?"":parse_fail; // 2. 判定失败原因：
    mavlink_msg_open_drone_id_arm_status_send(  // 3. 发送MAVLink OpenDroneID解锁状态消息（ODID标准消息）
        chan,
        status,
        reason);
}