QGC校准部分

最新推荐文章于 2025-03-11 14:56:59 发布

San金

最新推荐文章于 2025-03-11 14:56:59 发布

阅读量1.8k

点赞数 1

本文链接：https://blog.csdn.net/qq_44731440/article/details/118639734

版权

QGC校准：

主要是点击校准开始时发送校准的指令，校准的逻辑时飞控里实现，根据飞控回传的校准数据显示校准的步骤

开始校准

SensorsComponentController.cpp中调用_startLogCalibration()函数

void SensorsComponentController::calibrateGyro(void)
{
    _startLogCalibration();
    _vehicle->startCalibration(Vehicle::CalibrationGyro);
}

void SensorsComponentController::calibrateCompass(void)
{
    _startLogCalibration();
    _vehicle->startCalibration(Vehicle::CalibrationMag);
}

void SensorsComponentController::calibrateAccel(void)
{
    _startLogCalibration();
    _vehicle->startCalibration(Vehicle::CalibrationAccel);
}

void SensorsComponentController::calibrateLevel(void)
{
    _startLogCalibration();
    _vehicle->startCalibration(Vehicle::CalibrationLevel);
}

void SensorsComponentController::calibrateAirspeed(void)
{
    _startLogCalibration();
    _vehicle->startCalibration(Vehicle::CalibrationPX4Airspeed);
}

startCalibration(Vehicle::CalibrationType calType)

Vehicle.cc 中：

void Vehicle::startCalibration(Vehicle::CalibrationType calType)
{
    SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock();
if (!sharedLink) {
        qCDebug(VehicleLog) << "startCalibration: primary link gone!";
        return;
    }

    float param1 = 0;
    float param2 = 0;
    float param3 = 0;
    float param4 = 0;
    float param5 = 0;
    float param6 = 0;
    float param7 = 0;

    switch (calType) {
    case CalibrationGyro:
        param1 = 1;
        break;
    case CalibrationMag:
        param2 = 1;
        break;
    case CalibrationRadio:
        param4 = 1;
        break;
    case CalibrationCopyTrims:
        param4 = 2;
        break;
    case CalibrationAccel:
        param5 = 1;
        break;
    case CalibrationLevel:
        param5 = 2;
        break;
    case CalibrationEsc:
        param7 = 1;
        break;
    case CalibrationPX4Airspeed:
        param6 = 1;
        break;
    case CalibrationPX4Pressure:
        param3 = 1;
        break;
    case CalibrationAPMCompassMot:
        param6 = 1;
        break;
    case CalibrationAPMPressureAirspeed:
        param3 = 1;
        break;
    case CalibrationAPMPreFlight:
        param3 = 1; // GroundPressure/Airspeed
        if (multiRotor() || rover()) {
            // Gyro cal for ArduCopter only
            param1 = 1;
        }
    }

    // We can't use sendMavCommand here since we have no idea how long it will be before the command returns a result. This in turn
// causes the retry logic to break down.
//  我们不能在这里使用sendMavCommand，因为我们不知道在命令返回结果之前需要多长时间。这反过来
// 导致重试逻辑崩溃
    mavlink_message_t msg;
mavlink_msg_command_long_pack_chan
(
_mavlink->getSystemId(),
_mavlink->getComponentId(),
sharedLink->mavlinkChannel(),
&msg,
 id(),
    defaultComponentId(),            // target component
    MAV_CMD_PREFLIGHT_CALIBRATION,    // command id
                                       
0,         // 0=first transmission of command 0=第一次发送命令
  param1, param2, param3, param4, param5, param6, param7
);
    sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
}

mavlink_message_t 结构体

就是一个mavlink帧格式？

typedef struct __mavlink_message {
	uint16_t checksum;（检验和）   ///< sent at end of packet在包的末端发送
	uint8_t magic;          ///< protocol magic marker
	uint8_t len;         ///< Length of payload有效载荷长度
	uint8_t incompat_flags; ///< flags that must be understood
	uint8_t compat_flags;   ///< flags that can be ignored if not understood
	uint8_t seq;            ///< Sequence of packet
	uint8_t sysid;          ///< ID of message sender system/aircraft
	uint8_t compid;         ///< ID of the message sender component
	uint32_t msgid:24;      ///< ID of message in payload
	uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
	uint8_t ck[2];          ///< incoming checksum bytes
	uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN];
}) mavlink_message_t;

