目錄
文章目錄
- 目錄
- 摘要
- 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; i<num_gcs(); i++)
{
if (chan(i).initialised)
{
chan(i).update(); //更新資料， virtual const GCS_MAVLINK &chan(const uint8_t ofs) const = 0;
}
}
}
void 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; i<nbytes; i++)
{
const uint8_t c = (uint8_t)_port->read();
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; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++)
{
_jump_tracking[i].index = AP_MISSION_CMD_INDEX_NONE;
_jump_tracking[i].num_times_run = 0;
}
}
bool 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;
}