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

GCS_MAVLink: rearrange mission item handling code

Browse Source 
This splits the missionitemprotocol handling entirely into separate
header files and separate compilation units.
This commit is contained in:
Peter Barker 2019-07-17 12:32:05 +10:00 committed by Randy Mackay
parent b7a748df88
commit 93ca243987
10 changed files with 864 additions and 754 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

255
libraries/GCS_MAVLink/GCS.h
View File

@ -15,7 +15,10 @@
#include <AP_Common/Bitmask.h>
#include <AP_Devo_Telem/AP_Devo_Telem.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_Rally/AP_Rally.h>
#include "MissionItemProtocol_Waypoints.h"
#include "MissionItemProtocol_Rally.h"
#include "ap_message.h"
#define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0
@ -25,79 +28,6 @@
#define CHECK_PAYLOAD_SIZE(id) if (comm_get_txspace(chan) < packet_overhead()+MAVLINK_MSG_ID_ ## id ## _LEN) return false
#define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false
// GCS Message ID's
/// NOTE: to ensure we never block on sending MAVLink messages
/// please keep each MSG_ to a single MAVLink message. If need be
/// create new MSG_ IDs for additional messages on the same
/// stream
enum ap_message : uint8_t {
MSG_HEARTBEAT,
MSG_ATTITUDE,
MSG_LOCATION,
MSG_SYS_STATUS,
MSG_POWER_STATUS,
MSG_MEMINFO,
MSG_NAV_CONTROLLER_OUTPUT,
MSG_CURRENT_WAYPOINT,
MSG_VFR_HUD,
MSG_SERVO_OUTPUT_RAW,
MSG_RC_CHANNELS,
MSG_RC_CHANNELS_RAW,
MSG_RAW_IMU,
MSG_SCALED_IMU,
MSG_SCALED_IMU2,
MSG_SCALED_IMU3,
MSG_SCALED_PRESSURE,
MSG_SCALED_PRESSURE2,
MSG_SCALED_PRESSURE3,
MSG_SENSOR_OFFSETS,
MSG_GPS_RAW,
MSG_GPS_RTK,
MSG_GPS2_RAW,
MSG_GPS2_RTK,
MSG_SYSTEM_TIME,
MSG_SERVO_OUT,
MSG_NEXT_MISSION_REQUEST_WAYPOINTS,
MSG_NEXT_MISSION_REQUEST_RALLY,
MSG_NEXT_PARAM,
MSG_FENCE_STATUS,
MSG_AHRS,
MSG_SIMSTATE,
MSG_AHRS2,
MSG_AHRS3,
MSG_HWSTATUS,
MSG_WIND,
MSG_RANGEFINDER,
MSG_DISTANCE_SENSOR,
MSG_TERRAIN,
MSG_BATTERY2,
MSG_CAMERA_FEEDBACK,
MSG_MOUNT_STATUS,
MSG_OPTICAL_FLOW,
MSG_GIMBAL_REPORT,
MSG_MAG_CAL_PROGRESS,
MSG_MAG_CAL_REPORT,
MSG_EKF_STATUS_REPORT,
MSG_LOCAL_POSITION,
MSG_PID_TUNING,
MSG_VIBRATION,
MSG_RPM,
MSG_WHEEL_DISTANCE,
MSG_MISSION_ITEM_REACHED,
MSG_POSITION_TARGET_GLOBAL_INT,
MSG_POSITION_TARGET_LOCAL_NED,
MSG_ADSB_VEHICLE,
MSG_BATTERY_STATUS,
MSG_AOA_SSA,
MSG_LANDING,
MSG_ESC_TELEMETRY,
MSG_ORIGIN,
MSG_HOME,
MSG_NAMED_FLOAT,
MSG_EXTENDED_SYS_STATE,
MSG_LAST // MSG_LAST must be the last entry in this enum
};
// convenience macros for defining which ap_message ids are in which streams:
#define MAV_STREAM_ENTRY(stream_name) \
{ \
@ -107,183 +37,6 @@ enum ap_message : uint8_t {
}
#define MAV_STREAM_TERMINATOR { (streams)0, nullptr, 0 }
// MissionItemProtocol objects are used for transfering missions from
// a GCS to ArduPilot and vice-versa.
//
// There exists one MissionItemProtocol instance for each of the types
// of item that might be transfered - e.g. MissionItemProtocol_Rally
// for rally point uploads. These objects are static in GCS_MAVLINK
// and used by all of the backends.
//
// While prompting the GCS for items required to complete the mission,
// a link is stored to the link the MissionItemProtocol should send
// requests out on, and the "receiving" boolean is true. In this
// state downloading of items (and the item count!) is blocked.
// Starting of uploads (for the same protocol) is also blocked -
// essentially the GCS uploading a set of items (e.g. a mission) has a
// mutex over the mission.
class MissionItemProtocol
{
public:
// note that all of these methods are named after the packet they
// are handling; the "mission" part just comes as part of that.
void handle_mission_request_list(const class GCS_MAVLINK &link,
const mavlink_mission_request_list_t &packet,
const mavlink_message_t &msg);
void handle_mission_request_int(const GCS_MAVLINK &link,
const mavlink_mission_request_int_t &packet,
const mavlink_message_t &msg);
void handle_mission_request(const GCS_MAVLINK &link,
const mavlink_mission_request_t &packet,
const mavlink_message_t &msg);
void handle_mission_count(class GCS_MAVLINK &link,
const mavlink_mission_count_t &packet,
const mavlink_message_t &msg);
void handle_mission_write_partial_list(GCS_MAVLINK &link,
const mavlink_message_t &msg,
const mavlink_mission_write_partial_list_t &packet);
// called on receipt of a MISSION_ITEM or MISSION_ITEM_INT packet;
// the former is converted to the latter.
void handle_mission_item(const mavlink_message_t &msg,
const mavlink_mission_item_int_t &cmd);
void handle_mission_clear_all(const GCS_MAVLINK &link,
const mavlink_message_t &msg);
void queued_request_send();
void update();
bool active_link_is(const GCS_MAVLINK *_link) const { return _link == link; };
virtual MAV_MISSION_TYPE mission_type() const = 0;
bool receiving; // currently sending requests and expecting items
protected:
GCS_MAVLINK *link; // link currently receiving waypoints on
// return the ap_message which can be queued to be sent to send a
// item request to the GCS:
virtual ap_message next_item_ap_message_id() const = 0;
virtual bool clear_all_items() = 0;
uint16_t request_last; // last request index
private:
virtual void truncate(const mavlink_mission_count_t &packet) = 0;
uint16_t request_i; // request index
// waypoints
uint8_t dest_sysid; // where to send requests
uint8_t dest_compid; // "
uint32_t timelast_receive_ms;
uint32_t timelast_request_ms;
const uint16_t upload_timeout_ms = 8000;
// support for GCS getting waypoints etc from us:
virtual MAV_MISSION_RESULT 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) = 0;
void init_send_requests(GCS_MAVLINK &_link,
const mavlink_message_t &msg,
const int16_t _request_first,
const int16_t _request_last);
void send_mission_ack(const mavlink_message_t &msg, MAV_MISSION_RESULT result) const;
void send_mission_ack(const GCS_MAVLINK &link, const mavlink_message_t &msg, MAV_MISSION_RESULT result) const;
virtual uint16_t item_count() const = 0;
virtual uint16_t max_items() const = 0;
virtual MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t &mission_item_int) = 0;
virtual MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t &mission_item_int) = 0;
virtual void complete(const GCS_MAVLINK &_link) {};
virtual void timeout() {};
void convert_MISSION_REQUEST_to_MISSION_REQUEST_INT(const mavlink_mission_request_t &request,
mavlink_mission_request_int_t &request_int);
};
class MissionItemProtocol_Waypoints : public MissionItemProtocol {
public:
MissionItemProtocol_Waypoints(AP_Mission &_mission) :
mission(_mission) {}
void truncate(const mavlink_mission_count_t &packet) override;
MAV_MISSION_TYPE mission_type() const override { return MAV_MISSION_TYPE_MISSION; }
void complete(const GCS_MAVLINK &_link) override;
void timeout() override;
protected:
bool clear_all_items() override WARN_IF_UNUSED;
ap_message next_item_ap_message_id() const override {
return MSG_NEXT_MISSION_REQUEST_WAYPOINTS;
}
private:
AP_Mission &mission;
uint16_t item_count() const override { return mission.num_commands(); }
uint16_t max_items() const override { return mission.num_commands_max(); }
MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t &) override WARN_IF_UNUSED;
MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t &) override WARN_IF_UNUSED;
MAV_MISSION_RESULT 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) override WARN_IF_UNUSED;
};
class MissionItemProtocol_Rally : public MissionItemProtocol {
public:
MissionItemProtocol_Rally(AP_Rally &_rally) :
rally(_rally) {}
void truncate(const mavlink_mission_count_t &packet) override;
MAV_MISSION_TYPE mission_type() const override { return MAV_MISSION_TYPE_RALLY; }
void complete(const GCS_MAVLINK &_link) override;
void timeout() override;
protected:
ap_message next_item_ap_message_id() const override {
return MSG_NEXT_MISSION_REQUEST_RALLY;
}
bool clear_all_items() override WARN_IF_UNUSED;
private:
AP_Rally &rally;
uint16_t item_count() const override {
return rally.get_rally_total();
}
uint16_t max_items() const override { return rally.get_rally_max(); }
MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED;
MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED;
MAV_MISSION_RESULT 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) override WARN_IF_UNUSED;
static MAV_MISSION_RESULT convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, RallyLocation &ret) WARN_IF_UNUSED;
};
///
/// @class GCS_MAVLINK
/// @brief MAVLink transport control class

