diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 22c9a4f1fe..05c9820348 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -684,10 +684,331 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i } } + +MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_long_t &packet) +{ + 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))) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + } + + case MAV_CMD_NAV_LOITER_UNLIM: + if (!copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + if (!copter.set_mode(RTL, MODE_REASON_GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_NAV_LAND: + if (!copter.set_mode(LAND, MODE_REASON_GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + +#if MODE_FOLLOW_ENABLED == ENABLED + case MAV_CMD_DO_FOLLOW: + // param1: sysid of target to follow + if ((packet.param1 > 0) && (packet.param1 <= 255)) { + copter.g2.follow.set_target_sysid((uint8_t)packet.param1); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; +#endif + + 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)); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + + 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); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + + case MAV_CMD_DO_SET_HOME: + // param1 : use current (1=use current location, 0=use specified location) + // param5 : latitude + // param6 : longitude + // param7 : altitude (absolute) + if (is_equal(packet.param1,1.0f)) { + if (copter.set_home_to_current_location(true)) { + return MAV_RESULT_ACCEPTED; + } + } else { + // ensure param1 is zero + if (!is_zero(packet.param1)) { + return MAV_RESULT_FAILED; + } + // sanity check location + if (!check_latlng(packet.param5, packet.param6)) { + return MAV_RESULT_FAILED; + } + 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)) { + return MAV_RESULT_ACCEPTED; + } + } + return MAV_RESULT_FAILED; + + 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)) { + return MAV_RESULT_FAILED; + } + 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); + return MAV_RESULT_ACCEPTED; + +#if MOUNT == ENABLED + case MAV_CMD_DO_MOUNT_CONTROL: + 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); + return MAV_RESULT_ACCEPTED; +#endif + +#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(); + } + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; +#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(AP_Arming::ArmingMethod::MAVLINK, do_arming_checks)) { + return 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(); + return MAV_RESULT_ACCEPTED; + } else { + return MAV_RESULT_FAILED; + } + } else { + return MAV_RESULT_UNSUPPORTED; + } + return MAV_RESULT_FAILED; + +#if AC_FENCE == ENABLED + case MAV_CMD_DO_FENCE_ENABLE: + switch ((uint16_t)packet.param1) { + case 0: + copter.fence.enable(false); + return MAV_RESULT_ACCEPTED; + case 1: + copter.fence.enable(true); + return MAV_RESULT_ACCEPTED; + default: + return MAV_RESULT_FAILED; + } +#endif + +#if PARACHUTE == ENABLED + case MAV_CMD_DO_PARACHUTE: + // configure or release parachute + switch ((uint16_t)packet.param1) { + case PARACHUTE_DISABLE: + copter.parachute.enabled(false); + copter.Log_Write_Event(DATA_PARACHUTE_DISABLED); + return MAV_RESULT_ACCEPTED; + case PARACHUTE_ENABLE: + copter.parachute.enabled(true); + copter.Log_Write_Event(DATA_PARACHUTE_ENABLED); + return MAV_RESULT_ACCEPTED; + case PARACHUTE_RELEASE: + // treat as a manual release which performs some additional check of altitude + copter.parachute_manual_release(); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; +#endif + + case MAV_CMD_DO_MOTOR_TEST: + // 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) + return copter.mavlink_motor_test_start(chan, + (uint8_t)packet.param1, + (uint8_t)packet.param2, + (uint16_t)packet.param3, + packet.param4, + (uint8_t)packet.param5); + +#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()) { + return MAV_RESULT_FAILED; + } + switch ((uint8_t)packet.param2) { + case WINCH_RELAXED: + copter.g2.winch.relax(); + copter.Log_Write_Event(DATA_WINCH_RELAXED); + return MAV_RESULT_ACCEPTED; + case WINCH_RELATIVE_LENGTH_CONTROL: { + copter.g2.winch.release_length(packet.param3, fabsf(packet.param4)); + copter.Log_Write_Event(DATA_WINCH_LENGTH_CONTROL); + return MAV_RESULT_ACCEPTED; + } + 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); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + default: + break; + } + return MAV_RESULT_FAILED; +#endif + + /* Solo user presses Fly button */ + case MAV_CMD_SOLO_BTN_FLY_CLICK: { + if (copter.failsafe.radio) { + return MAV_RESULT_ACCEPTED; + } + + // 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); + } + return MAV_RESULT_ACCEPTED; + } + + /* Solo user holds down Fly button for a couple of seconds */ + case MAV_CMD_SOLO_BTN_FLY_HOLD: { + if (copter.failsafe.radio) { + return MAV_RESULT_ACCEPTED; + } + + if (!copter.motors->armed()) { + // if disarmed, arm motors + copter.init_arm_motors(AP_Arming::ArmingMethod::MAVLINK); + } 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); + } + return MAV_RESULT_ACCEPTED; + } + + /* Solo user presses pause button */ + case MAV_CMD_SOLO_BTN_PAUSE_CLICK: { + if (copter.failsafe.radio) { + return MAV_RESULT_ACCEPTED; + } + + 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 + } + } + } + return MAV_RESULT_ACCEPTED; + } + + case MAV_CMD_ACCELCAL_VEHICLE_POS: + if (!copter.ins.get_acal()->gcs_vehicle_position(packet.param1)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + + default: + return GCS_MAVLINK::handle_command_long_packet(packet); + } +} + + 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) { case MAVLINK_MSG_ID_HEARTBEAT: // MAV ID: 0 @@ -788,368 +1109,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) break; } - // 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); - - 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(AP_Arming::ArmingMethod::MAVLINK, 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_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: - // 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 - - /* 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(AP_Arming::ArmingMethod::MAVLINK); - } 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; - } - #if MODE_GUIDED_ENABLED == ENABLED case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82 { @@ -1269,22 +1228,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) // 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; - } + copter.mode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); } 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; + copter.mode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); } break; @@ -1327,7 +1275,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) if(!pos_ignore) { // sanity check location if (!check_latlng(packet.lat_int, packet.lon_int)) { - result = MAV_RESULT_FAILED; break; } Location loc; @@ -1371,22 +1318,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) } 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; - } + 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); } 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; + copter.mode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); } break; @@ -1395,7 +1331,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) 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); @@ -1459,7 +1394,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) #if PRECISION_LANDING == ENABLED case MAVLINK_MSG_ID_LANDING_TARGET: - result = MAV_RESULT_ACCEPTED; copter.precland.handle_msg(msg); break; #endif diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 1a203ac9d5..2dc4703bf5 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -36,6 +36,7 @@ protected: void send_position_target_global_int() override; MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; + MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; private: