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
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 406
      ArduCopter/GCS_Mavlink.cpp
  2. 1
      ArduCopter/GCS_Mavlink.h

406
ArduCopter/GCS_Mavlink.cpp
Unescape View File

@ -684,117 +684,9 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
} }
} }
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
{
// 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;
}
case MAVLINK_MSG_ID_PARAM_VALUE:
{
#if MOUNT == ENABLED
copter.camera_mount.handle_param_value(msg);
#endif
break;
}
case MAVLINK_MSG_ID_GIMBAL_REPORT:
{
#if MOUNT == ENABLED
handle_gimbal_report(copter.camera_mount, msg);
#endif
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: // MAV ID: 70
{
// 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 MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_long_t &packet)
copter.failsafe.last_heartbeat_ms = tnow;
break;
}
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;
}
// 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) { switch(packet.command) {
case MAV_CMD_NAV_TAKEOFF: { case MAV_CMD_NAV_TAKEOFF: {
@ -806,42 +698,39 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
float takeoff_alt = packet.param7 * 100; // Convert m to cm float takeoff_alt = packet.param7 * 100; // Convert m to cm
if (copter.flightmode->do_user_takeoff(takeoff_alt, is_zero(packet.param3))) { if (!copter.flightmode->do_user_takeoff(takeoff_alt, is_zero(packet.param3))) {
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_FAILED;
} else {
result = MAV_RESULT_FAILED;
} }
break; return MAV_RESULT_ACCEPTED;
} }
case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_UNLIM:
if (copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) { if (!copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_FAILED;
} }
break; return MAV_RESULT_ACCEPTED;
case MAV_CMD_NAV_RETURN_TO_LAUNCH: case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (copter.set_mode(RTL, MODE_REASON_GCS_COMMAND)) { if (!copter.set_mode(RTL, MODE_REASON_GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_FAILED;
} }
break; return MAV_RESULT_ACCEPTED;
case MAV_CMD_NAV_LAND: case MAV_CMD_NAV_LAND:
if (copter.set_mode(LAND, MODE_REASON_GCS_COMMAND)) { if (!copter.set_mode(LAND, MODE_REASON_GCS_COMMAND)) {
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_FAILED;
} }
break; return MAV_RESULT_ACCEPTED;
case MAV_CMD_DO_FOLLOW:
#if MODE_FOLLOW_ENABLED == ENABLED #if MODE_FOLLOW_ENABLED == ENABLED
case MAV_CMD_DO_FOLLOW:
// param1: sysid of target to follow // param1: sysid of target to follow
if ((packet.param1 > 0) && (packet.param1 <= 255)) { if ((packet.param1 > 0) && (packet.param1 <= 255)) {
copter.g2.follow.set_target_sysid((uint8_t)packet.param1); copter.g2.follow.set_target_sysid((uint8_t)packet.param1);
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }
return MAV_RESULT_FAILED;
#endif #endif
break;
case MAV_CMD_CONDITION_YAW: case MAV_CMD_CONDITION_YAW:
// param1 : target angle [0-360] // param1 : target angle [0-360]
@ -856,11 +745,9 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
packet.param2, packet.param2,
(int8_t)packet.param3, (int8_t)packet.param3,
is_positive(packet.param4)); is_positive(packet.param4));
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
} }
break; return MAV_RESULT_FAILED;
case MAV_CMD_DO_CHANGE_SPEED: case MAV_CMD_DO_CHANGE_SPEED:
// param1 : unused // param1 : unused
@ -869,40 +756,37 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
// param4 : unused // param4 : unused
if (packet.param2 > 0.0f) { if (packet.param2 > 0.0f) {
copter.wp_nav->set_speed_xy(packet.param2 * 100.0f); copter.wp_nav->set_speed_xy(packet.param2 * 100.0f);
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
} }
break; return MAV_RESULT_FAILED;
case MAV_CMD_DO_SET_HOME: case MAV_CMD_DO_SET_HOME:
// param1 : use current (1=use current location, 0=use specified location) // param1 : use current (1=use current location, 0=use specified location)
// param5 : latitude // param5 : latitude
// param6 : longitude // param6 : longitude
// param7 : altitude (absolute) // param7 : altitude (absolute)
result = MAV_RESULT_FAILED; // assume failure
if (is_equal(packet.param1,1.0f)) { if (is_equal(packet.param1,1.0f)) {
if (copter.set_home_to_current_location(true)) { if (copter.set_home_to_current_location(true)) {
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }
} else { } else {
// ensure param1 is zero // ensure param1 is zero
if (!is_zero(packet.param1)) { if (!is_zero(packet.param1)) {
break; return MAV_RESULT_FAILED;
} }
// sanity check location // sanity check location
if (!check_latlng(packet.param5, packet.param6)) { if (!check_latlng(packet.param5, packet.param6)) {
break; return MAV_RESULT_FAILED;
} }
Location new_home_loc; Location new_home_loc;
new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f); new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
if (copter.set_home(new_home_loc, true)) { if (copter.set_home(new_home_loc, true)) {
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }
} }
break; return MAV_RESULT_FAILED;
case MAV_CMD_DO_SET_ROI: case MAV_CMD_DO_SET_ROI:
// param1 : regional of interest mode (not supported) // param1 : regional of interest mode (not supported)
@ -913,18 +797,17 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
// param7 : z / alt // param7 : z / alt
// sanity check location // sanity check location
if (!check_latlng(packet.param5, packet.param6)) { if (!check_latlng(packet.param5, packet.param6)) {
break; return MAV_RESULT_FAILED;
} }
Location roi_loc; Location roi_loc;
roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
roi_loc.alt = (int32_t)(packet.param7 * 100.0f); roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
copter.flightmode->auto_yaw.set_roi(roi_loc); copter.flightmode->auto_yaw.set_roi(roi_loc);
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_DO_MOUNT_CONTROL:
#if MOUNT == ENABLED #if MOUNT == ENABLED
case MAV_CMD_DO_MOUNT_CONTROL:
if(!copter.camera_mount.has_pan_control()) { if(!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_fixed_yaw( copter.flightmode->auto_yaw.set_fixed_yaw(
(float)packet.param3 / 100.0f, (float)packet.param3 / 100.0f,
@ -932,9 +815,8 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
0,0); 0,0);
} }
copter.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); copter.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
#endif #endif
break;
#if MODE_AUTO_ENABLED == ENABLED #if MODE_AUTO_ENABLED == ENABLED
case MAV_CMD_MISSION_START: case MAV_CMD_MISSION_START:
@ -943,9 +825,9 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
if (copter.mission.state() != AP_Mission::MISSION_RUNNING) { if (copter.mission.state() != AP_Mission::MISSION_RUNNING) {
copter.mission.start_or_resume(); copter.mission.start_or_resume();
} }
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }
break; return MAV_RESULT_FAILED;
#endif #endif
case MAV_CMD_COMPONENT_ARM_DISARM: case MAV_CMD_COMPONENT_ARM_DISARM:
@ -953,63 +835,53 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
// attempt to arm and return success or failure // attempt to arm and return success or failure
const bool do_arming_checks = !is_equal(packet.param2,magic_force_arm_value); 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)) { if (copter.init_arm_motors(AP_Arming::ArmingMethod::MAVLINK, do_arming_checks)) {
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }
} else if (is_zero(packet.param1)) { } else if (is_zero(packet.param1)) {
if (copter.ap.land_complete || is_equal(packet.param2,magic_force_disarm_value)) { if (copter.ap.land_complete || is_equal(packet.param2,magic_force_disarm_value)) {
// force disarming by setting param2 = 21196 is deprecated // force disarming by setting param2 = 21196 is deprecated
copter.init_disarm_motors(); copter.init_disarm_motors();
result = MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} else { } else {
result = MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
} else { } else {
result = MAV_RESULT_UNSUPPORTED; return MAV_RESULT_UNSUPPORTED;
} }
break; return MAV_RESULT_FAILED;
case MAV_CMD_DO_FENCE_ENABLE:
#if AC_FENCE == ENABLED #if AC_FENCE == ENABLED
result = MAV_RESULT_ACCEPTED; case MAV_CMD_DO_FENCE_ENABLE:
switch ((uint16_t)packet.param1) { switch ((uint16_t)packet.param1) {
case 0: case 0:
copter.fence.enable(false); copter.fence.enable(false);
break; return MAV_RESULT_ACCEPTED;
case 1: case 1:
copter.fence.enable(true); copter.fence.enable(true);
break; return MAV_RESULT_ACCEPTED;
default: default:
result = MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
break;
} }
#else
// if fence code is not included return failure
result = MAV_RESULT_FAILED;
#endif #endif
break;
#if PARACHUTE == ENABLED #if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE: case MAV_CMD_DO_PARACHUTE:
// configure or release parachute // configure or release parachute
result = MAV_RESULT_ACCEPTED;
switch ((uint16_t)packet.param1) { switch ((uint16_t)packet.param1) {
case PARACHUTE_DISABLE: case PARACHUTE_DISABLE:
copter.parachute.enabled(false); copter.parachute.enabled(false);
copter.Log_Write_Event(DATA_PARACHUTE_DISABLED); copter.Log_Write_Event(DATA_PARACHUTE_DISABLED);
break; return MAV_RESULT_ACCEPTED;
case PARACHUTE_ENABLE: case PARACHUTE_ENABLE:
copter.parachute.enabled(true); copter.parachute.enabled(true);
copter.Log_Write_Event(DATA_PARACHUTE_ENABLED); copter.Log_Write_Event(DATA_PARACHUTE_ENABLED);
break; return MAV_RESULT_ACCEPTED;
case PARACHUTE_RELEASE: case PARACHUTE_RELEASE:
// treat as a manual release which performs some additional check of altitude // treat as a manual release which performs some additional check of altitude
copter.parachute_manual_release(); copter.parachute_manual_release();
break; return MAV_RESULT_ACCEPTED;
default:
result = MAV_RESULT_FAILED;
break;
} }
break; return MAV_RESULT_FAILED;
#endif #endif
case MAV_CMD_DO_MOTOR_TEST: case MAV_CMD_DO_MOTOR_TEST:
@ -1019,66 +891,60 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
// param4 : timeout (in seconds) // param4 : timeout (in seconds)
// param5 : num_motors (in sequence) // param5 : num_motors (in sequence)
// param6 : compass learning (0: disabled, 1: enabled) // 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, return copter.mavlink_motor_test_start(chan,
packet.param4, (uint8_t)packet.param5); (uint8_t)packet.param1,
break; (uint8_t)packet.param2,
(uint16_t)packet.param3,
packet.param4,
(uint8_t)packet.param5);
#if WINCH_ENABLED == ENABLED #if WINCH_ENABLED == ENABLED
case MAV_CMD_DO_WINCH: case MAV_CMD_DO_WINCH:
// param1 : winch number (ignored) // param1 : winch number (ignored)
// param2 : action (0=relax, 1=relative length control, 2=rate control). See WINCH_ACTIONS enum. // param2 : action (0=relax, 1=relative length control, 2=rate control). See WINCH_ACTIONS enum.
if (!copter.g2.winch.enabled()) { if (!copter.g2.winch.enabled()) {
result = MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} else { }
result = MAV_RESULT_ACCEPTED;
switch ((uint8_t)packet.param2) { switch ((uint8_t)packet.param2) {
case WINCH_RELAXED: case WINCH_RELAXED:
copter.g2.winch.relax(); copter.g2.winch.relax();
copter.Log_Write_Event(DATA_WINCH_RELAXED); copter.Log_Write_Event(DATA_WINCH_RELAXED);
break; return MAV_RESULT_ACCEPTED;
case WINCH_RELATIVE_LENGTH_CONTROL: { case WINCH_RELATIVE_LENGTH_CONTROL: {
copter.g2.winch.release_length(packet.param3, fabsf(packet.param4)); copter.g2.winch.release_length(packet.param3, fabsf(packet.param4));
copter.Log_Write_Event(DATA_WINCH_LENGTH_CONTROL); copter.Log_Write_Event(DATA_WINCH_LENGTH_CONTROL);
break; return MAV_RESULT_ACCEPTED;
} }
case WINCH_RATE_CONTROL: { case WINCH_RATE_CONTROL:
if (fabsf(packet.param4) <= copter.g2.winch.get_rate_max()) { if (fabsf(packet.param4) <= copter.g2.winch.get_rate_max()) {
copter.g2.winch.set_desired_rate(packet.param4); copter.g2.winch.set_desired_rate(packet.param4);
copter.Log_Write_Event(DATA_WINCH_RATE_CONTROL); copter.Log_Write_Event(DATA_WINCH_RATE_CONTROL);
} else { return MAV_RESULT_ACCEPTED;
result = MAV_RESULT_FAILED;
}
break;
} }
return MAV_RESULT_FAILED;
default: default:
result = MAV_RESULT_FAILED;
break; break;
} }
} return MAV_RESULT_FAILED;
break;
#endif #endif
/* Solo user presses Fly button */ /* Solo user presses Fly button */
case MAV_CMD_SOLO_BTN_FLY_CLICK: { case MAV_CMD_SOLO_BTN_FLY_CLICK: {
result = MAV_RESULT_ACCEPTED;
if (copter.failsafe.radio) { if (copter.failsafe.radio) {
break; return MAV_RESULT_ACCEPTED;
} }
// set mode to Loiter or fall back to AltHold // set mode to Loiter or fall back to AltHold
if (!copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) { if (!copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND); copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND);
} }
break; return MAV_RESULT_ACCEPTED;
} }
/* Solo user holds down Fly button for a couple of seconds */ /* Solo user holds down Fly button for a couple of seconds */
case MAV_CMD_SOLO_BTN_FLY_HOLD: { case MAV_CMD_SOLO_BTN_FLY_HOLD: {
result = MAV_RESULT_ACCEPTED;
if (copter.failsafe.radio) { if (copter.failsafe.radio) {
break; return MAV_RESULT_ACCEPTED;
} }
if (!copter.motors->armed()) { if (!copter.motors->armed()) {
@ -1093,15 +959,13 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
// if flying, land // if flying, land
copter.set_mode(LAND, MODE_REASON_GCS_COMMAND); copter.set_mode(LAND, MODE_REASON_GCS_COMMAND);
} }
break; return MAV_RESULT_ACCEPTED;
} }
/* Solo user presses pause button */ /* Solo user presses pause button */
case MAV_CMD_SOLO_BTN_PAUSE_CLICK: { case MAV_CMD_SOLO_BTN_PAUSE_CLICK: {
result = MAV_RESULT_ACCEPTED;
if (copter.failsafe.radio) { if (copter.failsafe.radio) {
break; return MAV_RESULT_ACCEPTED;
} }
if (copter.motors->armed()) { if (copter.motors->armed()) {
@ -1128,25 +992,120 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
} }
} }
} }
break; return MAV_RESULT_ACCEPTED;
} }
case MAV_CMD_ACCELCAL_VEHICLE_POS: case MAV_CMD_ACCELCAL_VEHICLE_POS:
result = MAV_RESULT_FAILED; if (!copter.ins.get_acal()->gcs_vehicle_position(packet.param1)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
if (copter.ins.get_acal()->gcs_vehicle_position(packet.param1)) { default:
result = MAV_RESULT_ACCEPTED; 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
{
// 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; break;
}
default: case MAVLINK_MSG_ID_PARAM_VALUE:
result = handle_command_long_message(packet); {
#if MOUNT == ENABLED
copter.camera_mount.handle_param_value(msg);
#endif
break;
}
case MAVLINK_MSG_ID_GIMBAL_REPORT:
{
#if MOUNT == ENABLED
handle_gimbal_report(copter.camera_mount, msg);
#endif
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: // MAV ID: 70
{
// 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; 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);
// send ACK or NAK // record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result); 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;
}
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; break;
} }
@ -1269,22 +1228,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
// send request // send request
if (!pos_ignore && !vel_ignore && acc_ignore) { 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)) { 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) { } 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); 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) { } 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)) { 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; break;
@ -1327,7 +1275,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
if(!pos_ignore) { if(!pos_ignore) {
// sanity check location // sanity check location
if (!check_latlng(packet.lat_int, packet.lon_int)) { if (!check_latlng(packet.lat_int, packet.lon_int)) {
result = MAV_RESULT_FAILED;
break; break;
} }
Location loc; Location loc;
@ -1371,22 +1318,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
} }
if (!pos_ignore && !vel_ignore && acc_ignore) { 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)) { 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) { } 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); 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) { } 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)) { 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; break;
@ -1395,7 +1331,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_DISTANCE_SENSOR: case MAVLINK_MSG_ID_DISTANCE_SENSOR:
{ {
result = MAV_RESULT_ACCEPTED;
copter.rangefinder.handle_msg(msg); copter.rangefinder.handle_msg(msg);
#if PROXIMITY_ENABLED == ENABLED #if PROXIMITY_ENABLED == ENABLED
copter.g2.proximity.handle_msg(msg); copter.g2.proximity.handle_msg(msg);
@ -1459,7 +1394,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
#if PRECISION_LANDING == ENABLED #if PRECISION_LANDING == ENABLED
case MAVLINK_MSG_ID_LANDING_TARGET: case MAVLINK_MSG_ID_LANDING_TARGET:
result = MAV_RESULT_ACCEPTED;
copter.precland.handle_msg(msg); copter.precland.handle_msg(msg);
break; break;
#endif #endif

1
ArduCopter/GCS_Mavlink.h
Unescape View File

@ -36,6 +36,7 @@ protected:
void send_position_target_global_int() override; void send_position_target_global_int() override;
MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) 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: private:

Write Preview
Loading…
Cancel
Save