420
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -180,78 +180,6 @@ GCS_MAVLINK::setup_uart(uint8_t instance)
}
}
/**
* @brief Send the next pending waypoint, called from deferred message
* handling code
*/
void MissionItemProtocol::queued_request_send()
{
if (!receiving) {
return;
}
if (request_i > request_last) {
return;
}
if (link == nullptr) {
AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
return;
}
mavlink_msg_mission_request_send(
link->get_chan(),
dest_sysid,
dest_compid,
request_i,
mission_type());
timelast_request_ms = AP_HAL::millis();
}
void MissionItemProtocol::update()
{
if (!receiving) {
// we don't need to do anything unless we're sending requests
return;
}
if (link == nullptr) {
AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
return;
}
// stop waypoint receiving if timeout
const uint32_t tnow = AP_HAL::millis();
if (tnow - timelast_receive_ms > upload_timeout_ms) {
receiving = false;
timeout();
link = nullptr;
return;
}
// resend request if we haven't gotten one:
const uint32_t wp_recv_timeout_ms = 1000U + (link->get_stream_slowdown_ms()*20);
if (tnow - timelast_request_ms > wp_recv_timeout_ms) {
timelast_request_ms = tnow;
link->send_message(next_item_ap_message_id());
}
}
void MissionItemProtocol::send_mission_ack(const mavlink_message_t &msg,
MAV_MISSION_RESULT result) const
{
if (link == nullptr) {
AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
return;
}
send_mission_ack(*link, msg, result);
}
void MissionItemProtocol::send_mission_ack(const GCS_MAVLINK &_link,
const mavlink_message_t &msg,
MAV_MISSION_RESULT result) const
{
mavlink_msg_mission_ack_send(_link.get_chan(),
msg.sysid,
msg.compid,
result,
mission_type());
}
void GCS_MAVLINK::send_meminfo(void)
{
unsigned __brkval = 0;
@ -543,62 +471,6 @@ void GCS_MAVLINK::handle_mission_request_list(const mavlink_message_t &msg)
prot->handle_mission_request_list(*this, packet, msg);
}
void MissionItemProtocol::handle_mission_request_list(
const GCS_MAVLINK &_link,
const mavlink_mission_request_list_t &packet,
const mavlink_message_t &msg)
{
if (receiving) {
// someone is uploading a mission; reject fetching of points
// until done or timeout
send_mission_ack(_link, msg, MAV_MISSION_DENIED);
return;
}
// reply with number of commands in the mission. The GCS will
// then request each command separately
mavlink_msg_mission_count_send(_link.get_chan(),
msg.sysid,
msg.compid,
item_count(),
mission_type());
}
void MissionItemProtocol::handle_mission_request_int(const GCS_MAVLINK &_link,
const mavlink_mission_request_int_t &packet,
const mavlink_message_t &msg)
{
if (receiving) {
// someone is uploading a mission; reject fetching of points
// until done or timeout
send_mission_ack(_link, msg, MAV_MISSION_DENIED);
return;
}
mavlink_mission_item_int_t ret_packet{};
ret_packet.target_system = msg.sysid;
ret_packet.target_component = msg.compid;
ret_packet.seq = packet.seq;
ret_packet.mission_type = packet.mission_type;
const MAV_MISSION_RESULT result_code = get_item(_link, msg, packet, ret_packet);
if (result_code != MAV_MISSION_ACCEPTED) {
// send failure message
send_mission_ack(_link, msg, result_code);
return;
}
// we already have a filled structure, use it in place of _send:
_mav_finalize_message_chan_send(_link.get_chan(),
MAVLINK_MSG_ID_MISSION_ITEM_INT,
(const char *)&ret_packet,
MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN,
MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN,
MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC);
}
/*
handle a MISSION_REQUEST mavlink packet
*/
@ -615,48 +487,6 @@ void GCS_MAVLINK::handle_mission_request_int(const mavlink_message_t &msg)
prot->handle_mission_request_int(*this, packet, msg);
}
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:
mavlink_msg_mission_count_send(_link.get_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)
ret_packet.command = cmd.id;
return MAV_MISSION_ACCEPTED;
}
void GCS_MAVLINK::handle_mission_request(const mavlink_message_t &msg)
{
// decode
@ -670,46 +500,6 @@ void GCS_MAVLINK::handle_mission_request(const mavlink_message_t &msg)
prot->handle_mission_request(*this, packet, msg);
}
void MissionItemProtocol::convert_MISSION_REQUEST_to_MISSION_REQUEST_INT(const mavlink_mission_request_t &request, mavlink_mission_request_int_t &request_int)
{
request_int.target_system = request.target_system;
request_int.target_component = request.target_component;
request_int.seq = request.seq;
request_int.mission_type = request.mission_type;
}
void MissionItemProtocol::handle_mission_request(const GCS_MAVLINK &_link,
const mavlink_mission_request_t &packet,
const mavlink_message_t &msg
)
{
mavlink_mission_request_int_t request_int;
convert_MISSION_REQUEST_to_MISSION_REQUEST_INT(packet, request_int);
mavlink_mission_item_int_t item_int{};
item_int.target_system = msg.sysid;
item_int.target_component = msg.compid;
MAV_MISSION_RESULT ret = get_item(_link, msg, request_int, item_int);
if (ret != MAV_MISSION_ACCEPTED) {
return;
}
mavlink_mission_item_t ret_packet{};
ret = AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(item_int, ret_packet);
if (ret != MAV_MISSION_ACCEPTED) {
return;
}
// we already have a filled structure, use it in place of _send:
_mav_finalize_message_chan_send(_link.get_chan(),
MAVLINK_MSG_ID_MISSION_ITEM,
(const char *)&ret_packet,
MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN,
MAVLINK_MSG_ID_MISSION_ITEM_LEN,
MAVLINK_MSG_ID_MISSION_ITEM_CRC);
}
/*
handle a MISSION_SET_CURRENT mavlink packet
*/
@ -747,66 +537,6 @@ void GCS_MAVLINK::handle_mission_count(const mavlink_message_t &msg)
prot->handle_mission_count(*this, packet, msg);
}
void MissionItemProtocol::init_send_requests(GCS_MAVLINK &_link,
const mavlink_message_t &msg,
const int16_t _request_first,
const int16_t _request_last)
{
// set variables to help handle the expected receiving of commands from the GCS
timelast_receive_ms = AP_HAL::millis(); // set time we last received commands to now
receiving = true; // record that we expect to receive commands
request_i = _request_first; // reset the next expected command number to zero
request_last = _request_last; // record how many commands we expect to receive
dest_sysid = msg.sysid; // record system id of GCS who wants to upload the mission
dest_compid = msg.compid; // record component id of GCS who wants to upload the mission
link = &_link;
timelast_request_ms = AP_HAL::millis();
link->send_message(next_item_ap_message_id());
}
void MissionItemProtocol::handle_mission_count(
GCS_MAVLINK &_link,
const mavlink_mission_count_t &packet,
const mavlink_message_t &msg)
{
if (receiving) {
// someone is already uploading a mission. If we are
// receiving from someone then we will allow them to restart -
// otherwise we deny.
if (msg.sysid != dest_sysid || msg.compid != dest_compid) {
// reject another upload until
send_mission_ack(_link, msg, MAV_MISSION_DENIED);
return;
}
}
if (packet.count > max_items()) {
send_mission_ack(_link, msg, MAV_MISSION_NO_SPACE);
return;
}
truncate(packet);
if (packet.count == 0) {
// no requests to send...
send_mission_ack(_link, msg, MAV_MISSION_ACCEPTED);
complete(_link);
return;
}
// start waypoint receiving
init_send_requests(_link, msg, 0, packet.count-1);
}
void MissionItemProtocol_Waypoints::truncate(const mavlink_mission_count_t &packet)
{
// new mission arriving, truncate mission to be the same length
mission.truncate(packet.count);
}
/*
handle a MISSION_CLEAR_ALL mavlink packet
*/
@ -826,26 +556,6 @@ void GCS_MAVLINK::handle_mission_clear_all(const mavlink_message_t &msg)
prot->handle_mission_clear_all(*this, msg);
}
bool MissionItemProtocol_Waypoints::clear_all_items()
{
return mission.clear();
}
bool MissionItemProtocol_Rally::clear_all_items()
{
rally.truncate(0);
return true;
}
void MissionItemProtocol::handle_mission_clear_all(const GCS_MAVLINK &_link,
const mavlink_message_t &msg)
{
bool success = true;
success = success && !receiving;
success = success && clear_all_items();
send_mission_ack(_link, msg, success ? MAV_MISSION_ACCEPTED : MAV_MISSION_ERROR);
}
bool GCS_MAVLINK::requesting_mission_items() const
{
for (uint8_t i=0; i<ARRAY_SIZE(supported_mission_types); i++) {
@ -871,24 +581,6 @@ void GCS_MAVLINK::handle_mission_write_partial_list(const mavlink_message_t &msg
use_prot->handle_mission_write_partial_list(*this, msg, packet);
}
void MissionItemProtocol::handle_mission_write_partial_list(GCS_MAVLINK &_link,
const mavlink_message_t &msg,
const mavlink_mission_write_partial_list_t &packet)
{
// start waypoint receiving
if ((unsigned)packet.start_index > item_count() ||
(unsigned)packet.end_index > item_count() ||
packet.end_index < packet.start_index) {
gcs().send_text(MAV_SEVERITY_WARNING,"Flight plan update rejected"); // FIXME: Remove this anytime after 2020-01-22
send_mission_ack(_link, msg, MAV_MISSION_ERROR);
return;
}
init_send_requests(_link, msg, packet.start_index, packet.end_index);
}
/*
pass mavlink messages to the AP_Mount singleton
*/
@ -1031,118 +723,6 @@ void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg)
prot->handle_mission_item(msg, packet);
}
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;
}
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;
}
void MissionItemProtocol_Waypoints::complete(const GCS_MAVLINK &_link)
{
_link.send_text(MAV_SEVERITY_INFO, "Flight plan received");
AP::logger().Write_EntireMission();
}
void MissionItemProtocol_Waypoints::timeout()
{
link->send_text(MAV_SEVERITY_WARNING, "Mission upload timeout");
}
void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, const mavlink_mission_item_int_t &cmd)
{
if (link == nullptr) {
AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
return;
}
// check if this is the requested waypoint
if (cmd.seq != request_i) {
send_mission_ack(msg, MAV_MISSION_INVALID_SEQUENCE);
return;
}
// make sure the item is coming from the system that initiated the upload
if (msg.sysid != dest_sysid) {
send_mission_ack(msg, MAV_MISSION_DENIED);
return;
}
if (msg.compid != dest_compid) {
send_mission_ack(msg, MAV_MISSION_DENIED);
return;
}
const uint16_t _item_count = item_count();
MAV_MISSION_RESULT result;
if (cmd.seq < _item_count) {
// command index is within the existing list, replace the command
result = replace_item(cmd);
} else if (cmd.seq == _item_count) {
// command is at the end of command list, add the command
result = append_item(cmd);
} else {
// beyond the end of the command list, return an error
result = MAV_MISSION_ERROR;
}
if (result != MAV_MISSION_ACCEPTED) {
send_mission_ack(msg, result);
return;
}
// update waypoint receiving state machine
timelast_receive_ms = AP_HAL::millis();
request_i++;
if (request_i > request_last) {
send_mission_ack(msg, MAV_MISSION_ACCEPTED);
complete(*link);
receiving = false;
link = nullptr;
return;
}
// if we have enough space, then send the next WP request immediately
if (HAVE_PAYLOAD_SPACE(link->get_chan(), MISSION_REQUEST)) {
queued_request_send();
} else {
link->send_message(next_item_ap_message_id());
}
}
ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) const
{
// MSG_NEXT_MISSION_REQUEST doesn't correspond to a mavlink message directly.

86
libraries/GCS_MAVLink/GCS_Rally.cpp
View File

@ -1,5 +1,7 @@
/*
GCS MAVLink functions related to rally points
GCS MAVLink functions related to upload and download of rally
points with the ArduPilot-specific protocol comprised of
MAVLINK_MSG_ID_RALLY_POINT and MAVLINK_MSG_ID_RALLY_FETCH_POINT.
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
@ -101,85 +103,3 @@ void GCS_MAVLINK::handle_common_rally_message(const mavlink_message_t &msg)
break;
}
}
MAV_MISSION_RESULT MissionItemProtocol_Rally::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)
{
RallyLocation rallypoint;
if (!rally.get_rally_point_with_index(packet.seq, rallypoint)) {
return MAV_MISSION_INVALID_SEQUENCE;
}
ret_packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT;
ret_packet.command = MAV_CMD_NAV_RALLY_POINT;
ret_packet.x = rallypoint.lat;
ret_packet.y = rallypoint.lng;
ret_packet.z = rallypoint.alt;
return MAV_MISSION_ACCEPTED;
}
void MissionItemProtocol_Rally::truncate(const mavlink_mission_count_t &packet)
{
rally.truncate(packet.count);
}
void MissionItemProtocol_Rally::timeout()
{
link->send_text(MAV_SEVERITY_WARNING, "Rally upload timeout");
}
MAV_MISSION_RESULT MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, RallyLocation &ret)
{
if (cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) {
return MAV_MISSION_UNSUPPORTED_FRAME;
}
if (!check_lat(cmd.x)) {
return MAV_MISSION_INVALID_PARAM5_X;
}
if (!check_lng(cmd.y)) {
return MAV_MISSION_INVALID_PARAM6_Y;
}
if (cmd.z < INT16_MIN || cmd.z > INT16_MAX) {
return MAV_MISSION_INVALID_PARAM7;
}
ret.lat = cmd.x;
ret.lng = cmd.y;
ret.alt = cmd.z;
return MAV_MISSION_ACCEPTED;
}
MAV_MISSION_RESULT MissionItemProtocol_Rally::replace_item(const mavlink_mission_item_int_t &cmd)
{
RallyLocation rallyloc;
const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_RallyLocation(cmd, rallyloc);
if (ret != MAV_MISSION_ACCEPTED) {
return ret;
}
if (!rally.set_rally_point_with_index(cmd.seq, rallyloc)) {
return MAV_MISSION_ERROR;
}
return MAV_MISSION_ACCEPTED;
}
MAV_MISSION_RESULT MissionItemProtocol_Rally::append_item(const mavlink_mission_item_int_t &cmd)
{
RallyLocation rallyloc;
const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_RallyLocation(cmd, rallyloc);
if (ret != MAV_MISSION_ACCEPTED) {
return ret;
}
if (!rally.append(rallyloc)) {
return MAV_MISSION_ERROR;
}
return MAV_MISSION_ACCEPTED;
}
void MissionItemProtocol_Rally::complete(const GCS_MAVLINK &_link)
{
_link.send_text(MAV_SEVERITY_INFO, "Rally points received");
AP::logger().Write_Rally();
}

305
libraries/GCS_MAVLink/MissionItemProtocol.cpp Normal file
View File

@ -0,0 +1,305 @@
#include "MissionItemProtocol.h"
#include "GCS.h"
void MissionItemProtocol::init_send_requests(GCS_MAVLINK &_link,
const mavlink_message_t &msg,
const int16_t _request_first,
const int16_t _request_last)
{
// set variables to help handle the expected receiving of commands from the GCS
timelast_receive_ms = AP_HAL::millis(); // set time we last received commands to now
receiving = true; // record that we expect to receive commands
request_i = _request_first; // reset the next expected command number to zero
request_last = _request_last; // record how many commands we expect to receive
dest_sysid = msg.sysid; // record system id of GCS who wants to upload the mission
dest_compid = msg.compid; // record component id of GCS who wants to upload the mission
link = &_link;
timelast_request_ms = AP_HAL::millis();
link->send_message(next_item_ap_message_id());
}
void MissionItemProtocol::handle_mission_clear_all(const GCS_MAVLINK &_link,
const mavlink_message_t &msg)
{
bool success = true;
success = success && !receiving;
success = success && clear_all_items();
send_mission_ack(_link, msg, success ? MAV_MISSION_ACCEPTED : MAV_MISSION_ERROR);
}
void MissionItemProtocol::handle_mission_count(
GCS_MAVLINK &_link,
const mavlink_mission_count_t &packet,
const mavlink_message_t &msg)
{
if (receiving) {
// someone is already uploading a mission. If we are
// receiving from someone then we will allow them to restart -
// otherwise we deny.
if (msg.sysid != dest_sysid || msg.compid != dest_compid) {
// reject another upload until
send_mission_ack(_link, msg, MAV_MISSION_DENIED);
return;
}
}
if (packet.count > max_items()) {
send_mission_ack(_link, msg, MAV_MISSION_NO_SPACE);
return;
}
truncate(packet);
if (packet.count == 0) {
// no requests to send...
send_mission_ack(_link, msg, MAV_MISSION_ACCEPTED);
complete(_link);
return;
}
// start waypoint receiving
init_send_requests(_link, msg, 0, packet.count-1);
}
void MissionItemProtocol::handle_mission_request_list(
const GCS_MAVLINK &_link,
const mavlink_mission_request_list_t &packet,
const mavlink_message_t &msg)
{
if (receiving) {
// someone is uploading a mission; reject fetching of points
// until done or timeout
send_mission_ack(_link, msg, MAV_MISSION_DENIED);
return;
}
// reply with number of commands in the mission. The GCS will
// then request each command separately
mavlink_msg_mission_count_send(_link.get_chan(),
msg.sysid,
msg.compid,
item_count(),
mission_type());
}
void MissionItemProtocol::handle_mission_request_int(const GCS_MAVLINK &_link,
const mavlink_mission_request_int_t &packet,
const mavlink_message_t &msg)
{
if (receiving) {
// someone is uploading a mission; reject fetching of points
// until done or timeout
send_mission_ack(_link, msg, MAV_MISSION_DENIED);
return;
}
mavlink_mission_item_int_t ret_packet{};
ret_packet.target_system = msg.sysid;
ret_packet.target_component = msg.compid;
ret_packet.seq = packet.seq;
ret_packet.mission_type = packet.mission_type;
const MAV_MISSION_RESULT result_code = get_item(_link, msg, packet, ret_packet);
if (result_code != MAV_MISSION_ACCEPTED) {
// send failure message
send_mission_ack(_link, msg, result_code);
return;
}
// we already have a filled structure, use it in place of _send:
_mav_finalize_message_chan_send(_link.get_chan(),
MAVLINK_MSG_ID_MISSION_ITEM_INT,
(const char *)&ret_packet,
MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN,
MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN,
MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC);
}
void MissionItemProtocol::handle_mission_request(const GCS_MAVLINK &_link,
const mavlink_mission_request_t &packet,
const mavlink_message_t &msg
)
{
// convert into a MISSION_REQUEST_INT and reuse its handling code
mavlink_mission_request_int_t request_int;
request_int.target_system = packet.target_system;
request_int.target_component = packet.target_component;
request_int.seq = packet.seq;
request_int.mission_type = packet.mission_type;
mavlink_mission_item_int_t item_int{};
item_int.target_system = msg.sysid;
item_int.target_component = msg.compid;
MAV_MISSION_RESULT ret = get_item(_link, msg, request_int, item_int);
if (ret != MAV_MISSION_ACCEPTED) {
return;
}
mavlink_mission_item_t ret_packet{};
ret = AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(item_int, ret_packet);
if (ret != MAV_MISSION_ACCEPTED) {
return;
}
// we already have a filled structure, use it in place of _send:
_mav_finalize_message_chan_send(_link.get_chan(),
MAVLINK_MSG_ID_MISSION_ITEM,
(const char *)&ret_packet,
MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN,
MAVLINK_MSG_ID_MISSION_ITEM_LEN,
MAVLINK_MSG_ID_MISSION_ITEM_CRC);
}
void MissionItemProtocol::handle_mission_write_partial_list(GCS_MAVLINK &_link,
const mavlink_message_t &msg,
const mavlink_mission_write_partial_list_t &packet)
{
// start waypoint receiving
if ((unsigned)packet.start_index > item_count() ||
(unsigned)packet.end_index > item_count() ||
packet.end_index < packet.start_index) {
gcs().send_text(MAV_SEVERITY_WARNING,"Flight plan update rejected"); // FIXME: Remove this anytime after 2020-01-22
send_mission_ack(_link, msg, MAV_MISSION_ERROR);
return;
}
init_send_requests(_link, msg, packet.start_index, packet.end_index);
}
void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, const mavlink_mission_item_int_t &cmd)
{
if (link == nullptr) {
AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
return;
}
// check if this is the requested waypoint
if (cmd.seq != request_i) {
send_mission_ack(msg, MAV_MISSION_INVALID_SEQUENCE);
return;
}
// make sure the item is coming from the system that initiated the upload
if (msg.sysid != dest_sysid) {
send_mission_ack(msg, MAV_MISSION_DENIED);
return;
}
if (msg.compid != dest_compid) {
send_mission_ack(msg, MAV_MISSION_DENIED);
return;
}
const uint16_t _item_count = item_count();
MAV_MISSION_RESULT result;
if (cmd.seq < _item_count) {
// command index is within the existing list, replace the command
result = replace_item(cmd);
} else if (cmd.seq == _item_count) {
// command is at the end of command list, add the command
result = append_item(cmd);
} else {
// beyond the end of the command list, return an error
result = MAV_MISSION_ERROR;
}
if (result != MAV_MISSION_ACCEPTED) {
send_mission_ack(msg, result);
return;
}
// update waypoint receiving state machine
timelast_receive_ms = AP_HAL::millis();
request_i++;
if (request_i > request_last) {
send_mission_ack(msg, MAV_MISSION_ACCEPTED);
complete(*link);
receiving = false;
link = nullptr;
return;
}
// if we have enough space, then send the next WP request immediately
if (HAVE_PAYLOAD_SPACE(link->get_chan(), MISSION_REQUEST)) {
queued_request_send();
} else {
link->send_message(next_item_ap_message_id());
}
}
void MissionItemProtocol::send_mission_ack(const mavlink_message_t &msg,
MAV_MISSION_RESULT result) const
{
if (link == nullptr) {
AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
return;
}
send_mission_ack(*link, msg, result);
}
void MissionItemProtocol::send_mission_ack(const GCS_MAVLINK &_link,
const mavlink_message_t &msg,
MAV_MISSION_RESULT result) const
{
mavlink_msg_mission_ack_send(_link.get_chan(),
msg.sysid,
msg.compid,
result,
mission_type());
}
/**
* @brief Send the next pending waypoint, called from deferred message
* handling code
*/
void MissionItemProtocol::queued_request_send()
{
if (!receiving) {
return;
}
if (request_i > request_last) {
return;
}
if (link == nullptr) {
AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
return;
}
mavlink_msg_mission_request_send(
link->get_chan(),
dest_sysid,
dest_compid,
request_i,
mission_type());
timelast_request_ms = AP_HAL::millis();
}
void MissionItemProtocol::update()
{
if (!receiving) {
// we don't need to do anything unless we're sending requests
return;
}
if (link == nullptr) {
AP::internalerror().error(AP_InternalError::error_t::gcs_bad_missionprotocol_link);
return;
}
// stop waypoint receiving if timeout
const uint32_t tnow = AP_HAL::millis();
if (tnow - timelast_receive_ms > upload_timeout_ms) {
receiving = false;
timeout();
link = nullptr;
return;
}
// resend request if we haven't gotten one:
const uint32_t wp_recv_timeout_ms = 1000U + (link->get_stream_slowdown_ms()*20);
if (tnow - timelast_request_ms > wp_recv_timeout_ms) {
timelast_request_ms = tnow;
link->send_message(next_item_ap_message_id());
}
}

111
libraries/GCS_MAVLink/MissionItemProtocol.h Normal file
View File

@ -0,0 +1,111 @@
#pragma once
#include "GCS_MAVLink.h"
#include "ap_message.h"
#include <stdint.h>
// MissionItemProtocol objects are used for transfering missions from
// a GCS to ArduPilot and vice-versa.
//
// There exists one MissionItemProtocol instance for each of the types
// of item that might be transfered - e.g. MissionItemProtocol_Rally
// for rally point uploads. These objects are static in GCS_MAVLINK
// and used by all of the backends.
//
// While prompting the GCS for items required to complete the mission,
// a link is stored to the link the MissionItemProtocol should send
// requests out on, and the "receiving" boolean is true. In this
// state downloading of items (and the item count!) is blocked.
// Starting of uploads (for the same protocol) is also blocked -
// essentially the GCS uploading a set of items (e.g. a mission) has a
// mutex over the mission.
class MissionItemProtocol
{
public:
// note that all of these methods are named after the packet they
// are handling; the "mission" part just comes as part of that.
void handle_mission_request_list(const class GCS_MAVLINK &link,
const mavlink_mission_request_list_t &packet,
const mavlink_message_t &msg);
void handle_mission_request_int(const GCS_MAVLINK &link,
const mavlink_mission_request_int_t &packet,
const mavlink_message_t &msg);
void handle_mission_request(const GCS_MAVLINK &link,
const mavlink_mission_request_t &packet,
const mavlink_message_t &msg);
void handle_mission_count(class GCS_MAVLINK &link,
const mavlink_mission_count_t &packet,
const mavlink_message_t &msg);
void handle_mission_write_partial_list(GCS_MAVLINK &link,
const mavlink_message_t &msg,
const mavlink_mission_write_partial_list_t &packet);
// called on receipt of a MISSION_ITEM or MISSION_ITEM_INT packet;
// the former is converted to the latter.
void handle_mission_item(const mavlink_message_t &msg,
const mavlink_mission_item_int_t &cmd);
void handle_mission_clear_all(const GCS_MAVLINK &link,
const mavlink_message_t &msg);
void queued_request_send();
void update();
bool active_link_is(const GCS_MAVLINK *_link) const { return _link == link; };
virtual MAV_MISSION_TYPE mission_type() const = 0;
bool receiving; // currently sending requests and expecting items
protected:
GCS_MAVLINK *link; // link currently receiving waypoints on
// return the ap_message which can be queued to be sent to send a
// item request to the GCS:
virtual ap_message next_item_ap_message_id() const = 0;
virtual bool clear_all_items() = 0;
uint16_t request_last; // last request index
private:
virtual void truncate(const mavlink_mission_count_t &packet) = 0;
uint16_t request_i; // request index
// waypoints
uint8_t dest_sysid; // where to send requests
uint8_t dest_compid; // "
uint32_t timelast_receive_ms;
uint32_t timelast_request_ms;
const uint16_t upload_timeout_ms = 8000;
// support for GCS getting waypoints etc from us:
virtual MAV_MISSION_RESULT 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) = 0;
void init_send_requests(GCS_MAVLINK &_link,
const mavlink_message_t &msg,
const int16_t _request_first,
const int16_t _request_last);
void send_mission_ack(const mavlink_message_t &msg, MAV_MISSION_RESULT result) const;
void send_mission_ack(const GCS_MAVLINK &link, const mavlink_message_t &msg, MAV_MISSION_RESULT result) const;
virtual uint16_t item_count() const = 0;
virtual uint16_t max_items() const = 0;
virtual MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t &mission_item_int) = 0;
virtual MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t &mission_item_int) = 0;
virtual void complete(const GCS_MAVLINK &_link) {};
virtual void timeout() {};
};

