本节主要记录自己学习ardupilot 航线规划代码的过程。 整体思路：无人机操作者通过手机APP或者Missionplanner软件，规划任务航线，航线规划完成后，借助无线通信模块（蓝牙/数传），把规划好的航线发送到无人机的飞控蓝牙或者数传接收端，飞控对蓝牙或者数传接收到的数据进行解析，把收到的航线任务存放到EEPROM中，为将要执行的航线任务做好了航点准备。此时采用手机APP或者Missionplanner发送解锁，起飞命令，然后切换AUTO模式，无人机开始执行auto模式，在auto模式下，无人机会不停的从EEPROM中读取目标任务点与实际的任务点进行对比，然后通过位置PID控制，姿态PID控制，最终输出需要的控制量的PWM到无人机的电调，进而实现控制电机的旋转大小，来实现无人机位置的调节，以实现航线任务规划。
函数分析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规划航线信息，那么就会调用上面的函数。
串口显示写入航点信息
*重点需要看的函数是：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; }后面的细节暂时不做分析
函数重点分析：第一步：进行解锁命令
//执行解锁命令 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.indexAP_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(); }
具体如何设定模式的过程，可以参考定点，定高代码分析，这里直接跳转到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; } }运行航点控制器代码
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更新的时间距离
https://blog.csdn.net/lixiaoweimashixiao/article/details/100536688
