zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: handle command_long in GCS base class

mission-4.1.18
Peter Barker 7 years ago committed by Peter Barker
parent
4775a67ea0
commit
7fc580921a
2 changed files with 328 additions and 393 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 720
      ArduCopter/GCS_Mavlink.cpp
  2. 1
      ArduCopter/GCS_Mavlink.h

720
ArduCopter/GCS_Mavlink.cpp
Unescape View File

@ -684,10 +684,331 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i @@ -684,10 +684,331 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
}
}
void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_long_t &packet)
{
MAV_RESULT result = MAV_RESULT_FAILED; // assume failure. Each messages id is responsible for return ACK or NAK if required
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)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: // MAV ID: 0
@ -788,368 +1109,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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

1
ArduCopter/GCS_Mavlink.h
Unescape View File

@ -36,6 +36,7 @@ protected: @@ -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:

Write Preview
Loading…
Cancel
Save