121
libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp Normal file
View File

@ -0,0 +1,121 @@
/*
Implementation details for transfering rally point 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 "MissionItemProtocol_Rally.h"
#include <AP_Logger/AP_Logger.h>
#include <AP_Rally/AP_Rally.h>
#include <GCS_MAVLink/GCS.h>
MAV_MISSION_RESULT MissionItemProtocol_Rally::append_item(const mavlink_mission_item_int_t &cmd)
{
RallyLocation rallyloc;
const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_RallyLocation(cmd, rallyloc);
if (ret != MAV_MISSION_ACCEPTED) {
return ret;
}
if (!rally.append(rallyloc)) {
return MAV_MISSION_ERROR;
}
return MAV_MISSION_ACCEPTED;
}
void MissionItemProtocol_Rally::complete(const GCS_MAVLINK &_link)
{
_link.send_text(MAV_SEVERITY_INFO, "Rally points received");
AP::logger().Write_Rally();
}
bool MissionItemProtocol_Rally::clear_all_items()
{
rally.truncate(0);
return true;
}
MAV_MISSION_RESULT MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, RallyLocation &ret)
{
if (cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
cmd.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) {
return MAV_MISSION_UNSUPPORTED_FRAME;
}
if (!check_lat(cmd.x)) {
return MAV_MISSION_INVALID_PARAM5_X;
}
if (!check_lng(cmd.y)) {
return MAV_MISSION_INVALID_PARAM6_Y;
}
if (cmd.z < INT16_MIN || cmd.z > INT16_MAX) {
return MAV_MISSION_INVALID_PARAM7;
}
ret.lat = cmd.x;
ret.lng = cmd.y;
ret.alt = cmd.z;
return MAV_MISSION_ACCEPTED;
}
MAV_MISSION_RESULT MissionItemProtocol_Rally::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)
{
RallyLocation rallypoint;
if (!rally.get_rally_point_with_index(packet.seq, rallypoint)) {
return MAV_MISSION_INVALID_SEQUENCE;
}
ret_packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT;
ret_packet.command = MAV_CMD_NAV_RALLY_POINT;
ret_packet.x = rallypoint.lat;
ret_packet.y = rallypoint.lng;
ret_packet.z = rallypoint.alt;
return MAV_MISSION_ACCEPTED;
}
uint16_t MissionItemProtocol_Rally::item_count() const {
return rally.get_rally_total();
}
uint16_t MissionItemProtocol_Rally::max_items() const {
return rally.get_rally_max();
}
MAV_MISSION_RESULT MissionItemProtocol_Rally::replace_item(const mavlink_mission_item_int_t &cmd)
{
RallyLocation rallyloc;
const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_RallyLocation(cmd, rallyloc);
if (ret != MAV_MISSION_ACCEPTED) {
return ret;
}
if (!rally.set_rally_point_with_index(cmd.seq, rallyloc)) {
return MAV_MISSION_ERROR;
}
return MAV_MISSION_ACCEPTED;
}
void MissionItemProtocol_Rally::timeout()
{
link->send_text(MAV_SEVERITY_WARNING, "Rally upload timeout");
}
void MissionItemProtocol_Rally::truncate(const mavlink_mission_count_t &packet)
{
rally.truncate(packet.count);
}

38
libraries/GCS_MAVLink/MissionItemProtocol_Rally.h Normal file
View File

@ -0,0 +1,38 @@
#pragma once
#include "MissionItemProtocol.h"
class MissionItemProtocol_Rally : public MissionItemProtocol {
public:
MissionItemProtocol_Rally(class AP_Rally &_rally) :
rally(_rally) {}
void truncate(const mavlink_mission_count_t &packet) override;
MAV_MISSION_TYPE mission_type() const override { return MAV_MISSION_TYPE_RALLY; }
void complete(const GCS_MAVLINK &_link) override;
void timeout() override;
protected:
ap_message next_item_ap_message_id() const override {
return MSG_NEXT_MISSION_REQUEST_RALLY;
}
bool clear_all_items() override WARN_IF_UNUSED;
private:
AP_Rally &rally;
uint16_t item_count() const override;
uint16_t max_items() const override;
MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED;
MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED;
MAV_MISSION_RESULT 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) override WARN_IF_UNUSED;
static MAV_MISSION_RESULT convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, class RallyLocation &ret) WARN_IF_UNUSED;
};

139
libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp Normal file
View File

@ -0,0 +1,139 @@
/*
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 "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();
}
void MissionItemProtocol_Waypoints::complete(const GCS_MAVLINK &_link)
{
_link.send_text(MAV_SEVERITY_INFO, "Flight plan received");
AP::logger().Write_EntireMission();
}
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:
mavlink_msg_mission_count_send(_link.get_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)
ret_packet.command = cmd.id;
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);
}

68
libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h Normal file
View File

@ -0,0 +1,68 @@
#pragma once
#include "MissionItemProtocol.h"
class MissionItemProtocol_Waypoints : public MissionItemProtocol {
public:
MissionItemProtocol_Waypoints(class AP_Mission &_mission) :
mission(_mission) {}
// mission_type returns the MAV_MISSION mavlink enumeration value
// which this module is responsible for handling
MAV_MISSION_TYPE mission_type() const override {
return MAV_MISSION_TYPE_MISSION;
}
// complete() is called by the base class after all waypoints have
// been received. _link is the link which the last item was
// transfered on.
void complete(const GCS_MAVLINK &_link) override;
// timeout() is called by the base class in the case that the GCS
// does not transfer all waypoints to the vehicle.
void timeout() override;
// truncate() is called to set the absolute number of items. It
// must be less than or equal to the current number of items (you
// can't truncate-to a longer list)
void truncate(const mavlink_mission_count_t &packet) override;
protected:
// clear_all_items() is called to clear all items on the vehicle
bool clear_all_items() override WARN_IF_UNUSED;
// next_item_ap_message_id returns an item from the ap_message
// enumeration which (when acted upon by the GCS class) will send
// a mavlink message to the GCS requesting it upload the next
// required waypoint.
ap_message next_item_ap_message_id() const override {
return MSG_NEXT_MISSION_REQUEST_WAYPOINTS;
}
private:
AP_Mission &mission;
// append_item() is called by the base class to add the supplied
// item to the end of the list of stored items.
MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t &) override WARN_IF_UNUSED;
// get_item() fills in ret_packet based on packet; _link is the
// link the request was received on, and msg is the undecoded
// request. Note that msg may not actually decode to a
// request_int_t!
MAV_MISSION_RESULT 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) override WARN_IF_UNUSED;
// item_count() returns the number of stored items
uint16_t item_count() const override;
// item_count() returns the maximum number of items which could be
// stored on-board
uint16_t max_items() const override;
// replace_item() replaces an item in the stored list
MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t &) override WARN_IF_UNUSED;
};

75
libraries/GCS_MAVLink/ap_message.h Normal file
View File

@ -0,0 +1,75 @@
// GCS Message ID's
/// NOTE: to ensure we never block on sending MAVLink messages
/// please keep each MSG_ to a single MAVLink message. If need be
/// create new MSG_ IDs for additional messages on the same
/// stream
#pragma once
enum ap_message : uint8_t {
MSG_HEARTBEAT,
MSG_ATTITUDE,
MSG_LOCATION,
MSG_SYS_STATUS,
MSG_POWER_STATUS,
MSG_MEMINFO,
MSG_NAV_CONTROLLER_OUTPUT,
MSG_CURRENT_WAYPOINT,
MSG_VFR_HUD,
MSG_SERVO_OUTPUT_RAW,
MSG_RC_CHANNELS,
MSG_RC_CHANNELS_RAW,
MSG_RAW_IMU,
MSG_SCALED_IMU,
MSG_SCALED_IMU2,
MSG_SCALED_IMU3,
MSG_SCALED_PRESSURE,
MSG_SCALED_PRESSURE2,
MSG_SCALED_PRESSURE3,
MSG_SENSOR_OFFSETS,
MSG_GPS_RAW,
MSG_GPS_RTK,
MSG_GPS2_RAW,
MSG_GPS2_RTK,
MSG_SYSTEM_TIME,
MSG_SERVO_OUT,
MSG_NEXT_MISSION_REQUEST_WAYPOINTS,
MSG_NEXT_MISSION_REQUEST_RALLY,
MSG_NEXT_PARAM,
MSG_FENCE_STATUS,
MSG_AHRS,
MSG_SIMSTATE,
MSG_AHRS2,
MSG_AHRS3,
MSG_HWSTATUS,
MSG_WIND,
MSG_RANGEFINDER,
MSG_DISTANCE_SENSOR,
MSG_TERRAIN,
MSG_BATTERY2,
MSG_CAMERA_FEEDBACK,
MSG_MOUNT_STATUS,
MSG_OPTICAL_FLOW,
MSG_GIMBAL_REPORT,
MSG_MAG_CAL_PROGRESS,
MSG_MAG_CAL_REPORT,
MSG_EKF_STATUS_REPORT,
MSG_LOCAL_POSITION,
MSG_PID_TUNING,
MSG_VIBRATION,
MSG_RPM,
MSG_WHEEL_DISTANCE,
MSG_MISSION_ITEM_REACHED,
MSG_POSITION_TARGET_GLOBAL_INT,
MSG_POSITION_TARGET_LOCAL_NED,
MSG_ADSB_VEHICLE,
MSG_BATTERY_STATUS,
MSG_AOA_SSA,
MSG_LANDING,
MSG_ESC_TELEMETRY,
MSG_ORIGIN,
MSG_HOME,
MSG_NAMED_FLOAT,
MSG_EXTENDED_SYS_STATE,
MSG_LAST // MSG_LAST must be the last entry in this enum
};