Mavlink_msg_command_long_pack_chan

Mavlink_msg_command_long.h 中的 Mavlink_msg_command_long_pack_chan

/**
 * @brief Pack a command_long message on a channel 
//  在通道上打包command_long消息
 * @param system_id ID of this system 
//  system_id系统ID
 * @param component_id ID of this component (e.g. 200 for IMU) 
// component_id该组件的ID(例如200为IMU)
 * @param chan The MAVLink channel this message will be sent over 
// chan将发送此消息的MAVLink通道
 * @param msg The MAVLink message to compress the data into 
// msg用于压缩数据的MAVLink消息
 * @param target_system  System which should execute the command //target_system应该执行该命令的系统
 * @param target_component  Component which should execute the command, 0 for all components 
// target_component应该执行该命令的组件，0表示所有组件
 * @param command  Command ID (of command to send). 
// command命令ID(发送命令)
 * @param confirmation  0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) 
// confirmation 0:第一次传输此命令。1-255:确认传输
 * @param param1  Parameter 1 (for the specific command).
 * @param param2  Parameter 2 (for the specific command).
 * @param param3  Parameter 3 (for the specific command).
 * @param param4  Parameter 4 (for the specific command).
 * @param param5  Parameter 5 (for the specific command).
 * @param param6  Parameter 6 (for the specific command).
 * @param param7  Parameter 7 (for the specific command).
 * @return length of the message in bytes (excluding serial stream start sign) 
// return 消息的字节长度(不包括串行流开始符号)
 */
static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
                               mavlink_message_t* msg,
                                   uint8_t target_system,uint8_t target_component,uint16_t command,uint8_t confirmation,float param1,float param2,float param3,float param4,float param5,float param6,float param7)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
    char buf[MAVLINK_MSG_ID_COMMAND_LONG_LEN];
    _mav_put_float(buf, 0, param1);
    _mav_put_float(buf, 4, param2);
    _mav_put_float(buf, 8, param3);
    _mav_put_float(buf, 12, param4);
    _mav_put_float(buf, 16, param5);
    _mav_put_float(buf, 20, param6);
    _mav_put_float(buf, 24, param7);
    _mav_put_uint16_t(buf, 28, command);
    _mav_put_uint8_t(buf, 30, target_system);
    _mav_put_uint8_t(buf, 31, target_component);
    _mav_put_uint8_t(buf, 32, confirmation);

        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
#else
    mavlink_command_long_t packet;
    packet.param1 = param1;
    packet.param2 = param2;
    packet.param3 = param3;
    packet.param4 = param4;
    packet.param5 = param5;
    packet.param6 = param6;
    packet.param7 = param7;
    packet.command = command;
    packet.target_system = target_system;
    packet.target_component = target_component;
    packet.confirmation = confirmation;

        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
#endif

    msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG;
    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_LONG_MIN_LEN, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC);
}

sendMessageOnlinkThreadSafe

// 在链路安全中发送信息

startCalibration 函数中sendMessageOnlinkThreadSafe 传入

//sendMessageOnLinkThreadSafe(sharedLink.get(), msg);

SendMessageOnlinkThreadSafe(接口，消息包)

bool Vehicle::sendMessageOnLinkThreadSafe(LinkInterface* link, mavlink_message_t message)
{
    if (!link->isConnected()) {
        qCDebug(VehicleLog) << "sendMessageOnLinkThreadSafe" << link << "not connected!";
        return false;
    }

// Give the plugin a chance to adjust
//给插件一个调整的机会
    _firmwarePlugin->adjustOutgoingMavlinkMessageThreadSafe(this, link, &message);

// Write message into buffer, prepending start sign
// 将消息写入缓冲区，预挂起开始号
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    int len = mavlink_msg_to_send_buffer(buffer, &message);

    link->writeBytesThreadSafe((const char*)buffer, len);
    _messagesSent++;
    emit messagesSentChanged();

    return true;
}