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;
}