mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-27 02:04:00 -04:00
Copter: handle command_long in GCS base class
This commit is contained in:
Peter Barker
Peter Barker
parent
4775a67ea0
commit
7fc580921a
@ -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
|
||||
|
||||
@ -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:
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user