aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: Add support for pausing and continuing auto missions using COMMAND_INT and COMMAND_LONG

Browse Source
This commit is contained in:
m 2021-11-24 13:46:19 +03:00 committed by Randy Mackay
parent 1af384ad12
commit e8677216b6
2 changed files with 38 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
ArduCopter/GCS_Mavlink.cpp
View File

@ -695,6 +695,11 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
case MAV_CMD_DO_REPOSITION:
return handle_command_int_do_reposition(packet);
// pause or resume an auto mission
case MAV_CMD_DO_PAUSE_CONTINUE:
return handle_command_pause_continue(packet);
default:
return GCS_MAVLINK::handle_command_int_packet(packet);
}
@ -963,11 +968,43 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
return MAV_RESULT_ACCEPTED;
}
// pause or resume an auto mission
case MAV_CMD_DO_PAUSE_CONTINUE: {
mavlink_command_int_t packet_int;
GCS_MAVLINK_Copter::convert_COMMAND_LONG_to_COMMAND_INT(packet, packet_int);
return handle_command_pause_continue(packet_int);
}
default:
return GCS_MAVLINK::handle_command_long_packet(packet);
}
}
MAV_RESULT GCS_MAVLINK_Copter::handle_command_pause_continue(const mavlink_command_int_t &packet)
{
if (copter.flightmode->mode_number() != Mode::Number::AUTO) {
// only supported in AUTO mode
return MAV_RESULT_FAILED;
}
// requested pause from GCS
if ((int8_t) packet.param1 == 0) {
copter.mode_auto.mission.stop();
copter.mode_auto.loiter_start();
gcs().send_text(MAV_SEVERITY_INFO, "Paused mission");
return MAV_RESULT_ACCEPTED;
}
// requested resume from GCS
if ((int8_t) packet.param1 == 1) {
copter.mode_auto.mission.resume();
gcs().send_text(MAV_SEVERITY_INFO, "Resumed mission");
return MAV_RESULT_ACCEPTED;
}
// fail pause or continue
return MAV_RESULT_FAILED;
}
void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg)
{
switch (msg.msgid) {

1
ArduCopter/GCS_Mavlink.h
View File

@ -32,6 +32,7 @@ protected:
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;
MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_pause_continue(const mavlink_command_int_t &packet);
void handle_mount_message(const mavlink_message_t &msg) override;