mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-01 03:04:04 -04:00
4161c425d1
stop passing through _link and the original msg, move use to the base class instead. starts fence and rally also using the "correct the GCS's count" code. This also corrects the error code when correcting the GCS's count to INVALID_SEQUENCE rather than just ERROR
135 lines
4.0 KiB
C++
135 lines
4.0 KiB
C++
/*
|
|
Implementation details for transfering waypoint information using
|
|
the MISSION_ITEM protocol to and from a GCS.
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "GCS_config.h"
|
|
#include <AP_Mission/AP_Mission_config.h>
|
|
|
|
#if HAL_GCS_ENABLED && AP_MISSION_ENABLED
|
|
|
|
#include "MissionItemProtocol_Waypoints.h"
|
|
|
|
#include <AP_Logger/AP_Logger.h>
|
|
#include <AP_Mission/AP_Mission.h>
|
|
|
|
#include "GCS.h"
|
|
|
|
MAV_MISSION_RESULT MissionItemProtocol_Waypoints::append_item(const mavlink_mission_item_int_t &mission_item_int)
|
|
{
|
|
// sanity check for DO_JUMP command
|
|
AP_Mission::Mission_Command cmd {};
|
|
|
|
const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
|
|
if (res != MAV_MISSION_ACCEPTED) {
|
|
return res;
|
|
}
|
|
|
|
if (cmd.id == MAV_CMD_DO_JUMP) {
|
|
if ((cmd.content.jump.target >= item_count() && cmd.content.jump.target > request_last) || cmd.content.jump.target == 0) {
|
|
return MAV_MISSION_ERROR;
|
|
}
|
|
}
|
|
|
|
if (!mission.add_cmd(cmd)) {
|
|
return MAV_MISSION_ERROR;
|
|
}
|
|
return MAV_MISSION_ACCEPTED;
|
|
}
|
|
|
|
bool MissionItemProtocol_Waypoints::clear_all_items()
|
|
{
|
|
return mission.clear();
|
|
}
|
|
|
|
MAV_MISSION_RESULT MissionItemProtocol_Waypoints::complete(const GCS_MAVLINK &_link)
|
|
{
|
|
_link.send_text(MAV_SEVERITY_INFO, "Flight plan received");
|
|
#if HAL_LOGGING_ENABLED
|
|
AP::logger().Write_EntireMission();
|
|
#endif
|
|
return MAV_MISSION_ACCEPTED;
|
|
}
|
|
|
|
MAV_MISSION_RESULT MissionItemProtocol_Waypoints::get_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet)
|
|
{
|
|
if (seq != 0 && // always allow HOME to be read
|
|
seq >= mission.num_commands()) {
|
|
return MAV_MISSION_INVALID_SEQUENCE;
|
|
}
|
|
|
|
AP_Mission::Mission_Command cmd;
|
|
|
|
// retrieve mission from eeprom
|
|
if (!mission.read_cmd_from_storage(seq, cmd)) {
|
|
return MAV_MISSION_ERROR;
|
|
}
|
|
|
|
if (!AP_Mission::mission_cmd_to_mavlink_int(cmd, ret_packet)) {
|
|
return MAV_MISSION_ERROR;
|
|
}
|
|
ret_packet.mission_type = MAV_MISSION_TYPE_MISSION;
|
|
|
|
// set packet's current field to 1 if this is the command being executed
|
|
if (cmd.id == (uint16_t)mission.get_current_nav_cmd().index) {
|
|
ret_packet.current = 1;
|
|
}
|
|
|
|
return MAV_MISSION_ACCEPTED;
|
|
}
|
|
|
|
uint16_t MissionItemProtocol_Waypoints::item_count() const {
|
|
return mission.num_commands();
|
|
}
|
|
|
|
uint16_t MissionItemProtocol_Waypoints::max_items() const {
|
|
return mission.num_commands_max();
|
|
}
|
|
|
|
MAV_MISSION_RESULT MissionItemProtocol_Waypoints::replace_item(const mavlink_mission_item_int_t &mission_item_int)
|
|
{
|
|
AP_Mission::Mission_Command cmd {};
|
|
|
|
const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
|
|
if (res != MAV_MISSION_ACCEPTED) {
|
|
return res;
|
|
}
|
|
|
|
// sanity check for DO_JUMP command
|
|
if (cmd.id == MAV_CMD_DO_JUMP) {
|
|
if ((cmd.content.jump.target >= item_count() && cmd.content.jump.target > request_last) || cmd.content.jump.target == 0) {
|
|
return MAV_MISSION_ERROR;
|
|
}
|
|
}
|
|
if (!mission.replace_cmd(cmd.index, cmd)) {
|
|
return MAV_MISSION_ERROR;
|
|
}
|
|
return MAV_MISSION_ACCEPTED;
|
|
}
|
|
|
|
void MissionItemProtocol_Waypoints::timeout()
|
|
{
|
|
link->send_text(MAV_SEVERITY_WARNING, "Mission upload timeout");
|
|
}
|
|
|
|
void MissionItemProtocol_Waypoints::truncate(const mavlink_mission_count_t &packet)
|
|
{
|
|
// new mission arriving, truncate mission to be the same length
|
|
mission.truncate(packet.count);
|
|
}
|
|
|
|
#endif // HAL_GCS_ENABLED && AP_MISSION_ENABLED
|