-
Notifications
You must be signed in to change notification settings - Fork 16.8k
/
MissionItemProtocol_Waypoints.cpp
150 lines (121 loc) · 4.84 KB
/
MissionItemProtocol_Waypoints.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
/*
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(const GCS_MAVLINK &_link,
const mavlink_message_t &msg,
const mavlink_mission_request_int_t &packet,
mavlink_mission_item_int_t &ret_packet)
{
if (packet.seq != 0 && // always allow HOME to be read
packet.seq >= mission.num_commands()) {
// try to educate the GCS on the actual size of the mission:
const mavlink_channel_t chan = _link.get_chan();
if (HAVE_PAYLOAD_SPACE(chan, MISSION_COUNT)) {
mavlink_msg_mission_count_send(chan,
msg.sysid,
msg.compid,
mission.num_commands(),
MAV_MISSION_TYPE_MISSION);
}
return MAV_MISSION_ERROR;
}
AP_Mission::Mission_Command cmd;
// retrieve mission from eeprom
if (!mission.read_cmd_from_storage(packet.seq, cmd)) {
return MAV_MISSION_ERROR;
}
if (!AP_Mission::mission_cmd_to_mavlink_int(cmd, ret_packet)) {
return MAV_MISSION_ERROR;
}
// 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;
} else {
ret_packet.current = 0;
}
// set auto continue to 1
ret_packet.autocontinue = 1; // 1 (true), 0 (false)
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