Ardupilot 航线规划代码学习
目录
文章目录
- 目录
- 摘要
- 1.接收外部mavlink数据
- 1.写任务列表
- 2.发送任务项目的航点数量
- 3.发送任务项目存储到EEPROM中
- 4.从EEPROM中读取航点信息
- 2.Missionplanner进行解锁，起飞，模式切换命令
- 3.设定运行自动模式代码
- 1.自动起飞
- 2.运行航线代码
摘要
本节主要记录自己学习ardupilot 航线规划代码的过程。
整体思路：无人机操作者通过手机APP或者Missionplanner软件，规划任务航线，航线规划完成后，借助无线通信模块（蓝牙/数传），把规划好的航线发送到无人机的飞控蓝牙或者数传接收端，飞控对蓝牙或者数传接收到的数据进行解析，把收到的航线任务存放到EEPROM中，为将要执行的航线任务做好了航点准备。此时采用手机APP或者Missionplanner发送解锁，起飞命令，然后切换AUTO模式，无人机开始执行auto模式，在auto模式下，无人机会不停的从EEPROM中读取目标任务点与实际的任务点进行对比，然后通过位置PID控制，姿态PID控制，最终输出需要的控制量的PWM到无人机的电调，进而实现控制电机的旋转大小，来实现无人机位置的调节，以实现航线任务规划。
1.接收外部mavlink数据
SCHED_TASK(gcs_check_input, 400, 180), //检测输入
void Copter::gcs_check_input(void)
{
gcs().update(); //其中GCS_Copter &gcs() { return _gcs; }
}
void GCS::update(void)
{
for (uint8_t i=0; ivoid GCS_MAVLINK::update(uint32_t max_time_us)
{
//接收新数据包------receive new packets
mavlink_message_t msg;
mavlink_status_t status;
uint32_t tstart_us = AP_HAL::micros();
uint32_t now_ms = AP_HAL::millis();
hal.util->perf_begin(_perf_update);
status.packet_rx_drop_count = 0;
//处理接收的字节-----process received bytes
uint16_t nbytes = comm_get_available(chan);
for (uint16_t i=0; iread();
const uint32_t protocol_timeout = 4000;
if (alternative.handler && now_ms - alternative.last_mavlink_ms > protocol_timeout)
{
/*我们安装了一个替代的协议处理程序，已4秒钟未分析mavlink数据包。尝试使用替代处理程序分析
we have an alternative protocol handler installed and we
haven't parsed a MAVLink packet for 4 seconds. Try
parsing using alternative handler
*/
if (alternative.handler(c, mavlink_comm_port[chan]))
{
alternative.last_alternate_ms = now_ms;
gcs_alternative_active[chan] = true;
}
/*如果4s没有成功解析的替代协议，我们也可以尝试解析为mavlink
we may also try parsing as MAVLink if we haven't had asuccessful parse on the alternative protocol for 4s
*/
if (now_ms - alternative.last_alternate_ms <= protocol_timeout)
{
continue;
}
}
bool parsed_packet = false;
//尝试获取新消息------ Try to get a new message
if (mavlink_parse_char(chan, c, &msg, &status)) //数据解析成功
{
hal.util->perf_begin(_perf_packet);
packetReceived(status, msg); //数据接收
hal.util->perf_end(_perf_packet);
parsed_packet = true;
gcs_alternative_active[chan] = false;
alternative.last_mavlink_ms = now_ms;
}
if (parsed_packet || i % 100 == 0)
{
// make sure we don't spend too much time parsing mavlink messages
if (AP_HAL::micros() - tstart_us > max_time_us)
{
break;
}
}
}
const uint32_t tnow = AP_HAL::millis();
// send a timesync message every 10 seconds; this is for data
// collection purposes
if (tnow - _timesync_request.last_sent_ms > _timesync_request.interval_ms)
{
if (HAVE_PAYLOAD_SPACE(chan, TIMESYNC))
{
send_timesync();
_timesync_request.last_sent_ms = tnow;
}
}
if (waypoint_receiving)
{
const uint32_t wp_recv_time = 1000U + (stream_slowdown*20);
// stop waypoint receiving if timeout
if (tnow - waypoint_timelast_receive > wp_recv_time+waypoint_receive_timeout)
{
waypoint_receiving = false;
} else if (tnow - waypoint_timelast_request > wp_recv_time)
{
waypoint_timelast_request = tnow;
send_message(MSG_NEXT_WAYPOINT);
}
}
hal.util->perf_end(_perf_update);
}
函数分析1：mavlink_parse_char(chan, c, &msg, &status)
MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
{
uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
if (msg_received == MAVLINK_FRAMING_BAD_CRC) {
// we got a bad CRC. Treat as a parse failure
mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);
mavlink_status_t* status = mavlink_get_channel_status(chan);
status->parse_error++;
status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
if (c == MAVLINK_STX)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
rxmsg->len = 0;
mavlink_start_checksum(rxmsg);
}
return 0;
}
return msg_received;
}
解析数据成功后，开始进行包数据处理，执行函数分析2:packetReceived(status, msg); //数据接收
void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,mavlink_message_t &msg)
{
// we exclude radio packets because we historically used this to
// make it possible to use the CLI over the radio
if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS)
{
mavlink_active |= (1U<<(chan-MAVLINK_COMM_0));
}
if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) &&
(status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) &&
serialmanager_p &&
serialmanager_p->get_mavlink_protocol(chan) == AP_SerialManager::SerialProtocol_MAVLink2)
{
// if we receive any MAVLink2 packets on a connection
// currently sending MAVLink1 then switch to sending
// MAVLink2
mavlink_status_t *cstatus = mavlink_get_channel_status(chan);
if (cstatus != nullptr) {
cstatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
}
}
if (routing.check_and_forward(chan, &msg) &&accept_packet(status, msg))
{
handleMessage(&msg); //处理
}
}
函数分析3： handleMessage(&msg); //处理
void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
{
MAV_RESULT result = MAV_RESULT_FAILED; // assume failure. Each messages id is responsible for return ACK or NAK if required
switch (msg->msgid) //消息ID
{
/***************************************心跳包数据MAV ID: 0，是心跳********************************************************/
case MAVLINK_MSG_ID_HEARTBEAT:
{
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
if(msg->sysid != copter.g.sysid_my_gcs) break;
copter.failsafe.last_heartbeat_ms = AP_HAL::millis();
break;
}
/***************************************MAV ID:22是参数数据**********************************************************************/
case MAVLINK_MSG_ID_PARAM_VALUE:
{
#if MOUNT == ENABLED
copter.camera_mount.handle_param_value(msg);
#endif
break;
}
/***************************************MAV ID:200万向节数据**********************************************************************/
case MAVLINK_MSG_ID_GIMBAL_REPORT:
{
#if MOUNT == ENABLED
handle_gimbal_report(copter.camera_mount, msg);
#endif
break;
}
/***************************************MAV ID: 70遥控器数据**********************************************************************/
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
{
// allow override of RC channel values for HIL
// or for complete GCS control of switch position
// and RC PWM values.
if(msg->sysid != copter.g.sysid_my_gcs)
{
break; // Only accept control from our gcs
}
if (!copter.ap.rc_override_enable)
{
if (copter.failsafe.rc_override_active) // if overrides were active previously, disable them
{
copter.failsafe.rc_override_active = false;
RC_Channels::clear_overrides();
}
break;
}
uint32_t tnow = AP_HAL::millis();
mavlink_rc_channels_override_t packet;
mavlink_msg_rc_channels_override_decode(msg, &packet);
RC_Channels::set_override(0, packet.chan1_raw, tnow);
RC_Channels::set_override(1, packet.chan2_raw, tnow);
RC_Channels::set_override(2, packet.chan3_raw, tnow);
RC_Channels::set_override(3, packet.chan4_raw, tnow);
RC_Channels::set_override(4, packet.chan5_raw, tnow);
RC_Channels::set_override(5, packet.chan6_raw, tnow);
RC_Channels::set_override(6, packet.chan7_raw, tnow);
RC_Channels::set_override(7, packet.chan8_raw, tnow);
// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
copter.failsafe.rc_override_active = RC_Channels::has_active_overrides();
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
copter.failsafe.last_heartbeat_ms = tnow;
break;
}
/***************************************MAV ID: 69手动控制**********************************************************************/
case MAVLINK_MSG_ID_MANUAL_CONTROL:
{
if (msg->sysid != copter.g.sysid_my_gcs)
{
break; // only accept control from our gcs
}
mavlink_manual_control_t packet;
mavlink_msg_manual_control_decode(msg, &packet);
if (packet.target != copter.g.sysid_this_mav) {
break; // only accept control aimed at us
}
if (packet.z < 0) { // Copter doesn't do negative thrust
break;
}
uint32_t tnow = AP_HAL::millis();
int16_t roll = (packet.y == INT16_MAX) ? 0 : copter.channel_roll->get_radio_min() + (copter.channel_roll->get_radio_max() - copter.channel_roll->get_radio_min()) * (packet.y + 1000) / 2000.0f;
int16_t pitch = (packet.x == INT16_MAX) ? 0 : copter.channel_pitch->get_radio_min() + (copter.channel_pitch->get_radio_max() - copter.channel_pitch->get_radio_min()) * (-packet.x + 1000) / 2000.0f;
int16_t throttle = (packet.z == INT16_MAX) ? 0 : copter.channel_throttle->get_radio_min() + (copter.channel_throttle->get_radio_max() - copter.channel_throttle->get_radio_min()) * (packet.z) / 1000.0f;
int16_t yaw = (packet.r == INT16_MAX) ? 0 : copter.channel_yaw->get_radio_min() + (copter.channel_yaw->get_radio_max() - copter.channel_yaw->get_radio_min()) * (packet.r + 1000) / 2000.0f;
RC_Channels::set_override(uint8_t(copter.rcmap.roll() - 1), roll, tnow);
RC_Channels::set_override(uint8_t(copter.rcmap.pitch() - 1), pitch, tnow);
RC_Channels::set_override(uint8_t(copter.rcmap.throttle() - 1), throttle, tnow);
RC_Channels::set_override(uint8_t(copter.rcmap.yaw() - 1), yaw, tnow);
// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
copter.failsafe.rc_override_active = RC_Channels::has_active_overrides();
// a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes
copter.failsafe.last_heartbeat_ms = tnow;
break;
}
/***************************************MAV ID: 75命令初始化**********************************************************************/
case MAVLINK_MSG_ID_COMMAND_INT: //协议75
{
// decode packet
mavlink_command_int_t packet;
mavlink_msg_command_int_decode(msg, &packet);
// hal.console->printf("packet.param1=%f\r\n",packet.param1);
// hal.console->printf("packet.param2=%f\r\n",packet.param2);
// hal.console->printf("packet.param3=%f\r\n",packet.param3);
// hal.console->printf("packet.param4=%f\r\n",packet.param4);
// hal.console->printf("packet.command=%d\r\n",packet.command);
// hal.console->printf("packet.target_system=%f\r\n",packet.target_system);
// hal.console->printf("packet.target_component=%d\r\n",packet.target_component);
// hal.console->printf("packet.frame=%f\r\n",packet.frame);
// hal.console->printf("packet.current=%f\r\n",packet.current);
switch(packet.command)
{
case MAV_CMD_DO_FOLLOW:
#if MODE_FOLLOW_ENABLED == ENABLED
// param1: sysid of target to follow
if ((packet.param1 > 0) && (packet.param1 <= 255))
{
copter.g2.follow.set_target_sysid((uint8_t)packet.param1);
result = MAV_RESULT_ACCEPTED;
}
#endif
break;
case MAV_CMD_DO_SET_HOME: //179
{
// assume failure
result = MAV_RESULT_FAILED;
if (is_equal(packet.param1, 1.0f))
{
// if param1 is 1, use current location
if (copter.set_home_to_current_location(true))
{
result = MAV_RESULT_ACCEPTED;
}
break;
}
// ensure param1 is zero
if (!is_zero(packet.param1))
{
break;
}
// check frame type is supported
if (packet.frame != MAV_FRAME_GLOBAL &&
packet.frame != MAV_FRAME_GLOBAL_INT &&
packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)
{
break;
}
// sanity check location
if (!check_latlng(packet.x, packet.y))
{
break;
}
Location new_home_loc {};
new_home_loc.lat = packet.x;
new_home_loc.lng = packet.y;
new_home_loc.alt = packet.z * 100;
// handle relative altitude
if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)
{
if (!AP::ahrs().home_is_set())
{
// cannot use relative altitude if home is not set
break;
}
new_home_loc.alt += copter.ahrs.get_home().alt;
}
if (copter.set_home(new_home_loc, true))
{
result = MAV_RESULT_ACCEPTED;
}
break;
}
case MAV_CMD_DO_SET_ROI:
{
// param1 : /* Region of interest mode (not used)*/
// param2 : /* MISSION index/ target ID (not used)*/
// param3 : /* ROI index (not used)*/
// param4 : /* empty */
// x : lat
// y : lon
// z : alt
// sanity check location
if (!check_latlng(packet.x, packet.y)) {
break;
}
Location roi_loc;
roi_loc.lat = packet.x;
roi_loc.lng = packet.y;
roi_loc.alt = (int32_t)(packet.z * 100.0f);
copter.flightmode->auto_yaw.set_roi(roi_loc);
result = MAV_RESULT_ACCEPTED;
break;
}
default:
result = MAV_RESULT_UNSUPPORTED;
break;
}
// send ACK or NAK
mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result);
break;
}
/***************************************MAV ID: 76准备飞行请求信息Pre-Flight calibration requests**********************************************************************/
case MAVLINK_MSG_ID_COMMAND_LONG: // MAV ID: 76
{
// decode packet
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
// hal.console->printf("^^^^^^^^^^^^^^^^^^\r\n");
// hal.console->printf("packet.param1=%f\r\n",packet.param1); //0
// hal.console->printf("packet.param2=%f\r\n",packet.param2);//0
// hal.console->printf("packet.param3=%f\r\n",packet.param3);//0
// hal.console->printf("packet.param4=%f\r\n",packet.param4);//0
// hal.console->printf("packet.param5=%f\r\n",packet.param5);//22.7
// hal.console->printf("packet.param6=%f\r\n",packet.param6);//114
// hal.console->printf("packet.param7=%f\r\n",packet.param7);//108.6
// hal.console->printf("packet.command=%d\r\n",packet.command);//179
// hal.console->printf("packet.target_system=%f\r\n",packet.target_system);//0
// hal.console->printf("packet.target_component=%d\r\n",packet.target_component);//1
//
//
// hal.console->printf("^^^^^^^^^^^^*****^^^^^\r\n");
//检测喷洒是否开启
if(packet.command==183&&packet.param1==10)
{
if(packet.param2>1500)
spray_on=true;
else
spray_on=false;
}
switch(packet.command)
{
case MAV_CMD_NAV_TAKEOFF:
{
// param3 : horizontal navigation by pilot acceptable
// param4 : yaw angle (not supported)
// param5 : latitude (not supported)
// param6 : longitude (not supported)
// param7 : altitude [metres]
float takeoff_alt = packet.param7 * 100; // Convert m to cm
if (copter.flightmode->do_user_takeoff(takeoff_alt, is_zero(packet.param3))) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
break;
}
case MAV_CMD_NAV_LOITER_UNLIM:
if (copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND))
{
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (copter.set_mode(RTL, MODE_REASON_GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_NAV_LAND:
if (copter.set_mode(LAND, MODE_REASON_GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_FOLLOW:
#if MODE_FOLLOW_ENABLED == ENABLED
// param1: sysid of target to follow
if ((packet.param1 > 0) && (packet.param1 <= 255)) {
copter.g2.follow.set_target_sysid((uint8_t)packet.param1);
result = MAV_RESULT_ACCEPTED;
}
#endif
break;
case MAV_CMD_CONDITION_YAW:
// param1 : target angle [0-360]
// param2 : speed during change [deg per second]
// param3 : direction (-1:ccw, +1:cw)
// param4 : relative offset (1) or absolute angle (0)
if ((packet.param1 >= 0.0f) &&
(packet.param1 <= 360.0f) &&
(is_zero(packet.param4) || is_equal(packet.param4,1.0f))) {
copter.flightmode->auto_yaw.set_fixed_yaw(
packet.param1,
packet.param2,
(int8_t)packet.param3,
is_positive(packet.param4));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
break;
case MAV_CMD_DO_CHANGE_SPEED:
// param1 : unused
// param2 : new speed in m/s
// param3 : unused
// param4 : unused
if (packet.param2 > 0.0f) {
copter.wp_nav->set_speed_xy(packet.param2 * 100.0f);
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
break;
case MAV_CMD_DO_SET_HOME:
// param1 : use current (1=use current location, 0=use specified location)
// param5 : latitude
// param6 : longitude
// param7 : altitude (absolute)
result = MAV_RESULT_FAILED; // assume failure
if (is_equal(packet.param1,1.0f))
{
if (copter.set_home_to_current_location(true))
{
result = MAV_RESULT_ACCEPTED;
}
} else
{
// ensure param1 is zero
if (!is_zero(packet.param1))
{
break;
}
// sanity check location
if (!check_latlng(packet.param5, packet.param6))
{
break;
}
Location new_home_loc;
new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
if (copter.set_home(new_home_loc, true))
{
result = MAV_RESULT_ACCEPTED;
}
}
break;
case MAV_CMD_DO_SET_ROI:
// param1 : regional of interest mode (not supported)
// param2 : mission index/ target id (not supported)
// param3 : ROI index (not supported)
// param5 : x / lat
// param6 : y / lon
// param7 : z / alt
// sanity check location
if (!check_latlng(packet.param5, packet.param6)) {
break;
}
Location roi_loc;
roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
copter.flightmode->auto_yaw.set_roi(roi_loc);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_DO_MOUNT_CONTROL:
#if MOUNT == ENABLED
if(!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_fixed_yaw(
(float)packet.param3 / 100.0f,
0.0f,
0,0);
}
copter.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
result = MAV_RESULT_ACCEPTED;
#endif
break;
#if MODE_AUTO_ENABLED == ENABLED
case MAV_CMD_MISSION_START:
if (copter.motors->armed() && copter.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) {
copter.set_auto_armed(true);
if (copter.mission.state() != AP_Mission::MISSION_RUNNING) {
copter.mission.start_or_resume();
}
result = MAV_RESULT_ACCEPTED;
}
break;
#endif
case MAV_CMD_COMPONENT_ARM_DISARM:
if (is_equal(packet.param1,1.0f)) {
// attempt to arm and return success or failure
const bool do_arming_checks = !is_equal(packet.param2,magic_force_arm_value);
if (copter.init_arm_motors(true, do_arming_checks)) {
result = MAV_RESULT_ACCEPTED;
}
} else if (is_zero(packet.param1)) {
if (copter.ap.land_complete || is_equal(packet.param2,magic_force_disarm_value)) {
// force disarming by setting param2 = 21196 is deprecated
copter.init_disarm_motors();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else {
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) {
AP_Notify::flags.firmware_update = 1;
copter.notify.update();
hal.scheduler->delay(200);
// when packet.param1 == 3 we reboot to hold in bootloader
hal.scheduler->reboot(is_equal(packet.param1,3.0f));
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_FENCE_ENABLE:
#if AC_FENCE == ENABLED
result = MAV_RESULT_ACCEPTED;
switch ((uint16_t)packet.param1) {
case 0:
copter.fence.enable(false);
break;
case 1:
copter.fence.enable(true);
break;
default:
result = MAV_RESULT_FAILED;
break;
}
#else
// if fence code is not included return failure
result = MAV_RESULT_FAILED;
#endif
break;
#if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE:
// configure or release parachute
result = MAV_RESULT_ACCEPTED;
switch ((uint16_t)packet.param1)
{
case PARACHUTE_DISABLE:
copter.parachute.enabled(false);
copter.Log_Write_Event(DATA_PARACHUTE_DISABLED);
break;
case PARACHUTE_ENABLE:
copter.parachute.enabled(true);
copter.Log_Write_Event(DATA_PARACHUTE_ENABLED);
break;
case PARACHUTE_RELEASE:
// treat as a manual release which performs some additional check of altitude
copter.parachute_manual_release();
break;
default:
result = MAV_RESULT_FAILED;
break;
}
break;
#endif
case MAV_CMD_DO_MOTOR_TEST:
// hal.uartC->printf("packet.param1=%f\r\n",packet.param1);
// hal.uartC->printf("packet.param2=%f\r\n",packet.param2);
// hal.uartC->printf("packet.param3=%f\r\n",packet.param3);
// hal.uartC->printf("packet.param4=%f\r\n",packet.param4);
// hal.uartC->printf("packet.param5=%f\r\n",packet.param5);
// hal.uartC->printf("packet.param6=%f\r\n",packet.param6);
// hal.uartC->printf("packet.param7=%f\r\n",packet.param7);
// param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
// param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
// param3 : throttle (range depends upon param2)
// param4 : timeout (in seconds)
// param5 : num_motors (in sequence)
// param6 : compass learning (0: disabled, 1: enabled)
result = copter.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3,
packet.param4, (uint8_t)packet.param5);
break;
#if WINCH_ENABLED == ENABLED
case MAV_CMD_DO_WINCH:
// param1 : winch number (ignored)
// param2 : action (0=relax, 1=relative length control, 2=rate control). See WINCH_ACTIONS enum.
if (!copter.g2.winch.enabled()) {
result = MAV_RESULT_FAILED;
} else {
result = MAV_RESULT_ACCEPTED;
switch ((uint8_t)packet.param2) {
case WINCH_RELAXED:
copter.g2.winch.relax();
copter.Log_Write_Event(DATA_WINCH_RELAXED);
break;
case WINCH_RELATIVE_LENGTH_CONTROL: {
copter.g2.winch.release_length(packet.param3, fabsf(packet.param4));
copter.Log_Write_Event(DATA_WINCH_LENGTH_CONTROL);
break;
}
case WINCH_RATE_CONTROL: {
if (fabsf(packet.param4) <= copter.g2.winch.get_rate_max()) {
copter.g2.winch.set_desired_rate(packet.param4);
copter.Log_Write_Event(DATA_WINCH_RATE_CONTROL);
} else {
result = MAV_RESULT_FAILED;
}
break;
}
default:
result = MAV_RESULT_FAILED;
break;
}
}
break;
#endif
case MAV_CMD_AIRFRAME_CONFIGURATION:
{
// Param 1: Select which gear, not used in ArduPilot
// Param 2: 0 = Deploy, 1 = Retract
// For safety, anything other than 1 will deploy
switch ((uint8_t)packet.param2)
{
case 1:
copter.landinggear.set_position(AP_LandingGear::LandingGear_Retract);
result = MAV_RESULT_ACCEPTED;
break;
default:
copter.landinggear.set_position(AP_LandingGear::LandingGear_Deploy);
result = MAV_RESULT_ACCEPTED;
break;
}
break;
}
/* Solo user presses Fly button */
case MAV_CMD_SOLO_BTN_FLY_CLICK:
{
result = MAV_RESULT_ACCEPTED;
if (copter.failsafe.radio) {
break;
}
// set mode to Loiter or fall back to AltHold
if (!copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
}
break;
}
/* Solo user holds down Fly button for a couple of seconds */
case MAV_CMD_SOLO_BTN_FLY_HOLD: {
result = MAV_RESULT_ACCEPTED;
if (copter.failsafe.radio) {
break;
}
if (!copter.motors->armed()) {
// if disarmed, arm motors
copter.init_arm_motors(true);
} else if (copter.ap.land_complete) {
// if armed and landed, takeoff
if (copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
copter.flightmode->do_user_takeoff(packet.param1*100, true);
}
} else {
// if flying, land
copter.set_mode(LAND, MODE_REASON_GCS_COMMAND);
}
break;
}
/* Solo user presses pause button */
case MAV_CMD_SOLO_BTN_PAUSE_CLICK: {
result = MAV_RESULT_ACCEPTED;
if (copter.failsafe.radio) {
break;
}
if (copter.motors->armed()) {
if (copter.ap.land_complete) {
// if landed, disarm motors
copter.init_disarm_motors();
} else {
// assume that shots modes are all done in guided.
// NOTE: this may need to change if we add a non-guided shot mode
bool shot_mode = (!is_zero(packet.param1) && (copter.control_mode == GUIDED || copter.control_mode == GUIDED_NOGPS));
if (!shot_mode) {
#if MODE_BRAKE_ENABLED == ENABLED
if (copter.set_mode(BRAKE, MODE_REASON_GCS_COMMAND)) {
copter.mode_brake.timeout_to_loiter_ms(2500);
} else {
copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
}
#else
copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
#endif
} else {
// SoloLink is expected to handle pause in shots
}
}
}
break;
}
case MAV_CMD_ACCELCAL_VEHICLE_POS:
result = MAV_RESULT_FAILED;
if (copter.ins.get_acal()->gcs_vehicle_position(packet.param1)) {
result = MAV_RESULT_ACCEPTED;
}
break;
default:
result = handle_command_long_message(packet);
break;
}
// send ACK or NAK
mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result);
break;
}
/***************************************MAV ID: 82设定姿态目标**********************************************************************/
#if MODE_GUIDED_ENABLED == ENABLED
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82
{
// decode packet
mavlink_set_attitude_target_t packet;
mavlink_msg_set_attitude_target_decode(msg, &packet);
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!copter.flightmode->in_guided_mode()) {
break;
}
// ensure type_mask specifies to use attitude and thrust
if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) {
break;
}
// convert thrust to climb rate
packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f);
float climb_rate_cms = 0.0f;
if (is_equal(packet.thrust, 0.5f)) {
climb_rate_cms = 0.0f;
} else if (packet.thrust > 0.5f) {
// climb at up to WPNAV_SPEED_UP
climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_speed_up();
} else {
// descend at up to WPNAV_SPEED_DN
climb_rate_cms = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav->get_speed_down());
}
// if the body_yaw_rate field is ignored, use the commanded yaw position
// otherwise use the commanded yaw rate
bool use_yaw_rate = false;
if ((packet.type_mask & (1<<2)) == 0) {
use_yaw_rate = true;
}
copter.mode_guided.set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
climb_rate_cms, use_yaw_rate, packet.body_yaw_rate);
break;
}
/***************************************MAV ID: 84设定位置目标**********************************************************************/
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84
{
// decode packet
mavlink_set_position_target_local_ned_t packet;
mavlink_msg_set_position_target_local_ned_decode(msg, &packet);
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!copter.flightmode->in_guided_mode())
{
break;
}
// check for supported coordinate frames
if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_NED &&
packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
break;
}
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
/*
* for future use:
* bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;
*/
// prepare position
Vector3f pos_vector;
if (!pos_ignore) {
// convert to cm
pos_vector = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);
// rotate to body-frame if necessary
if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
copter.rotate_body_frame_to_NE(pos_vector.x, pos_vector.y);
}
// add body offset if necessary
if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||
packet.coordinate_frame == MAV_FRAME_BODY_NED ||
packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
pos_vector += copter.inertial_nav.get_position();
} else {
// convert from alt-above-home to alt-above-ekf-origin
pos_vector.z = copter.pv_alt_above_origin(pos_vector.z);
}
}
// prepare velocity
Vector3f vel_vector;
if (!vel_ignore) {
// convert to cm
vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f);
// rotate to body-frame if necessary
if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y);
}
}
// prepare yaw
float yaw_cd = 0.0f;
bool yaw_relative = false;
float yaw_rate_cds = 0.0f;
if (!yaw_ignore) {
yaw_cd = ToDeg(packet.yaw) * 100.0f;
yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
}
if (!yaw_rate_ignore) {
yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
// send request
if (!pos_ignore && !vel_ignore && acc_ignore) {
if (copter.mode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (pos_ignore && !vel_ignore && acc_ignore) {
copter.mode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
result = MAV_RESULT_ACCEPTED;
} else if (!pos_ignore && vel_ignore && acc_ignore) {
if (copter.mode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else {
result = MAV_RESULT_FAILED;
}
break;
}
/***************************************MAV ID: 86设定位置目标全局初始化**********************************************************************/
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86
{
// decode packet
mavlink_set_position_target_global_int_t packet;
mavlink_msg_set_position_target_global_int_decode(msg, &packet);
// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!copter.flightmode->in_guided_mode()) {
break;
}
// check for supported coordinate frames
if (packet.coordinate_frame != MAV_FRAME_GLOBAL &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INT
packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT &&
packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
break;
}
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
/*
* for future use:
* bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;
*/
Vector3f pos_neu_cm; // position (North, East, Up coordinates) in centimeters
if(!pos_ignore) {
// sanity check location
if (!check_latlng(packet.lat_int, packet.lon_int)) {
result = MAV_RESULT_FAILED;
break;
}
Location loc;
loc.lat = packet.lat_int;
loc.lng = packet.lon_int;
loc.alt = packet.alt*100;
switch (packet.coordinate_frame) {
case MAV_FRAME_GLOBAL_RELATIVE_ALT: // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INT
case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
loc.flags.relative_alt = true;
loc.flags.terrain_alt = false;
break;
case MAV_FRAME_GLOBAL_TERRAIN_ALT:
case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
loc.flags.relative_alt = true;
loc.flags.terrain_alt = true;
break;
case MAV_FRAME_GLOBAL:
case MAV_FRAME_GLOBAL_INT:
default:
// pv_location_to_vector does not support absolute altitudes.
// Convert the absolute altitude to a home-relative altitude before calling pv_location_to_vector
loc.alt -= copter.ahrs.get_home().alt;
loc.flags.relative_alt = true;
loc.flags.terrain_alt = false;
break;
}
pos_neu_cm = copter.pv_location_to_vector(loc);
}
// prepare yaw
float yaw_cd = 0.0f;
bool yaw_relative = false;
float yaw_rate_cds = 0.0f;
if (!yaw_ignore) {
yaw_cd = ToDeg(packet.yaw) * 100.0f;
yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
}
if (!yaw_rate_ignore) {
yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
}
if (!pos_ignore && !vel_ignore && acc_ignore) {
if (copter.mode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (pos_ignore && !vel_ignore && acc_ignore) {
copter.mode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
result = MAV_RESULT_ACCEPTED;
} else if (!pos_ignore && vel_ignore && acc_ignore) {
if (copter.mode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else {
result = MAV_RESULT_FAILED;
}
break;
}
#endif
/***************************************MAV ID: 132距离传感器信息**********************************************************************/
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
{
result = MAV_RESULT_ACCEPTED;
copter.rangefinder.handle_msg(msg);
#if PROXIMITY_ENABLED == ENABLED
copter.g2.proximity.handle_msg(msg);
#endif
break;
}
/***************************************MAV ID: 90仿真信息**********************************************************************/
#if HIL_MODE != HIL_MODE_DISABLED
case MAVLINK_MSG_ID_HIL_STATE: // MAV ID: 90
{
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
// sanity check location
if (!check_latlng(packet.lat, packet.lon)) {
break;
}
// set gps hil sensor
Location loc;
loc.lat = packet.lat;
loc.lng = packet.lon;
loc.alt = packet.alt/10;
Vector3f vel(packet.vx, packet.vy, packet.vz);
vel *= 0.01f;
gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
packet.time_usec/1000,
loc, vel, 10, 0);
// rad/sec
Vector3f gyros;
gyros.x = packet.rollspeed;
gyros.y = packet.pitchspeed;
gyros.z = packet.yawspeed;
// m/s/s
Vector3f accels;
accels.x = packet.xacc * (GRAVITY_MSS/1000.0f);
accels.y = packet.yacc * (GRAVITY_MSS/1000.0f);
accels.z = packet.zacc * (GRAVITY_MSS/1000.0f);
ins.set_gyro(0, gyros);
ins.set_accel(0, accels);
AP::baro().setHIL(packet.alt*0.001f);
copter.compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
copter.compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);
break;
}
#endif // HIL_MODE != HIL_MODE_DISABLED
/***************************************MAV ID: 166遥控信息，109遥控器状态**********************************************************************/
case MAVLINK_MSG_ID_RADIO:
case MAVLINK_MSG_ID_RADIO_STATUS: // MAV ID: 109
{
handle_radio_status(msg, copter.DataFlash, copter.should_log(MASK_LOG_PM));
break;
}
/***************************************MAV ID: 149着陆信息**********************************************************************/
#if PRECISION_LANDING == ENABLED
case MAVLINK_MSG_ID_LANDING_TARGET:
result = MAV_RESULT_ACCEPTED;
copter.precland.handle_msg(msg);
break;
#endif
/***************************************MAV ID: 160,161栅栏信息**********************************************************************/
#if AC_FENCE == ENABLED
// send or receive fence points with GCS
case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160
case MAVLINK_MSG_ID_FENCE_FETCH_POINT:
copter.fence.handle_msg(*this, msg);
break;
#endif // AC_FENCE == ENABLED
/***************************************MAV ID: 204，157挂载信息**********************************************************************/
#if MOUNT == ENABLED
//deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
case MAVLINK_MSG_ID_MOUNT_CONFIGURE: // MAV ID: 204
copter.camera_mount.configure_msg(msg);
break;
//deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
case MAVLINK_MSG_ID_MOUNT_CONTROL:
if(!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_fixed_yaw(
mavlink_msg_mount_control_get_input_c(msg)/100.0f,
0.0f,
0,
0);
}
copter.camera_mount.control_msg(msg);
break;
#endif // MOUNT == ENABLED
/***************************************MAV ID: 134,135地形信息**********************************************************************/
case MAVLINK_MSG_ID_TERRAIN_DATA:
case MAVLINK_MSG_ID_TERRAIN_CHECK:
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
copter.terrain.handle_data(chan, msg);
#endif
break;
/***************************************MAV ID: 243home点信息**********************************************************************/
case MAVLINK_MSG_ID_SET_HOME_POSITION:
{
mavlink_set_home_position_t packet;
mavlink_msg_set_home_position_decode(msg, &packet);
if((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0))
{
copter.set_home_to_current_location(true);
} else {
// sanity check location
if (!check_latlng(packet.latitude, packet.longitude)) {
break;
}
Location new_home_loc;
new_home_loc.lat = packet.latitude;
new_home_loc.lng = packet.longitude;
new_home_loc.alt = packet.altitude / 10;
copter.set_home(new_home_loc, true);
}
break;
}
/***************************************MAV ID: 246，10001，10002，10003**********************************************************************/
case MAVLINK_MSG_ID_ADSB_VEHICLE:
case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG:
case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC:
case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT:
#if ADSB_ENABLED == ENABLED
copter.adsb.handle_message(chan, msg);
#endif
break;
#if TOY_MODE_ENABLED == ENABLED
case MAVLINK_MSG_ID_NAMED_VALUE_INT:
copter.g2.toy_mode.handle_message(msg);
break;
#endif
default:
// hal.uartC->printf("^^^^^^^^^^^^^^\r\n");
// hal.uartC->printf("^^^^123^^\r\n");
handle_common_message(msg); //这个是处理msg命令
// hal.uartC->printf("++++++++++\r\n");
// hal.uartC->printf("+++123+++++\r\n");
break;
} // end switch
} // end handle mavlink
分析函数4：handle_common_message(msg); //这个是处理msg命令
void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
{
// hal.uartC->printf("------------\r\n");
// hal.uartC->printf("msg->msgid=%d\r\n",msg->msgid);
switch (msg->msgid)
{
/***************************************MAV ID: 77，命令**********************************************************************/
case MAVLINK_MSG_ID_COMMAND_ACK:
{
handle_command_ack(msg);
break;
}
/***************************************MAV ID: 256**********************************************************************/
case MAVLINK_MSG_ID_SETUP_SIGNING:
handle_setup_signing(msg);
break;
/***************************************MAV ID: 21,23,20**********************************************************************/
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: //21
case MAVLINK_MSG_ID_PARAM_SET: //23
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: //20
handle_common_param_message(msg);
break;
/***************************************MAV ID: 48全局gps坐标**********************************************************************/
case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN:
handle_set_gps_global_origin(msg);
break;
/***************************************MAV ID: 11000**********************************************************************/
case MAVLINK_MSG_ID_DEVICE_OP_READ:
handle_device_op_read(msg);
break;
/***************************************MAV ID: 11002**********************************************************************/
case MAVLINK_MSG_ID_DEVICE_OP_WRITE:
handle_device_op_write(msg);
break;
/***************************************MAV ID: 111**********************************************************************/
case MAVLINK_MSG_ID_TIMESYNC:
handle_timesync(msg);
break;
/***************************************MAV ID: 117,119,121，122,185**********************************************************************/
case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
case MAVLINK_MSG_ID_LOG_ERASE:
case MAVLINK_MSG_ID_LOG_REQUEST_END:
case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS:
DataFlash_Class::instance()->handle_mavlink_msg(*this, msg); //处理log数据
break;
/***************************************MAV ID: 155**********************************************************************/
case MAVLINK_MSG_ID_DIGICAM_CONTROL:
{
AP_Camera *camera = get_camera();
if (camera == nullptr) {
return;
}
camera->control_msg(msg);
}
break;
/***************************************MAV ID: 11设置模式**********************************************************************/
case MAVLINK_MSG_ID_SET_MODE:
handle_set_mode(msg);
break;
/***************************************MAV ID: 183发送版本**********************************************************************/
case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
handle_send_autopilot_version(msg);
break;
/***************************************MAV ID: 任务信息**********************************************************************/
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: //38------消息任务写入部分列表打包
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: //43------消息任务写入部分列表打包
case MAVLINK_MSG_ID_MISSION_COUNT: //44------总共有多少个航点
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: //45------清除任务
case MAVLINK_MSG_ID_MISSION_ITEM: //39----任务序列
case MAVLINK_MSG_ID_MISSION_ITEM_INT: //73
case MAVLINK_MSG_ID_MISSION_REQUEST_INT: //51-----请求初始化
case MAVLINK_MSG_ID_MISSION_REQUEST: //40--任务请求
case MAVLINK_MSG_ID_MISSION_ACK: //47应答
case MAVLINK_MSG_ID_MISSION_SET_CURRENT: //41设置当前位置
// hal.uartC->printf("&&&&&&&&&&&&&&&&&&\r\n");
handle_common_mission_message(msg); //处理常用信息
break;
/***************************************MAV ID: 126串口数据**********************************************************************/
case MAVLINK_MSG_ID_SERIAL_CONTROL:
handle_serial_control(msg);
break;
/***************************************MAV ID: 233,232，113，123**********************************************************************/
case MAVLINK_MSG_ID_GPS_RTCM_DATA:
case MAVLINK_MSG_ID_GPS_INPUT:
case MAVLINK_MSG_ID_HIL_GPS:
case MAVLINK_MSG_ID_GPS_INJECT_DATA:
AP::gps().handle_msg(msg);
break;
/***************************************MAV ID: 253**********************************************************************/
case MAVLINK_MSG_ID_STATUSTEXT:
handle_statustext(msg);
break;
/***************************************MAV ID: 186**********************************************************************/
case MAVLINK_MSG_ID_LED_CONTROL:
// send message to Notify
AP_Notify::handle_led_control(msg);
break;
/***************************************MAV ID: 258**********************************************************************/
case MAVLINK_MSG_ID_PLAY_TUNE:
// send message to Notify
AP_Notify::handle_play_tune(msg);
break;
/***************************************MAV ID: 175,176**********************************************************************/
case MAVLINK_MSG_ID_RALLY_POINT:
case MAVLINK_MSG_ID_RALLY_FETCH_POINT:
handle_common_rally_message(msg);
break;
/***************************************MAV ID: 66**********************************************************************/
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
handle_request_data_stream(msg);
break;
/***************************************MAV ID: 172**********************************************************************/
case MAVLINK_MSG_ID_DATA96:
handle_data_packet(msg);
break;
/***************************************MAV ID: 11011**********************************************************************/
case MAVLINK_MSG_ID_VISION_POSITION_DELTA:
handle_vision_position_delta(msg);
break;
/***************************************MAV ID: 102**********************************************************************/
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
handle_vision_position_estimate(msg);
break;
/***************************************MAV ID: 101**********************************************************************/
case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
handle_global_vision_position_estimate(msg);
break;
/***************************************MAV ID: 104**********************************************************************/
case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
handle_vicon_position_estimate(msg);
break;
/***************************************MAV ID: 138**********************************************************************/
case MAVLINK_MSG_ID_ATT_POS_MOCAP:
handle_att_pos_mocap(msg);
break;
/***************************************MAV ID: 2**********************************************************************/
case MAVLINK_MSG_ID_SYSTEM_TIME:
handle_system_time_message(msg);
break;
}
}
**分析函数5：handle_common_mission_message(msg); //处理常用信息
void GCS_MAVLINK::handle_common_mission_message(mavlink_message_t *msg)
{
AP_Mission *_mission = get_mission(); //&copter.mission
if (_mission == nullptr)
{
return;
}
switch (msg->msgid)
{
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38
{
handle_mission_write_partial_list(*_mission, msg);
break;
}
// GCS has sent us a mission item, store to EEPROM
case MAVLINK_MSG_ID_MISSION_ITEM: // MAV ID: 39
case MAVLINK_MSG_ID_MISSION_ITEM_INT: //73,处理航点信息
{
// hal.uartC->printf("!!!!!!!!!!!!!!!! \r\n");
if (handle_mission_item(msg, *_mission))
{
DataFlash_Class::instance()->Log_Write_EntireMission(*_mission); //把任务数量写到EEPROM中
}
break;
}
// read an individual command from EEPROM and send it to the GCS
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
case MAVLINK_MSG_ID_MISSION_REQUEST: // MAV ID: 40, 51
{
// hal.uartC->printf("!!!!!!!!!!!!!!!!456!!!!!!!!!!!!!!!!\r\n");
handle_mission_request(*_mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_SET_CURRENT: // MAV ID: 41
{
// hal.uartC->printf("!!!!!!!!!!!!!!!!789!!!!!!!!!!!!!!!!\r\n");
handle_mission_set_current(*_mission, msg);
break;
}
// GCS request the full list of commands, we return just the number and leave the GCS to then request each command individually
//GCS请求命令的完整列表，我们只返回数字，让地面军事系统单独请求每个命令。
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: // MAV ID: 43
{
// hal.uartC->printf("!!!!!!!!!!!!!!!!1111%!!!!!!!!!!!!!!!!r\n");
handle_mission_request_list(*_mission, msg);
break;
}
// GCS provides the full number of commands it wishes to upload
// individual commands will then be sent from the GCS using the MAVLINK_MSG_ID_MISSION_ITEM message
case MAVLINK_MSG_ID_MISSION_COUNT: // MAV ID: 44
{
// hal.uartC->printf("!!!!!!!!!!!!!!!!222!!!!!!! \r\n");
// hal.uartC->printf("msg->seq=%d\r\n",msg->seq);
// hal.uartC->printf("(&((msg)->payload64[0]))=%d\r\n",(&((msg)->payload64[0]))); //(&((msg)->payload64[0]))
// hal.uartC->printf("!!!!!!!456!!!!!!!\r\n");
handle_mission_count(*_mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: // MAV ID: 45
{
// hal.uartC->printf("!!!!!!!3333!!!!!!! \r\n");
handle_mission_clear_all(*_mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_ACK:
/* not used */
break;
}
}
到这来如果通过missionplanner规划航线信息，那么就会调用上面的函数。
1.写任务列表
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38
{
handle_mission_write_partial_list(*_mission, msg);
break;
}
void GCS_MAVLINK::handle_mission_write_partial_list(AP_Mission &mission, mavlink_message_t *msg)
{
// decode---译码
mavlink_mission_write_partial_list_t packet;
mavlink_msg_mission_write_partial_list_decode(msg, &packet);
//开始航点信息接收------ start waypoint receiving
if ((unsigned)packet.start_index > mission.num_commands() ||
(unsigned)packet.end_index > mission.num_commands() ||
packet.end_index < packet.start_index)
{
send_text(MAV_SEVERITY_WARNING,"Flight plan update rejected");
return;
}
waypoint_timelast_receive = AP_HAL::millis();
waypoint_timelast_request = 0;
waypoint_receiving = true;
waypoint_request_i = packet.start_index;
waypoint_request_last= packet.end_index;
waypoint_dest_sysid = msg->sysid; // record system id of GCS who wants to partially update the mission
waypoint_dest_compid = msg->compid; // record component id of GCS who wants to partially update the mission
}
static inline void mavlink_msg_mission_write_partial_list_decode(const mavlink_message_t* msg, mavlink_mission_write_partial_list_t* mission_write_partial_list)
{
#if MAVLINK_NEED_BYTE_SWAP
mission_write_partial_list->start_index = mavlink_msg_mission_write_partial_list_get_start_index(msg);
mission_write_partial_list->end_index = mavlink_msg_mission_write_partial_list_get_end_index(msg);
mission_write_partial_list->target_system = mavlink_msg_mission_write_partial_list_get_target_system(msg);
mission_write_partial_list->target_component = mavlink_msg_mission_write_partial_list_get_target_component(msg);
#else
memcpy(mission_write_partial_list, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN);
#endif
}
2.发送任务项目的航点数量
//GCS系统提供想要上传的全部命令，然后将使用mavlink_msg_id_mission_item消息从地面站系统发送个别命令。
case MAVLINK_MSG_ID_MISSION_COUNT: // MAV ID: 44
{
// hal.uartC->printf("!!!!!!!!!!!!!!!!222!!!!!!! \r\n");
// hal.uartC->printf("msg->seq=%d\r\n",msg->seq);
// hal.uartC->printf("(&((msg)->payload64[0]))=%d\r\n",(&((msg)->payload64[0]))); //(&((msg)->payload64[0]))
// hal.uartC->printf("!!!!!!!456!!!!!!!\r\n");
handle_mission_count(*_mission, msg);
break;
}
void GCS_MAVLINK::handle_mission_count(AP_Mission &mission, mavlink_message_t *msg)
{
// decode
mavlink_mission_count_t packet;
mavlink_msg_mission_count_decode(msg, &packet);
// hal.uartC->printf("packet.count =%d\r\n",packet.count );//参数packet.count表示总共的航点数量
//开始航点接收----start waypoint receiving
if (packet.count > mission.num_commands_max())
{
// send NAK
mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_NO_SPACE,
MAV_MISSION_TYPE_MISSION);
return;
}
//新任务到达，将任务截断为相同长度---- new mission arriving, truncate mission to be the same length
mission.truncate(packet.count);
//设置变量以帮助处理从地面站系统接收到的预期命令---set variables to help handle the expected receiving of commands from the GCS
waypoint_timelast_receive = AP_HAL::millis(); //set time we last received commands to now---将上次接收命令的时间设置为现在
waypoint_receiving = true; // record that we expect to receive commands---我们希望接收命令的记录
waypoint_request_i = 0; // reset the next expected command number to zero---将下一个预期的命令号重置为零
waypoint_request_last = packet.count; // record how many commands we expect to receive---记录我们希望接收多少命令
waypoint_timelast_request = 0; // set time we last requested commands to zero---将上次请求命令的时间设置为零
waypoint_dest_sysid = msg->sysid; // record system id of GCS who wants to upload the mission--记录想要上传任务的地面军事系统的系统ID
waypoint_dest_compid = msg->compid; // record component id of GCS who wants to upload the mission---记录想要上传任务的地面军事系统的组件ID
}
3.发送任务项目存储到EEPROM中
case MAVLINK_MSG_ID_MISSION_ITEM: // MAV ID: 39
case MAVLINK_MSG_ID_MISSION_ITEM_INT: //73,处理航点信息
{
hal.uartC->printf("!!!!!!!!!!!!!!!! \r\n");
if (handle_mission_item(msg, *_mission))
{
DataFlash_Class::instance()->Log_Write_EntireMission(*_mission); //把任务数量写到EEPROM中
}
break;
}
串口显示写入航点信息
*重点需要看的函数是：handle_mission_item(msg, _mission)
bool GCS_MAVLINK::handle_mission_item(mavlink_message_t *msg, AP_Mission &mission)
{
MAV_MISSION_RESULT result = MAV_MISSION_ACCEPTED;
struct AP_Mission::Mission_Command cmd = {};
bool mission_is_complete = false;
uint16_t seq=0;
uint16_t current = 0;
if (msg->msgid == MAVLINK_MSG_ID_MISSION_ITEM) //39
{
// hal.uartC->printf("++++++\r\n");
mavlink_mission_item_t packet;
mavlink_msg_mission_item_decode(msg, &packet);
//将mavlink包转换为任务命令 convert mavlink packet to mission command
result = AP_Mission::mavlink_to_mission_cmd(packet, cmd);
if (result != MAV_MISSION_ACCEPTED)
{
goto mission_ack;
}
seq = packet.seq;
current = packet.current;
} else
{
mavlink_mission_item_int_t packet;
mavlink_msg_mission_item_int_decode(msg, &packet);
// convert mavlink packet to mission command---将mavlink包转换为任务命令
result = AP_Mission::mavlink_int_to_mission_cmd(packet, cmd);
if (result != MAV_MISSION_ACCEPTED)
{
goto mission_ack;
}
seq = packet.seq;
current = packet.current;
}
hal.uartC->printf("current=%d\r\n",current);
if (current == 2)
{
// current = 2 is a flag to tell us this is a "guided mode"
// waypoint and not for the mission
////current=2是一个标志，告诉我们这是一个“引导模式” 航点信息，而不是任务
result = (handle_guided_request(cmd) ? MAV_MISSION_ACCEPTED
: MAV_MISSION_ERROR) ;
// verify we received the command
//确认我们收到命令
goto mission_ack;
}
if (current == 3)
{
//current = 3 is a flag to tell us this is a alt change only
// add home alt if needed
//current=3是一个标志，告诉我们这是一个alt更改，只在需要时添加home alt
handle_change_alt_request(cmd);
//确认我们收到命令--- verify we recevied the command
result = MAV_MISSION_ACCEPTED;
goto mission_ack;
}
hal.uartC->printf("waypoint_receiving=%d\r\n",waypoint_receiving);
// Check if receiving waypoints (mission upload expected)检---查接收航点（预计任务上传）
if (!waypoint_receiving)
{
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// check if this is the requested waypoint--检查这是否是请求的航点
hal.uartC->printf("waypoint_request_i=%d\r\n",waypoint_request_i); //waypoint_request_i=
hal.uartC->printf("seq=%d\r\n",seq);//seq=6
if (seq != waypoint_request_i)
{
result = MAV_MISSION_INVALID_SEQUENCE;
goto mission_ack;
}
// sanity check for DO_JUMP command---do_jump命令的健全性检查
hal.uartC->printf("cmd.id =%d\r\n",cmd.id );//cmd.id =16表示home点和航点信息，cmd.id =22表示的是起飞点信息
if (cmd.id == MAV_CMD_DO_JUMP)
{
if ((cmd.content.jump.target >= mission.num_commands() && cmd.content.jump.target >= waypoint_request_last) || cmd.content.jump.target == 0)
{
result = MAV_MISSION_ERROR;
goto mission_ack;
}
}
//如果命令索引在现有列表中，请替换该命令--- if command index is within the existing list, replace the command
hal.uartC->printf("mission.num_commands() =%d\r\n",mission.num_commands() );
hal.uartC->printf("seq =%d\r\n",seq );
if (seq < mission.num_commands())
{
hal.uartC->printf("mission.replace_cmd(seq,cmd) =%d\r\n",mission.replace_cmd(seq,cmd) );//cmd.id =16表示home点和航点信息，cmd.id =22表示的是起飞点信息
if (mission.replace_cmd(seq,cmd)) //这个是把航点增加进去
{
result = MAV_MISSION_ACCEPTED;
}else
{
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// if command is at the end of command list, add the command
//如果命令位于命令列表的末尾，则添加该命令
} else if (seq == mission.num_commands())
{
hal.uartC->printf("--------\r\n" );
if (mission.add_cmd(cmd)) //增加航点,
{
result = MAV_MISSION_ACCEPTED;
}else
{
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// if beyond the end of the command list, return an error
} else
{
result = MAV_MISSION_ERROR;
goto mission_ack;
}
// update waypoint receiving state machine
waypoint_timelast_receive = AP_HAL::millis();
waypoint_request_i++;
if (waypoint_request_i >= waypoint_request_last)
{
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
MAV_MISSION_ACCEPTED,
MAV_MISSION_TYPE_MISSION);
send_text(MAV_SEVERITY_INFO,"Flight plan received");
waypoint_receiving = false;
mission_is_complete = true;
// XXX ignores waypoint radius for individual waypoints, can
// only set WP_RADIUS parameter
} else
{
waypoint_timelast_request = AP_HAL::millis();
// if we have enough space, then send the next WP immediately
if (HAVE_PAYLOAD_SPACE(chan, MISSION_ITEM))
{
queued_waypoint_send();
} else {
send_message(MSG_NEXT_WAYPOINT);
}
}
gcs().send_text(MAV_SEVERITY_INFO, "Waypoint Num %d ",mission.num_commands() ); //增加航点数量提示
return mission_is_complete;
mission_ack:
// we are rejecting the mission/waypoint---我们拒绝执行任务/航路点
mavlink_msg_mission_ack_send_buf(
msg,
chan,
msg->sysid,
msg->compid,
result,
MAV_MISSION_TYPE_MISSION);
return mission_is_complete;
}
注意下面存储航点信息的地方
bool AP_Mission::replace_cmd(uint16_t index, Mission_Command& cmd)
{
//健康检查索引---- sanity check index
if (index >= (unsigned)_cmd_total)
{
return false;
}
//尝试将命令写入存储器---- attempt to write the command to storage
return write_cmd_to_storage(index, cmd);
}
bool AP_Mission::add_cmd(Mission_Command& cmd)
{
// attempt to write the command to storage
bool ret = write_cmd_to_storage(_cmd_total, cmd);
if (ret)
{
// update command's index
cmd.index = _cmd_total;
// increment total number of commands
_cmd_total.set_and_save(_cmd_total + 1);
}
return ret;
}
到这里可以看到最终航点信息被写到EEPROM中
bool AP_Mission::write_cmd_to_storage(uint16_t index, Mission_Command& cmd)
{
// hal.uartC->printf("num_commands_max()=%ld\r\n",num_commands_max());//最大是724
//命令索引范围检查----range check cmd's index
if (index >= num_commands_max())
{
return false;
}
//计算命令的存储位置------- calculate where in storage the command should be placed
uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
if (cmd.id < 256)
{
_storage.write_byte(pos_in_storage, cmd.id);
_storage.write_uint16(pos_in_storage+1, cmd.p1);
_storage.write_block(pos_in_storage+3, cmd.content.bytes, 12);
} else
{
// if the command ID is above 256 we store a 0 followed by the 16 bit command ID
//如果命令id大于256，则存储0，后跟16位命令id
_storage.write_byte(pos_in_storage, 0);
_storage.write_uint16(pos_in_storage+1, cmd.id);
_storage.write_uint16(pos_in_storage+3, cmd.p1);
_storage.write_block(pos_in_storage+5, cmd.content.bytes, 10);
}
//记得上次任务改变的时候-------remember when the mission last changed
_last_change_time_ms = AP_HAL::millis();
//成功返回1-----------------return success
return true;
}
后面的细节暂时不做分析
4.从EEPROM中读取航点信息
// read an individual command from EEPROM and send it to the GCS
//从EEPROM读取一个单独的命令并发送给地面站系统
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
case MAVLINK_MSG_ID_MISSION_REQUEST: // MAV ID: 40, 51
{
// hal.uartC->printf("!!!!!!!!!!!!!!!!456!!!!!!!!!!!!!!!!\r\n");
handle_mission_request(*_mission, msg); //需要分析的函数
break;
}
2.Missionplanner进行解锁，起飞，模式切换命令
void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) //消息ID
{
case MAVLINK_MSG_ID_COMMAND_LONG: // MAV ID: 76
{
// decode packet
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
// hal.console->printf("^^^^^^^^^^^^^^^^^^\r\n");
// hal.console->printf("packet.param1=%f\r\n",packet.param1); //0
// hal.console->printf("packet.param2=%f\r\n",packet.param2);//0
// hal.console->printf("packet.param3=%f\r\n",packet.param3);//0
// hal.console->printf("packet.param4=%f\r\n",packet.param4);//0
// hal.console->printf("packet.param5=%f\r\n",packet.param5);//22.7
// hal.console->printf("packet.param6=%f\r\n",packet.param6);//114
// hal.console->printf("packet.param7=%f\r\n",packet.param7);//108.6
// hal.console->printf("packet.command=%d\r\n",packet.command);//179
// hal.console->printf("packet.target_system=%f\r\n",packet.target_system);//0
// hal.console->printf("packet.target_component=%d\r\n",packet.target_component);//1
//
//
// hal.console->printf("^^^^^^^^^^^^*****^^^^^\r\n");
//检测喷洒是否开启
if(packet.command==183&&packet.param1==10)
{
if(packet.param2>1500)
spray_on=true;
else
spray_on=false;
}
switch(packet.command)
{
case MAV_CMD_NAV_TAKEOFF:
{
// param3 : horizontal navigation by pilot acceptable
// param4 : yaw angle (not supported)
// param5 : latitude (not supported)
// param6 : longitude (not supported)
// param7 : altitude [metres]
float takeoff_alt = packet.param7 * 100; // Convert m to cm
if (copter.flightmode->do_user_takeoff(takeoff_alt, is_zero(packet.param3))) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
break;
}
case MAV_CMD_NAV_LOITER_UNLIM:
if (copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND))
{
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (copter.set_mode(RTL, MODE_REASON_GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_NAV_LAND:
if (copter.set_mode(LAND, MODE_REASON_GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_FOLLOW:
#if MODE_FOLLOW_ENABLED == ENABLED
// param1: sysid of target to follow
if ((packet.param1 > 0) && (packet.param1 <= 255)) {
copter.g2.follow.set_target_sysid((uint8_t)packet.param1);
result = MAV_RESULT_ACCEPTED;
}
#endif
break;
case MAV_CMD_CONDITION_YAW:
// param1 : target angle [0-360]
// param2 : speed during change [deg per second]
// param3 : direction (-1:ccw, +1:cw)
// param4 : relative offset (1) or absolute angle (0)
if ((packet.param1 >= 0.0f) &&
(packet.param1 <= 360.0f) &&
(is_zero(packet.param4) || is_equal(packet.param4,1.0f))) {
copter.flightmode->auto_yaw.set_fixed_yaw(
packet.param1,
packet.param2,
(int8_t)packet.param3,
is_positive(packet.param4));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
break;
case MAV_CMD_DO_CHANGE_SPEED:
// param1 : unused
// param2 : new speed in m/s
// param3 : unused
// param4 : unused
if (packet.param2 > 0.0f) {
copter.wp_nav->set_speed_xy(packet.param2 * 100.0f);
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
break;
case MAV_CMD_DO_SET_HOME:
// param1 : use current (1=use current location, 0=use specified location)
// param5 : latitude
// param6 : longitude
// param7 : altitude (absolute)
result = MAV_RESULT_FAILED; // assume failure
if (is_equal(packet.param1,1.0f))
{
if (copter.set_home_to_current_location(true))
{
result = MAV_RESULT_ACCEPTED;
}
} else
{
// ensure param1 is zero
if (!is_zero(packet.param1))
{
break;
}
// sanity check location
if (!check_latlng(packet.param5, packet.param6))
{
break;
}
Location new_home_loc;
new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
if (copter.set_home(new_home_loc, true))
{
result = MAV_RESULT_ACCEPTED;
}
}
break;
case MAV_CMD_DO_SET_ROI:
// param1 : regional of interest mode (not supported)
// param2 : mission index/ target id (not supported)
// param3 : ROI index (not supported)
// param5 : x / lat
// param6 : y / lon
// param7 : z / alt
// sanity check location
if (!check_latlng(packet.param5, packet.param6)) {
break;
}
Location roi_loc;
roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
copter.flightmode->auto_yaw.set_roi(roi_loc);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_DO_MOUNT_CONTROL:
#if MOUNT == ENABLED
if(!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_fixed_yaw(
(float)packet.param3 / 100.0f,
0.0f,
0,0);
}
copter.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
result = MAV_RESULT_ACCEPTED;
#endif
break;
#if MODE_AUTO_ENABLED == ENABLED
case MAV_CMD_MISSION_START:
if (copter.motors->armed() && copter.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) {
copter.set_auto_armed(true);
if (copter.mission.state() != AP_Mission::MISSION_RUNNING) {
copter.mission.start_or_resume();
}
result = MAV_RESULT_ACCEPTED;
}
break;
#endif
//这里就是解锁函数
case MAV_CMD_COMPONENT_ARM_DISARM:
if (is_equal(packet.param1,1.0f)) {
// attempt to arm and return success or failure
const bool do_arming_checks = !is_equal(packet.param2,magic_force_arm_value);
if (copter.init_arm_motors(true, do_arming_checks)) {
result = MAV_RESULT_ACCEPTED;
}
} else if (is_zero(packet.param1)) {
if (copter.ap.land_complete || is_equal(packet.param2,magic_force_disarm_value)) {
// force disarming by setting param2 = 21196 is deprecated
copter.init_disarm_motors();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else {
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) {
AP_Notify::flags.firmware_update = 1;
copter.notify.update();
hal.scheduler->delay(200);
// when packet.param1 == 3 we reboot to hold in bootloader
hal.scheduler->reboot(is_equal(packet.param1,3.0f));
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_FENCE_ENABLE:
#if AC_FENCE == ENABLED
result = MAV_RESULT_ACCEPTED;
switch ((uint16_t)packet.param1) {
case 0:
copter.fence.enable(false);
break;
case 1:
copter.fence.enable(true);
break;
default:
result = MAV_RESULT_FAILED;
break;
}
#else
// if fence code is not included return failure
result = MAV_RESULT_FAILED;
#endif
break;
#if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE:
// configure or release parachute
result = MAV_RESULT_ACCEPTED;
switch ((uint16_t)packet.param1)
{
case PARACHUTE_DISABLE:
copter.parachute.enabled(false);
copter.Log_Write_Event(DATA_PARACHUTE_DISABLED);
break;
case PARACHUTE_ENABLE:
copter.parachute.enabled(true);
copter.Log_Write_Event(DATA_PARACHUTE_ENABLED);
break;
case PARACHUTE_RELEASE:
// treat as a manual release which performs some additional check of altitude
copter.parachute_manual_release();
break;
default:
result = MAV_RESULT_FAILED;
break;
}
break;
#endif
case MAV_CMD_DO_MOTOR_TEST:
// hal.uartC->printf("packet.param1=%f\r\n",packet.param1);
// hal.uartC->printf("packet.param2=%f\r\n",packet.param2);
// hal.uartC->printf("packet.param3=%f\r\n",packet.param3);
// hal.uartC->printf("packet.param4=%f\r\n",packet.param4);
// hal.uartC->printf("packet.param5=%f\r\n",packet.param5);
// hal.uartC->printf("packet.param6=%f\r\n",packet.param6);
// hal.uartC->printf("packet.param7=%f\r\n",packet.param7);
// param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
// param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
// param3 : throttle (range depends upon param2)
// param4 : timeout (in seconds)
// param5 : num_motors (in sequence)
// param6 : compass learning (0: disabled, 1: enabled)
result = copter.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3,
packet.param4, (uint8_t)packet.param5);
break;
#if WINCH_ENABLED == ENABLED
case MAV_CMD_DO_WINCH:
// param1 : winch number (ignored)
// param2 : action (0=relax, 1=relative length control, 2=rate control). See WINCH_ACTIONS enum.
if (!copter.g2.winch.enabled()) {
result = MAV_RESULT_FAILED;
} else {
result = MAV_RESULT_ACCEPTED;
switch ((uint8_t)packet.param2) {
case WINCH_RELAXED:
copter.g2.winch.relax();
copter.Log_Write_Event(DATA_WINCH_RELAXED);
break;
case WINCH_RELATIVE_LENGTH_CONTROL: {
copter.g2.winch.release_length(packet.param3, fabsf(packet.param4));
copter.Log_Write_Event(DATA_WINCH_LENGTH_CONTROL);
break;
}
case WINCH_RATE_CONTROL: {
if (fabsf(packet.param4) <= copter.g2.winch.get_rate_max()) {
copter.g2.winch.set_desired_rate(packet.param4);
copter.Log_Write_Event(DATA_WINCH_RATE_CONTROL);
} else {
result = MAV_RESULT_FAILED;
}
break;
}
default:
result = MAV_RESULT_FAILED;
break;
}
}
break;
#endif
case MAV_CMD_AIRFRAME_CONFIGURATION:
{
// Param 1: Select which gear, not used in ArduPilot
// Param 2: 0 = Deploy, 1 = Retract
// For safety, anything other than 1 will deploy
switch ((uint8_t)packet.param2)
{
case 1:
copter.landinggear.set_position(AP_LandingGear::LandingGear_Retract);
result = MAV_RESULT_ACCEPTED;
break;
default:
copter.landinggear.set_position(AP_LandingGear::LandingGear_Deploy);
result = MAV_RESULT_ACCEPTED;
break;
}
break;
}
/* Solo user presses Fly button */
case MAV_CMD_SOLO_BTN_FLY_CLICK:
{
result = MAV_RESULT_ACCEPTED;
if (copter.failsafe.radio) {
break;
}
// set mode to Loiter or fall back to AltHold
if (!copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
}
break;
}
/* Solo user holds down Fly button for a couple of seconds */
case MAV_CMD_SOLO_BTN_FLY_HOLD: {
result = MAV_RESULT_ACCEPTED;
if (copter.failsafe.radio) {
break;
}
if (!copter.motors->armed()) {
// if disarmed, arm motors
copter.init_arm_motors(true);
} else if (copter.ap.land_complete) {
// if armed and landed, takeoff
if (copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
copter.flightmode->do_user_takeoff(packet.param1*100, true);
}
} else {
// if flying, land
copter.set_mode(LAND, MODE_REASON_GCS_COMMAND);
}
break;
}
/* Solo user presses pause button */
case MAV_CMD_SOLO_BTN_PAUSE_CLICK: {
result = MAV_RESULT_ACCEPTED;
if (copter.failsafe.radio) {
break;
}
if (copter.motors->armed()) {
if (copter.ap.land_complete) {
// if landed, disarm motors
copter.init_disarm_motors();
} else {
// assume that shots modes are all done in guided.
// NOTE: this may need to change if we add a non-guided shot mode
bool shot_mode = (!is_zero(packet.param1) && (copter.control_mode == GUIDED || copter.control_mode == GUIDED_NOGPS));
if (!shot_mode) {
#if MODE_BRAKE_ENABLED == ENABLED
if (copter.set_mode(BRAKE, MODE_REASON_GCS_COMMAND)) {
copter.mode_brake.timeout_to_loiter_ms(2500);
} else {
copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
}
#else
copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
#endif
} else {
// SoloLink is expected to handle pause in shots
}
}
}
break;
}
case MAV_CMD_ACCELCAL_VEHICLE_POS:
result = MAV_RESULT_FAILED;
if (copter.ins.get_acal()->gcs_vehicle_position(packet.param1)) {
result = MAV_RESULT_ACCEPTED;
}
break;
default:
result = handle_command_long_message(packet);
break;
}
// send ACK or NAK
mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result);
break;
}
}
}
函数重点分析：第一步：进行解锁命令
//执行解锁命令
case MAV_CMD_COMPONENT_ARM_DISARM:
if (is_equal(packet.param1,1.0f))
{
// attempt to arm and return success or failure
const bool do_arming_checks = !is_equal(packet.param2,magic_force_arm_value);
if (copter.init_arm_motors(true, do_arming_checks)) //进行解锁
{
result = MAV_RESULT_ACCEPTED;
}
} else if (is_zero(packet.param1))
{
if (copter.ap.land_complete || is_equal(packet.param2,magic_force_disarm_value))
{
// force disarming by setting param2 = 21196 is deprecated
copter.init_disarm_motors(); //进行上锁
result = MAV_RESULT_ACCEPTED;
} else
{
result = MAV_RESULT_FAILED;
}
} else
{
result = MAV_RESULT_UNSUPPORTED;
}
break;
函数重点分析：第二步：进行起飞命令
case MAV_CMD_NAV_TAKEOFF: //起飞命令
{
// param3 : horizontal navigation by pilot acceptable---可接受飞行员水平导航
// param4 : yaw angle (not supported)-------偏航角度
// param5 : latitude (not supported)-------纬度
// param6 : longitude (not supported)------经度
// param7 : altitude [metres]----高度信息
float takeoff_alt = packet.param7 * 100; //高度转换成cm----Convert m to cm
if (copter.flightmode->do_user_takeoff(takeoff_alt, is_zero(packet.param3))) //起飞命令
{
result = MAV_RESULT_ACCEPTED;
} else
{
result = MAV_RESULT_FAILED;
}
break;
}
注意函数copter.flightmode->do_user_takeoff(takeoff_alt, is_zero(packet.param3)
bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
{
if (!copter.motors->armed()) //检查电机有没有解锁，解锁了，往下运行，否则退出
{
return false;
}
if (!ap.land_complete) //是否着地了
{
// can't takeoff again!
return false;
}
if (!has_user_takeoff(must_navigate)) //判断该模式是否支持起飞
{
//此模式不支持用户起飞------ this mode doesn't support user takeoff
return false;
}
if (takeoff_alt_cm <= copter.current_loc.alt) //起飞高度不能低于当前高度
{
// can't takeoff downwards...
return false;
}
#if FRAME_CONFIG == HELI_FRAME
// Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning
if (!copter.motors->rotor_runup_complete()) {
return false;
}
#endif
if (!do_user_takeoff_start(takeoff_alt_cm)) //开始起飞
{
return false;
}
copter.set_auto_armed(true); //设置自动解锁了
return true;
}
bool Copter::ModeGuided::do_user_takeoff_start(float final_alt_above_home)
{
guided_mode = Guided_TakeOff;
//初始化wpnav目的地------ initialise wpnav destination
Location_Class target_loc = copter.current_loc;
target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME); //设定目标高度
if (!wp_nav->set_wp_destination(target_loc)) //设定导航目的地
{
// failure to set destination can only be because of missing terrain data
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK
return false;
}
//初始化偏航initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
//当我们起飞的时候清除i---- clear i term when we're taking off
set_throttle_takeoff();
//获取初始化高度为导航---- get initial alt for WP_NAVALT_MIN
copter.auto_takeoff_set_start_alt();
return true;
}
*函数重点分析：第三步：执行航线任务
执行航点模式，需要手动操作或者APP,MP来操作，这里讲解通过APP来实现。
首先设定当前模式为定点模式，然后切换成航线模式。
//开始执行航线
#if MODE_AUTO_ENABLED == ENABLED
case MAV_CMD_MISSION_START:
if (copter.motors->armed() && copter.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) //判断是否解锁，另外模式是否是自动模式
{
copter.set_auto_armed(true); //设定自动锁模式是1
if (copter.mission.state() != AP_Mission::MISSION_RUNNING) //返回3种状态
{
copter.mission.start_or_resume(); //开始执行请求
}
result = MAV_RESULT_ACCEPTED;
}
break;
#endif
重点分析：copter.mission.start_or_resume(); //开始执行请求
void AP_Mission::start_or_resume()
{
if (_restart)
{
start(); //开始
} else
{
resume();//继续
}
}
分析函数1：
void AP_Mission::start()
{
_flags.state = MISSION_RUNNING;
reset(); //将任务重置为第一个命令，重置跳跃跟踪----- reset mission to the first command, resets jump tracking
//前进到第一命令----- advance to the first command
if (!advance_current_nav_cmd()) //优先的当前导航命令
{
//故障集任务完成--- on failure set mission complete
complete();
}
}
分析函数：
void AP_Mission::reset()
{
_flags.nav_cmd_loaded = false;
_flags.do_cmd_loaded = false;
_flags.do_cmd_all_done = false;
_nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
_do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
_prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE;
_prev_nav_cmd_wp_index = AP_MISSION_CMD_INDEX_NONE;
_prev_nav_cmd_id = AP_MISSION_CMD_ID_NONE;
init_jump_tracking();
}
void AP_Mission::init_jump_tracking()
{
for(uint8_t i=0; ibool AP_Mission::advance_current_nav_cmd()
{
Mission_Command cmd;
uint16_t cmd_index;
//如果我们不运行，将结束---- exit immediately if we're not running
if (_flags.state != MISSION_RUNNING)
{
return false;
}
//立即退出，假如当前命令没有完成----exit immediately if current nav command has not completed
if (_flags.nav_cmd_loaded)
{
return false;
}
//停止当前正在运行的do命令---- stop the current running do command
_do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
_flags.do_cmd_loaded = false;
_flags.do_cmd_all_done = false;
//获取起始点------ get starting point for search
cmd_index = _nav_cmd.index;
if (cmd_index == AP_MISSION_CMD_INDEX_NONE)
{
//从任务命令列表开始------ start from beginning of the mission command list
cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
}else
{
//从超过当前导航命令的一个位置开始---- start from one position past the current nav command
cmd_index++;
}
//避免无休止的循环---- avoid endless loops
uint8_t max_loops = 255;
// search until we find next nav command or reach end of command list
//搜索直到找到下一个导航命令或到达命令列表的末尾
while (!_flags.nav_cmd_loaded)
{
//获取下一条命令---- get next command
if (!get_next_cmd(cmd_index, cmd, true)) //从EEPROM中获取命令信息
{
return false;
}
//检查导航或“do”命令---- check if navigation or "do" command
if (is_nav_cmd(cmd))
{
//保存以前的导航命令索引----- save previous nav command index
_prev_nav_cmd_id = _nav_cmd.id;
_prev_nav_cmd_index = _nav_cmd.index;
//如果前一个导航命令索引包含lat、long、alt，请单独保存--- save separate previous nav command index if it contains lat,long,alt
if (!(cmd.content.location.lat == 0 && cmd.content.location.lng == 0))
{
_prev_nav_cmd_wp_index = _nav_cmd.index;
}
//设置当前导航命令并启动----- set current navigation command and start it
_nav_cmd = cmd;
_flags.nav_cmd_loaded = true;
_cmd_start_fn(_nav_cmd);
}else
{
//设置当前do命令并启动它（如果尚未设置）---- set current do command and start it (if not already set)
if (!_flags.do_cmd_loaded)
{
_do_cmd = cmd;
_flags.do_cmd_loaded = true;
_cmd_start_fn(_do_cmd);
} else
{
//防止无休止的do命令循环---- protect against endless loops of do-commands
if (max_loops-- == 0)
{
return false;
}
}
}
//转到下一个命令--- move onto next command
cmd_index = cmd.index+1;
}
// if we have not found a do command then set flag to show there are no do-commands to be run before nav command completes
//如果找不到do命令，则设置标志以显示在nav命令完成之前没有要运行的do命令
if (!_flags.do_cmd_loaded)
{
_flags.do_cmd_all_done = true;
}
// if we got this far we must have successfully advanced the nav command
//如果我们走到这一步，我们一定成功地推进了导航命令
return true;
}
从EEPROM中读取所要执行的航点信息
bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool increment_jump_num_times_if_found)
{
uint16_t cmd_index = start_index;
Mission_Command temp_cmd;
uint16_t jump_index = AP_MISSION_CMD_INDEX_NONE;
//搜索到任务命令列表的末尾---- search until the end of the mission command list
uint8_t max_loops = 64;
while(cmd_index < (unsigned)_cmd_total)
{
// 读取下一条命令信息------load the next command
if (!read_cmd_from_storage(cmd_index, temp_cmd))
{
// this should never happen because of check above but just in case
//这不应该因为上面的检查而发生，只是以防万一
return false;
}
//检查do jump命令----- check for do-jump command
if (temp_cmd.id == MAV_CMD_DO_JUMP)
{
if (max_loops-- == 0)
{
return false;
}
// check for invalid target
if ((temp_cmd.content.jump.target >= (unsigned)_cmd_total) || (temp_cmd.content.jump.target == 0))
{
// To-Do: log an error?
return false;
}
// check for endless loops
if (!increment_jump_num_times_if_found && jump_index == cmd_index)
{
// we have somehow reached this jump command twice and there is no chance it will complete
// To-Do: log an error?
return false;
}
// record this command so we can check for endless loops
if (jump_index == AP_MISSION_CMD_INDEX_NONE)
{
jump_index = cmd_index;
}
// check if jump command is 'repeat forever'
if (temp_cmd.content.jump.num_times == AP_MISSION_JUMP_REPEAT_FOREVER)
{
// continue searching from jump target
cmd_index = temp_cmd.content.jump.target;
}else
{
// get number of times jump command has already been run
int16_t jump_times_run = get_jump_times_run(temp_cmd);
if (jump_times_run < temp_cmd.content.jump.num_times)
{
// update the record of the number of times run
if (increment_jump_num_times_if_found)
{
increment_jump_times_run(temp_cmd);
}
// continue searching from jump target
cmd_index = temp_cmd.content.jump.target;
}else
{
// jump has been run specified number of times so move search to next command in mission
cmd_index++;
}
}
}else
{
// this is a non-jump command so return it
cmd = temp_cmd;
return true;
}
}
// if we got this far we did not find a navigation command
return false;
}
这里重点要关注： _cmd_start_fn(_do_cmd);
我们先看函数：
struct Mission_Command _do_cmd; // current "do" command. It's position in the command list is held in _do_cmd.index
AP_Mission mission创建对象，带参数
这里是主应用程序的调用地方
// main program function pointers
FUNCTOR_TYPEDEF(mission_cmd_fn_t, bool, const Mission_Command&);
FUNCTOR_TYPEDEF(mission_complete_fn_t, void);
AP_Mission mission{ahrs,
FUNCTOR_BIND_MEMBER(&Copter::start_command, bool, const AP_Mission::Mission_Command &), //这个采用模板bool
FUNCTOR_BIND_MEMBER(&Copter::verify_command_callback, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&Copter::exit_mission, void)};
因此这里的回调的应程序函数是Mission_Command 引用的
// command structure
struct Mission_Command
{
uint16_t index; //此命令位于命令列表中-------- this commands position in the command list
uint16_t id; // mavlink command id
uint16_t p1; // general purpose parameter 1
Content content;
// return a human-readable interpretation of the ID stored in this command
const char *type() const;
};
最终调用的函数是
bool start_command(const AP_Mission::Mission_Command& cmd)
{
return mode_auto.start_command(cmd);
}
因此 _cmd_start_fn(_do_cmd);函数会回调下面的函数，所以有_do_cmd会调用start_command（）函数
bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
{
// To-Do: logging when new commands start/end
//待办事项：当新命令开始/结束时记录
if (copter.should_log(MASK_LOG_CMD))
{
copter.DataFlash.Log_Write_Mission_Cmd(copter.mission, cmd); //记录任务
}
// hal.uartC->printf("cmd.id=%f\r\n",cmd.id);
switch(cmd.id)
{
///
///执行导航命令--------navigation commands
///
case MAV_CMD_NAV_TAKEOFF: // 22
do_takeoff(cmd);
break;
case MAV_CMD_NAV_WAYPOINT: // 16 导航到航点------- Navigate to Waypoint
do_nav_wp(cmd);
break;
case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint
do_land(cmd);
break;
case MAV_CMD_NAV_PAYLOAD_PLACE: // 94 place at Waypoint
do_payload_place(cmd);
break;
case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely
do_loiter_unlimited(cmd);
break;
case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times
do_circle(cmd);
break;
case MAV_CMD_NAV_LOITER_TIME: // 19
do_loiter_time(cmd);
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH: //20
do_RTL();
break;
case MAV_CMD_NAV_SPLINE_WAYPOINT: // 82 Navigate to Waypoint using spline
do_spline_wp(cmd);
break;
#if NAV_GUIDED == ENABLED
case MAV_CMD_NAV_GUIDED_ENABLE: // 92 accept navigation commands from external nav computer
do_nav_guided_enable(cmd);
break;
#endif
case MAV_CMD_NAV_DELAY: // 94 Delay the next navigation command
do_nav_delay(cmd);
break;
//
// conditional commands
//
case MAV_CMD_CONDITION_DELAY: // 112
do_wait_delay(cmd);
break;
case MAV_CMD_CONDITION_DISTANCE: // 114
do_within_distance(cmd);
break;
case MAV_CMD_CONDITION_YAW: // 115
do_yaw(cmd);
break;
///
/// do commands
///
case MAV_CMD_DO_CHANGE_SPEED: // 178
do_change_speed(cmd);
break;
case MAV_CMD_DO_SET_HOME: // 179
do_set_home(cmd);
break;
case MAV_CMD_DO_SET_SERVO:
copter.ServoRelayEvents.do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
break;
case MAV_CMD_DO_SET_RELAY:
copter.ServoRelayEvents.do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
break;
case MAV_CMD_DO_REPEAT_SERVO:
copter.ServoRelayEvents.do_repeat_servo(cmd.content.repeat_servo.channel, cmd.content.repeat_servo.pwm,
cmd.content.repeat_servo.repeat_count, cmd.content.repeat_servo.cycle_time * 1000.0f);
break;
case MAV_CMD_DO_REPEAT_RELAY:
copter.ServoRelayEvents.do_repeat_relay(cmd.content.repeat_relay.num, cmd.content.repeat_relay.repeat_count,
cmd.content.repeat_relay.cycle_time * 1000.0f);
break;
case MAV_CMD_DO_SET_ROI: // 201
// point the copter and camera at a region of interest (ROI)
do_roi(cmd);
break;
case MAV_CMD_DO_MOUNT_CONTROL: // 205
// point the camera to a specified angle
do_mount_control(cmd);
break;
case MAV_CMD_DO_FENCE_ENABLE:
#if AC_FENCE == ENABLED
if (cmd.p1 == 0) { //disable
copter.fence.enable(false);
gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled");
} else { //enable fence
copter.fence.enable(true);
gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled");
}
#endif //AC_FENCE == ENABLED
break;
#if CAMERA == ENABLED
case MAV_CMD_DO_CONTROL_VIDEO: // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
break;
case MAV_CMD_DO_DIGICAM_CONFIGURE: // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|
do_digicam_configure(cmd);
break;
case MAV_CMD_DO_DIGICAM_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
do_digicam_control(cmd);
break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
copter.camera.set_trigger_distance(cmd.content.cam_trigg_dist.meters);
break;
#endif
#if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE: // Mission command to configure or release parachute
do_parachute(cmd);
break;
#endif
#if GRIPPER_ENABLED == ENABLED
case MAV_CMD_DO_GRIPPER: // Mission command to control gripper
do_gripper(cmd);
break;
#endif
#if NAV_GUIDED == ENABLED
case MAV_CMD_DO_GUIDED_LIMITS: // 220 accept guided mode limits
do_guided_limits(cmd);
break;
#endif
#if WINCH_ENABLED == ENABLED
case MAV_CMD_DO_WINCH: // Mission command to control winch
do_winch(cmd);
break;
#endif
default:
// do nothing with unrecognized MAVLink messages
break;
}
// always return success
return true;
}
这个函数通过参数
这个函数的实现过程跟void Copter::fast_loop()的调用大致一样
void Copter::fast_loop()
{
// update INS immediately to get current gyro data populated
ins.update();
// run low level rate controllers that only require IMU data
attitude_control->rate_controller_run();
// send outputs to the motors library immediately
motors_output();
// run EKF state estimator (expensive)
// --------------------
read_AHRS();
#if FRAME_CONFIG == HELI_FRAME
update_heli_control_dynamics();
#endif //HELI_FRAME
// Inertial Nav
// --------------------
read_inertia();
// check if ekf has reset target heading or position
check_ekf_reset();
//运行姿态控制器------run the attitude controllers
update_flight_mode();
// update home from EKF if necessary
update_home_from_EKF();
// check if we've landed or crashed
update_land_and_crash_detectors();
#if MOUNT == ENABLED
// camera mount's fast update
camera_mount.update_fast();
#endif
// log sensor health
if (should_log(MASK_LOG_ANY))
{
Log_Sensor_Health();
}
}
这里我们大致研究下：
void Copter::loop()
{
scheduler.loop();
G_Dt = scheduler.get_last_loop_time_s();
}
void AP_Scheduler::loop()
{
// wait for an INS sample
AP::ins().wait_for_sample();
const uint32_t sample_time_us = AP_HAL::micros();
if (_loop_timer_start_us == 0) {
_loop_timer_start_us = sample_time_us;
_last_loop_time_s = get_loop_period_s();
} else {
_last_loop_time_s = (sample_time_us - _loop_timer_start_us) * 1.0e-6;
}
// Execute the fast loop
// ---------------------
if (_fastloop_fn)
{
_fastloop_fn(); //重点看这个函数
}
// tell the scheduler one tick has passed
tick();
// run all the tasks that are due to run. Note that we only
// have to call this once per loop, as the tasks are scheduled
// in multiples of the main loop tick. So if they don't run on
// the first call to the scheduler they won't run on a later
// call until scheduler.tick() is called again
const uint32_t loop_us = get_loop_period_us();
const uint32_t time_available = (sample_time_us + loop_us) - AP_HAL::micros();
run(time_available > loop_us ? 0u : time_available);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// move result of AP_HAL::micros() forward:
hal.scheduler->delay_microseconds(1);
#endif
// check loop time
perf_info.check_loop_time(sample_time_us - _loop_timer_start_us);
_loop_timer_start_us = sample_time_us;
}
_fastloop_fn(); //重点看这个函数
// function that is called before anything in the scheduler table:
scheduler_fastloop_fn_t _fastloop_fn;
FUNCTOR_TYPEDEF(scheduler_fastloop_fn_t, void); //指针函数
AP_Scheduler::AP_Scheduler(scheduler_fastloop_fn_t fastloop_fn) :
_fastloop_fn(fastloop_fn) //通过fastloop_fn传入，建立fastloop_fn函数和_fastloop_fn等价
{
if (_s_instance) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("Too many schedulers");
#endif
return;
}
_s_instance = this;
AP_Param::setup_object_defaults(this, var_info);
// only allow 50 to 2000 Hz
if (_loop_rate_hz < 50) {
_loop_rate_hz.set(50);
} else if (_loop_rate_hz > 2000) {
_loop_rate_hz.set(2000);
}
_last_loop_time_s = 1.0 / _loop_rate_hz;
}
这里大致总结下实现过程：
（1）SCHED_TASK(gcs_check_input, 400, 180), //检测输入
（2） gcs().update();
（3） chan(i).update(); //更新数据
（4）void GCS_MAVLINK::update(uint32_t max_time_us)
（5）packetReceived(status, msg); //数据接收
（6）void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status,mavlink_message_t &msg)
（7）void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,mavlink_message_t &msg)
（8） handleMessage(&msg); //处理
（9） //开始执行航线
#if MODE_AUTO_ENABLED == ENABLED
case MAV_CMD_MISSION_START:
if (copter.motors->armed() && copter.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) //判断是否解锁，另外模式是否是自动模式
{
copter.set_auto_armed(true); //设定自动锁模式是1
if (copter.mission.state() != AP_Mission::MISSION_RUNNING) //返回3种状态
{
copter.mission.start_or_resume(); //开始执行请求
}
result = MAV_RESULT_ACCEPTED;
}
break;
#endif
（10） copter.mission.start_or_resume(); //开始执行请求-----》 start(); //开始
（11）advance_current_nav_cmd()
（12）_cmd_start_fn(_nav_cmd);
（13） bool start_command(const AP_Mission::Mission_Command& cmd)
（14）mode_auto.start_command(cmd);
（15）bool Copter::ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
分析函数2：complete();
void AP_Mission::complete()
{
// flag mission as complete
_flags.state = MISSION_COMPLETE;
// callback to main program's mission complete function
_mission_complete_fn();
}
3.设定运行自动模式代码
具体如何设定模式的过程，可以参考定点，定高代码分析，这里直接跳转到bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)函数
bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
{
//如果我们已经处于所需模式，请立即返回------ return immediately if we are already in the desired mode
if (mode == control_mode)
{
control_mode_reason = reason;
return true;
}
Copter::Mode *new_flightmode = mode_from_mode_num(mode); //获取新的模式
if (new_flightmode == nullptr)
{
gcs().send_text(MAV_SEVERITY_WARNING,"No such mode");
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
return false;
}
bool ignore_checks = !motors->armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter a non-manual throttle mode if the
// rotor runup is not complete
if (!ignore_checks && !new_flightmode->has_manual_throttle() && !motors->rotor_runup_complete()){
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
return false;
}
#endif
if (!new_flightmode->init(ignore_checks))//注意设定的模式初始化函数，通过这个函数就可以调转到不同的模式下
{
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
return false;
}
// perform any cleanup required by previous flight mode
exit_mode(flightmode, new_flightmode);
// update flight mode
flightmode = new_flightmode;
control_mode = mode;
control_mode_reason = reason;
DataFlash.Log_Write_Mode(control_mode, reason);
#if ADSB_ENABLED == ENABLED
adsb.set_is_auto_mode((mode == AUTO) || (mode == RTL) || (mode == GUIDED));
#endif
#if AC_FENCE == ENABLED
// pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
// this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
// but it should be harmless to disable the fence temporarily in these situations as well
fence.manual_recovery_start();
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry.update_control_mode(control_mode);
#endif
#if DEVO_TELEM_ENABLED == ENABLED
devo_telemetry.update_control_mode(control_mode);
#endif
#if CAMERA == ENABLED
camera.set_is_auto_mode(control_mode == AUTO);
#endif
// update notify object
notify_flight_mode();
// return success
return true;
}
############################
if (!new_flightmode->init(ignore_checks))//注意设定的模式初始化函数，通过这个函数就可以调转到不同的模式下
注意调运设定AUTO模式的是在
############################
bool Copter::ModeAuto::init(bool ignore_checks)
{
//判断位置是否定位OK，同时满足任务数量是否大于1；或者已经解锁了
if ((copter.position_ok() && copter.mission.num_commands() > 1) || ignore_checks)
{
_mode = Auto_Loiter; //模式是自动悬停模式
// reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips)
//拒绝切换到自动模式，如果着陆时有电机待命，但第一个命令不是起飞（减少翻转的机会）
if (motors->armed() && ap.land_complete && !copter.mission.starts_with_takeoff_cmd())
{
gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
return false;
}
// stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
//阻止ROI从以前的任务运行中转移
//要做的：当上一个命令不是wp命令时，重置偏航作为自动wp启动的一部分，以消除对这个特殊的roi检查的需要。
if (auto_yaw.mode() == AUTO_YAW_ROI)
{
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
//初始化航点和平方根曲线控制器 initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
//清楚指点限制clear guided limits
copter.mode_guided.limit_clear();
//启动/恢复任务（基于MIS重启参数）---- start/resume the mission (based on MIS_RESTART parameter)
copter.mission.start_or_resume();
return true;
} else
{
return false;
}
}
运行更新模式： update_flight_mode();
void Copter::update_flight_mode()
{
//更新ekf速度限制-用于限制使用光流时的速度----- Update EKF speed limit - used to limit speed when we are using optical flow
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
target_rangefinder_alt_used = false;
//运行模式
flightmode->run(); //这里运行自动模式
}
void Copter::ModeAuto::run()
{
//呼叫正确的自动控制器------ call the correct auto controller
switch (_mode)
{
case Auto_TakeOff: //起飞
takeoff_run();
break;
case Auto_WP: //自动航线，绕圈移动到边
case Auto_CircleMoveToEdge:
wp_run();
break;
case Auto_Land:
land_run(); //自动着陆
break;
case Auto_RTL:
rtl_run(); //自动返航
break;
case Auto_Circle:
circle_run();//绕圈模式
break;
case Auto_Spline: //曲线运转
spline_run();
break;
case Auto_NavGuided:
#if NAV_GUIDED == ENABLED
nav_guided_run(); //指点模式
#endif
break;
case Auto_Loiter: //自动悬停
loiter_run();
break;
case Auto_NavPayloadPlace: //自动导航装货地点
payload_place_run();
break;
}
}
1.自动起飞
takeoff_run();
void Copter::ModeAuto::takeoff_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
//如果未启用自动防护或电机互锁，将油门设置为零并立即退出。
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock())
{
//初始化航点目标----------initialise wpnav targets
wp_nav->shift_wp_origin_to_current_pos();
zero_throttle_and_relax_ac();
//清除积分项，当我们起飞时-----clear i term when we're taking off
set_throttle_takeoff();
return;
}
//处理飞行偏航输入-------process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio)
{
//获取飞行棋目标偏航速度----get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
#if FRAME_CONFIG == HELI_FRAME
// helicopters stay in landed state until rotor speed runup has finished
if (motors->rotor_runup_complete())
{
set_land_complete(false);
} else
{
// initialise wpnav targets
wp_nav->shift_wp_origin_to_current_pos();
}
#else
set_land_complete(false); //设定着落标志
#endif
//设定电机全速运转-----set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
//运行航点控制器-------run waypoint controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());//更新导航算法
//号召垂直速度控制器----- call z-axis position controller (wpnav should have already updated it's alt target)
pos_control->update_z_controller();
//号召姿态控制器--------call attitude controller
copter.auto_takeoff_attitude_run(target_yaw_rate);
}
2.运行航线代码
wp_run();
void Copter::ModeAuto::wp_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
//如果未启用自动防护或电机互锁，将油门设置为零并立即退出。
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock())
{
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
//将航点原点重置到当前位置，因为无人接可能在地面上，所以我们不希望它在起飞时向左或向右倾斜。当然，如果人们只是起飞的话会更好）
zero_throttle_and_relax_ac();
//清楚我们起飞的时间-------- clear i term when we're taking off
set_throttle_takeoff();
return;
}
//处理偏航输入------- process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio) //遥控器没有丢失
{
//获得飞行员所需的偏航角速度---- get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate))
{
auto_yaw.set_mode(AUTO_YAW_HOLD); //没有的话，设置自动保持
}
}
//将电机设置为全范围--- set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
//运行航点控制器---- run waypoint controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
//呼叫Z轴位置控制器（wpnav应该已经更新了它的alt目标）
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD)
{
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
} else
{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true);
}
}
运行航点控制器代码
copter.failsafe_terrain_set_status(wp_nav->update_wpnav())------》wp_nav->update_wpnav();
bool AC_WPNav::update_wpnav()
{
bool ret = true;
//从位置控制器获取时间------ get dt from pos controller
float dt = _pos_control.get_dt();
// allow the accel and speed values to be set without changing
// out of auto mode. This makes it easier to tune auto flight
//允许在不更改的情况下设置加速度和速度值
//退出自动模式。这样可以更容易地调整自动飞行
_pos_control.set_accel_xy(_wp_accel_cmss);
_pos_control.set_accel_z(_wp_accel_z_cmss);
//必要时推进目标----- advance the target if necessary
if (!advance_wp_target_along_track(dt))
{
// To-Do: handle inability to advance along track (probably because of missing terrain data)
//处理无法沿轨道前进的问题（可能是因为缺少地形数据）
ret = false;
}
// freeze feedforwards during known discontinuities
if (_flags.new_wp_destination)
{
_flags.new_wp_destination = false;
_pos_control.freeze_ff_z();
}
_pos_control.update_xy_controller(1.0f);
check_wp_leash_length();
_wp_last_update = AP_HAL::millis();
return ret;
}
这里重点函数是：advance_wp_target_along_track(dt)
设起始点O,目标点D，飞机所在当前点C，OC在OD上的投影为OH。该函数完成沿航迹更新中间目标点。航迹指的是OD。切记，中间目标点都是在OD线段上，当前的坐标点大部分都在目标点附近的，由于航点信息是20ms更新一次，因此从O飞向目标点的过程，可以划分成很多小的目标点的集合OD={D0,D1,D2,D3,D4…D},每个小的目标点的间距就是20ms更新的时间距离
注意上面这个坐标系只是为了分析用的，实际中Ardupilot的坐标系是北东地（北—》X,东—》Y,，Z->地），这里我做了个仿真来分析下面这个函数的算法实现过程。现在在函数bool AC_WPNav::advance_wp_target_along_track(float dt)中添加打印函数：
主要截取起飞点到2号标号航点的输出数据
2号航点到3号航点的数据
3号航点到4号航点的数据
4号航点到5号航点的数据
5号航点到6号航点的数据
bool AC_WPNav::advance_wp_target_along_track(float dt)
{
//车辆沿轨道行驶的距离OH（单位：cm）。从轨道到车辆画一条垂直线来测量。这里表示的是线段OH
float track_covered; // distance (in cm) along the track that the vehicle has traveled. Measured by drawing a perpendicular line from the track to the vehicle.
//距轨道覆盖位置（即线路上最接近车辆的点）和车辆的距离误差（单位：cm）--CH向量
Vector3f track_error; // distance error (in cm) from the track_covered position (i.e. closest point on the line to the vehicle) and the vehicle
//单次运行允许的最大目标距离：OM
float track_desired_max; // the farthest distance (in cm) along the track that the leash will allow WP控制器本次更新飞机能在OD上跑与O的最远距离哦,即OMAX
//WP控制器本次更新飞机能在能在OD上跑的最远距离，注意是飞机本次跑的距离。即HMAX
float track_leash_slack; // additional distance (in cm) along the track from our track_covered position that our leash will allow--
bool reached_leash_limit = false; // true when track has reached leash limit and we need to slow down the target point--当航迹达到系索极限，我们需要减慢目标点时为真。
//获取当前位置------ get current location
Vector3f curr_pos = _inav.get_position();
//计算地形调整------ calculate terrain adjustments
float terr_offset = 0.0f;
if (_terrain_alt && !get_terrain_offset(terr_offset)) //_terrain_alt=true目标点和起始点的高度是高于地形高度，false是高于起始点高度,terr_offset等于海拔高度减去测距仪测量高度，如果有则返回true ;
{
return false;
}
//从线段原点计算三维矢量，这里表示的是OC-------- calculate 3d vector from segment's origin
Vector3f curr_delta = (curr_pos - Vector3f(0,0,terr_offset)) - _origin; //我们求得curr_pos是基于地理坐标系,Z是海拔高度，这里我们应该减去地形高度，得到距离当地地面的高度，注意这个_origin是不断更新的
//计算我们沿着轨道走了多远，这里表示的是线段OH------- calculate how far along the track we are，其中_pos_delta_unit.y表示每个轴在从起点到终点的总轨迹中所占的百分比
track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z;
//计算从起点到终点的路段上最接近车辆的点，这里转换成OH向量----- calculate the point closest to the vehicle on the segment from origin to destination
Vector3f track_covered_pos = _pos_delta_unit * track_covered; //
//计算从车辆到从起点到终点段上最近点的距离矢量，这里表示的是CH向量--- calculate the distance vector from the vehicle to the closest point on the segment from origin to destination
track_error = curr_delta - track_covered_pos; //计算出误差x，y,z
//计算出误差是HC那么长----calculate the horizontal error
_track_error_xy = norm(track_error.x, track_error.y); //_track_error_xy=20
//计算垂直方向的误差-----calculate the vertical error
float track_error_z = fabsf(track_error.z);
//如果我们向上移动，获取向上的速度；如果我们向下移动，获取向下的速度----- get up leash if we are moving up, down leash if we are moving down
float leash_z = track_error.z >= 0 ? _pos_control.get_leash_up_z() : _pos_control.get_leash_down_z(); //leash_z = _leash_up_z = 362.5
// use pythagoras's theorem calculate how far along the track we could move the intermediate target before reaching the end of the leash
// track_desired_max is the distance from the vehicle to our target point along the track. It is the "hypotenuse" which we want to be no longer than our leash (aka _track_leash_length)
// track_error is the line from the vehicle to the closest point on the track. It is the "opposite" side
// track_leash_slack is the line from the closest point on the track to the target point. It is the "adjacent" side. We adjust this so the track_desired_max is no longer than the leash
//利用毕达哥拉斯定理（勾股定理）计算出在到达牵索末端之前，我们可以沿着轨道移动中间目标的距离。
//按照当前给定的条件，理论上，无人机在一个循环时间可以跑的距离track_leash_length_abs=1300，这里可以用CM1来表示
float track_leash_length_abs = fabsf(_track_leash_length); //CM1
//计算轨迹误差的最大绝对值-----track_error_max_abs=_track_error_xy=20
float track_error_max_abs = MAX(_track_leash_length*track_error_z/leash_z, _track_leash_length*_track_error_xy/_pos_control.get_leash_xy());
//计算出HM1的长度
track_leash_slack = (track_leash_length_abs > track_error_max_abs) ? safe_sqrt(sq(_track_leash_length) - sq(track_error_max_abs)) : 0;
//这个值就是OM1，表示的是目标长度，这里假设C当前的位置是虚拟的，因此一般一个周期track_desired_max的值是1300左右
track_desired_max = track_covered + track_leash_slack;
//检查目标是否已经超出控制范围----- check if target is already beyond the leash
if (_track_desired > track_desired_max)
{
reached_leash_limit = true;
}
//获取当前速度------ get current velocity
const Vector3f &curr_vel = _inav.get_velocity();
//获取轨道上的速度----get speed along track
float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z; //假设curr_vel.x=300，curr_vel.y=400
//计算速度从线性转换到sqrt的点----- calculate point at which velocity switches from linear to sqrt
float linear_velocity = _wp_speed_cms; //设置的航点速度linear_velocity=500
float kP = _pos_control.get_pos_xy_p().kP();
if (is_positive(kP)) // avoid divide by zero
{
linear_velocity = _track_accel/kP; //linear_velocity=100
}
//让限制速度在轨道上的某个范围内高于或低于当前速度----- let the limited_speed_xy_cms be some range above or below current velocity along track
if (speed_along_track < -linear_velocity)
{
// we are traveling fast in the opposite direction of travel to the waypoint so do not move the intermediate point
//我们正以相反的行驶方向快速行驶到停车点，因此不要移动中间点
_limited_speed_xy_cms = 0;
}else
{
// increase intermediate target point's velocity if not yet at the leash limit
//如果还没有达到牵引力限制，则增加中间目标点的速度
if(dt > 0 && !reached_leash_limit)
{
_limited_speed_xy_cms += 2.0f * _track_accel * dt; //v=v0+2at
}
// do not allow speed to be below zero or over top speed
//不允许速度低于零或超过最高速度
_limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms, 0.0f, _track_speed);
//检查我们是否应该开始减速----- check if we should begin slowing down
if (!_flags.fast_waypoint) //如果我们忽略航路点半径，并考虑中间目标到达航点后，航路点完成，则为真。
{
float dist_to_dest = _track_length - _track_desired; //每次的位移与期望到达的位置的误差
if (!_flags.slowing_down && dist_to_dest <= _slow_down_dist) //_slow_down_dist:一旦车辆在离目的地的这段距离内，就应该开始减速,这个值是全速度运行一周期距离的一半:s/2
{
_flags.slowing_down = true;
}
//如果目标减速，限制速度----- if target is slowing down, limit the speed
if (_flags.slowing_down)
{
_limited_speed_xy_cms = MIN(_limited_speed_xy_cms, get_slow_down_speed(dist_to_dest, _track_accel)); //开始进行减速
}
}
// if our current velocity is within the linear velocity range limit the intermediate point's velocity to be no more than the linear_velocity above or below our current velocity
//如果我们的流速在线性速度范围内，则中间点的流速不得超过或低于当前流速的线性速度。
if (fabsf(speed_along_track) < linear_velocity)
{
_limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms,speed_along_track-linear_velocity,speed_along_track+linear_velocity); //被限制在400-600之间的速度
}
}
//推进当前目标------ advance the current target
if (!reached_leash_limit) //当航迹达到系索极限，我们需要减慢目标点时为真。
{
_track_desired += _limited_speed_xy_cms * dt; //目标速度=当前速度+v*t
//如果我们到达牵索末端，减速------ reduce speed if we reach end of leash
if (_track_desired > track_desired_max)
{
_track_desired = track_desired_max; //最大速度
_limited_speed_xy_cms -= 2.0f * _track_accel * dt; //限制速度开始减速，进行两倍速度减速
if (_limited_speed_xy_cms < 0.0f)
{
_limited_speed_xy_cms = 0.0f;
}
}
}
// do not let desired point go past the end of the track unless it's a fast waypoint
//除非是一个快速的航路点，否则不要让所需的点越过跑道的尽头。
if (!_flags.fast_waypoint)
{
_track_desired = constrain_float(_track_desired, 0, _track_length); //_track_length表示每个周期运行的期望位置长度这个是一个固定值1300
} else
{
_track_desired = constrain_float(_track_desired, 0, _track_length + WPNAV_WP_FAST_OVERSHOOT_MAX);//快速航点允许2米超调，以便顺利过渡到下一个航路点。
}
//重新计算所需位置------ recalculate the desired position
Vector3f final_target = _origin + _pos_delta_unit * _track_desired;
//将最终目标Z转换为EKF原点以上的高度------ convert final_target.z to altitude above the ekf origin
final_target.z += terr_offset;
_pos_control.set_pos_target(final_target); //设定每次循环的目标点
//检查我们是否到达了航路点------- check if we've reached the waypoint
if( !_flags.reached_destination )
{
if( _track_desired >= _track_length ) //_track_length这个是我们的总体航线
{
// "fast" waypoints are complete once the intermediate point reaches the destination
if (_flags.fast_waypoint)
{
_flags.reached_destination = true; //说明到达航点
}else
{
//常规航路点也要求直升机在航路点半径范围内----- regular waypoints also require the copter to be within the waypoint radius
Vector3f dist_to_dest = (curr_pos - Vector3f(0,0,terr_offset)) - _destination;
if( dist_to_dest.length() <= _wp_radius_cm ) //只要在要求的范围内，都说明到达了航点
{
_flags.reached_destination = true; //如果当前点到目标点的距离，在半径内，说明到达目标点
}
}
}
}
//如果起点和终点水平距离至少为2米，则更新目标偏航。----- update the target yaw if origin and destination are at least 2m apart horizontally
if (_track_length_xy >= WPNAV_YAW_DIST_MIN)//将导致目标偏航更新到下一个航路点的最小航迹长度。在该距离下，偏航目标将在当前航向冻结。
{
if (_pos_control.get_leash_xy() < WPNAV_YAW_DIST_MIN)
{
// if the leash is short (i.e. moving slowly) and destination is at least 2m horizontally, point along the segment from origin to destination
//如果牵索较短（即缓慢移动），且目的地水平方向至少为2米，则沿着从起点到终点的段指向。
set_yaw_cd(get_bearing_cd(_origin, _destination));
} else
{
Vector3f horiz_leash_xy = final_target - curr_pos; //最终目标需要的水平位置信息
horiz_leash_xy.z = 0;
if (horiz_leash_xy.length() > MIN(WPNAV_YAW_DIST_MIN, _pos_control.get_leash_xy()*WPNAV_YAW_LEASH_PCT_MIN))
{
set_yaw_cd(RadiansToCentiDegrees(atan2f(horiz_leash_xy.y,horiz_leash_xy.x))); //RadiansToCentiDegrees:表示弧度到度
}
}
}
//成功沿轨道前进------- successfully advanced along track
return true;
}