Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: allow upload of fence using mission item protocol 

GCS_MAVLink: add support for fence inclusion circles

GCS_MAVLink: factor out a transfer_is_complete; start commenting properly
This commit is contained in:
Peter Barker 2019-04-29 13:05:59 +10:00 committed by Andrew Tridgell
parent 107b9d95ba
commit 179db476bf
9 changed files with 336 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.cpp
View File

@ -23,10 +23,12 @@ void GCS::get_sensor_status_flags(uint32_t &present,
MissionItemProtocol_Waypoints *GCS::_missionitemprotocol_waypoints;
MissionItemProtocol_Rally *GCS::_missionitemprotocol_rally;
MissionItemProtocol_Fence *GCS::_missionitemprotocol_fence;
const MAV_MISSION_TYPE GCS_MAVLINK::supported_mission_types[] = {
MAV_MISSION_TYPE_MISSION,
MAV_MISSION_TYPE_RALLY,
MAV_MISSION_TYPE_FENCE,
};
/*

4
libraries/GCS_MAVLink/GCS.h
View File

@ -18,6 +18,7 @@
#include "MissionItemProtocol_Waypoints.h"
#include "MissionItemProtocol_Rally.h"
#include "MissionItemProtocol_Fence.h"
#include "ap_message.h"
#define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0
@ -85,7 +86,7 @@ public:
mission_type);
}
static const MAV_MISSION_TYPE supported_mission_types[2];
static const MAV_MISSION_TYPE supported_mission_types[3];
// packetReceived is called on any successful decode of a mavlink message
virtual void packetReceived(const mavlink_status_t &status,
@ -742,6 +743,7 @@ public:
static MissionItemProtocol_Waypoints *_missionitemprotocol_waypoints;
static MissionItemProtocol_Rally *_missionitemprotocol_rally;
static MissionItemProtocol_Fence *_missionitemprotocol_fence;
MissionItemProtocol *get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const;
void try_send_queued_message_for_type(MAV_MISSION_TYPE type);

20
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -16,6 +16,7 @@
*/
#include "GCS.h"
#include <AC_Fence/AC_Fence.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Arming/AP_Arming.h>
@ -432,6 +433,8 @@ MissionItemProtocol *GCS::get_prot_for_mission_type(const MAV_MISSION_TYPE missi
return _missionitemprotocol_waypoints;
case MAV_MISSION_TYPE_RALLY:
return _missionitemprotocol_rally;
case MAV_MISSION_TYPE_FENCE:
return _missionitemprotocol_fence;
default:
return nullptr;
}
@ -1858,6 +1861,10 @@ void GCS::update_send()
if (rally != nullptr) {
_missionitemprotocol_rally = new MissionItemProtocol_Rally(*rally);
}
AC_Fence *fence = AP::fence();
if (fence != nullptr) {
_missionitemprotocol_fence = new MissionItemProtocol_Fence(*fence);
}
}
if (_missionitemprotocol_waypoints != nullptr) {
_missionitemprotocol_waypoints->update();
@ -1865,6 +1872,9 @@ void GCS::update_send()
if (_missionitemprotocol_rally != nullptr) {
_missionitemprotocol_rally->update();
}
if (_missionitemprotocol_fence != nullptr) {
_missionitemprotocol_fence->update();
}
for (uint8_t i=0; i<num_gcs(); i++) {
chan(i)->update_send();
}
@ -3910,6 +3920,11 @@ bool GCS_MAVLINK::try_send_mission_message(const enum ap_message id)
gcs().try_send_queued_message_for_type(MAV_MISSION_TYPE_RALLY);
ret = true;
break;
case MSG_NEXT_MISSION_REQUEST_FENCE:
CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
gcs().try_send_queued_message_for_type(MAV_MISSION_TYPE_FENCE);
ret = true;
break;
default:
ret = true;
break;
@ -4145,6 +4160,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
case MSG_MISSION_ITEM_REACHED:
case MSG_NEXT_MISSION_REQUEST_WAYPOINTS:
case MSG_NEXT_MISSION_REQUEST_RALLY:
case MSG_NEXT_MISSION_REQUEST_FENCE:
ret = try_send_mission_message(id);
break;
@ -4664,6 +4680,10 @@ uint64_t GCS_MAVLINK::capabilities() const
if (AP::rally()) {
ret |= MAV_PROTOCOL_CAPABILITY_MISSION_RALLY;
}
if (AP::fence()) {
// FIXME: plane also supports this...
ret |= MAV_PROTOCOL_CAPABILITY_MISSION_FENCE;
}
return ret;
}

1
libraries/GCS_MAVLink/GCS_Fence.cpp
View File

@ -69,4 +69,3 @@ void GCS_MAVLINK::send_fence_status() const
mavlink_breach_type,
fence->get_breach_time());
}

40
libraries/GCS_MAVLink/MissionItemProtocol.cpp
View File

@ -63,19 +63,28 @@ void MissionItemProtocol::handle_mission_count(
send_mission_ack(_link, msg, MAV_MISSION_DENIED);
return;
}
// the upload count may have changed; free resources and
// allocate them again:
free_upload_resources();
}
if (packet.count > max_items()) {
// FIXME: different items take up different storage space!
send_mission_ack(_link, msg, MAV_MISSION_NO_SPACE);
return;
}
MAV_MISSION_RESULT ret_alloc = allocate_receive_resources(packet.count);
if (ret_alloc != MAV_MISSION_ACCEPTED) {
send_mission_ack(_link, msg, ret_alloc);
return;
}
truncate(packet);
if (packet.count == 0) {
// no requests to send...
const MAV_MISSION_RESULT result = complete(_link);
send_mission_ack(_link, msg, result);
transfer_is_complete(_link, msg);
return;
}
@ -193,6 +202,12 @@ void MissionItemProtocol::handle_mission_write_partial_list(GCS_MAVLINK &_link,
return;
}
MAV_MISSION_RESULT ret_alloc = allocate_update_resources();
if (ret_alloc != MAV_MISSION_ACCEPTED) {
send_mission_ack(_link, msg, ret_alloc);
return;
}
init_send_requests(_link, msg, packet.start_index, packet.end_index);
}
@ -235,6 +250,7 @@ void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, cons
send_mission_ack(msg, result);
receiving = false;
link = nullptr;
free_upload_resources();
return;
}
@ -243,10 +259,7 @@ void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, cons
request_i++;
if (request_i > request_last) {
const MAV_MISSION_RESULT complete_result = complete(*link);
send_mission_ack(msg, complete_result);
receiving = false;
link = nullptr;
transfer_is_complete(*link, msg);
return;
}
// if we have enough space, then send the next WP request immediately
@ -257,6 +270,15 @@ void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, cons
}
}
void MissionItemProtocol::transfer_is_complete(const GCS_MAVLINK &_link, const mavlink_message_t &msg)
{
const MAV_MISSION_RESULT result = complete(_link);
send_mission_ack(_link, msg, result);
free_upload_resources();
receiving = false;
link = nullptr;
}
void MissionItemProtocol::send_mission_ack(const mavlink_message_t &msg,
MAV_MISSION_RESULT result) const
{
@ -317,7 +339,13 @@ void MissionItemProtocol::update()
if (tnow - timelast_receive_ms > upload_timeout_ms) {
receiving = false;
timeout();
mavlink_msg_mission_ack_send(link->get_chan(),
dest_sysid,
dest_compid,
MAV_MISSION_OPERATION_CANCELLED,
mission_type());
link = nullptr;
free_upload_resources();
return;
}
// resend request if we haven't gotten one:

17
libraries/GCS_MAVLink/MissionItemProtocol.h
View File

@ -96,6 +96,13 @@ private:
const mavlink_message_t &msg,
const int16_t _request_first,
const int16_t _request_last);
virtual MAV_MISSION_RESULT allocate_receive_resources(const uint16_t count) WARN_IF_UNUSED {
return MAV_MISSION_ACCEPTED;
}
virtual MAV_MISSION_RESULT allocate_update_resources() WARN_IF_UNUSED {
return MAV_MISSION_ACCEPTED;
}
virtual void free_upload_resources() { }
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;
@ -106,9 +113,19 @@ private:
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;
// complete - method called when transfer is complete - backends
// are expected to override this method to do any required tidy up.
virtual MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) {
return MAV_MISSION_ACCEPTED;
};
// transfer_is_complete - tidy up after a transfer is complete;
// this method will call complete() so the backends can do their
// bit.
void transfer_is_complete(const GCS_MAVLINK &_link, const mavlink_message_t &msg);
// timeout - called if the GCS fails to continue to supply items
// in a transfer. Backends are expected to tidy themselves up in
// this routine
virtual void timeout() {};
bool mavlink2_requirement_met(const GCS_MAVLINK &_link, const mavlink_message_t &msg) const;

211
libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp Normal file
View File

@ -0,0 +1,211 @@
#include "MissionItemProtocol_Fence.h"
#include <AC_Fence/AC_Fence.h>
MAV_MISSION_RESULT MissionItemProtocol_Fence::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)
{
const uint8_t num_stored_items = _fence.polyfence().num_stored_items();
if (packet.seq > num_stored_items) {
return MAV_MISSION_INVALID_SEQUENCE;
}
AC_PolyFenceItem fenceitem;
if (!_fence.polyfence().get_item(packet.seq, fenceitem)) {
return MAV_MISSION_ERROR;
}
MAV_CMD ret_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION; // initialised to avoid compiler warning
float p1 = 0;
switch (fenceitem.type) {
case AC_PolyFenceType::POLYGON_INCLUSION:
ret_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION;
p1 = fenceitem.vertex_count;
break;
case AC_PolyFenceType::POLYGON_EXCLUSION:
ret_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION;
p1 = fenceitem.vertex_count;
break;
case AC_PolyFenceType::RETURN_POINT:
ret_cmd = MAV_CMD_NAV_FENCE_RETURN_POINT;
break;
case AC_PolyFenceType::CIRCLE_EXCLUSION:
ret_cmd = MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION;
p1 = fenceitem.radius;
break;
case AC_PolyFenceType::CIRCLE_INCLUSION:
ret_cmd = MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION;
p1 = fenceitem.radius;
break;
case AC_PolyFenceType::END_OF_STORAGE:
return MAV_MISSION_ERROR;
}
ret_packet.command = ret_cmd;
ret_packet.param1 = p1;
ret_packet.x = fenceitem.loc.x;
ret_packet.y = fenceitem.loc.y;
ret_packet.z = 0;
return MAV_MISSION_ACCEPTED;
}
uint16_t MissionItemProtocol_Fence::item_count() const
{
if (receiving) {
return _new_items_count;
}
return _fence.polyfence().num_stored_items();
}
static MAV_MISSION_RESULT convert_MISSION_ITEM_INT_to_AC_PolyFenceItem(const mavlink_mission_item_int_t &mission_item_int, AC_PolyFenceItem &ret)
{
if (mission_item_int.frame != MAV_FRAME_GLOBAL &&
mission_item_int.frame != MAV_FRAME_GLOBAL_INT) {
return MAV_MISSION_UNSUPPORTED_FRAME;
}
switch (mission_item_int.command) {
case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION:
ret.type = AC_PolyFenceType::POLYGON_INCLUSION;
ret.vertex_count = mission_item_int.param1;
break;
case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION:
ret.type = AC_PolyFenceType::POLYGON_EXCLUSION;
ret.vertex_count = mission_item_int.param1;
break;
case MAV_CMD_NAV_FENCE_RETURN_POINT:
ret.type = AC_PolyFenceType::RETURN_POINT;
break;
case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION:
ret.type = AC_PolyFenceType::CIRCLE_EXCLUSION;
ret.radius = mission_item_int.param1;
break;
case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION:
ret.type = AC_PolyFenceType::CIRCLE_INCLUSION;
ret.radius = mission_item_int.param1;
break;
default:
return MAV_MISSION_UNSUPPORTED;
}
ret.loc.x = mission_item_int.x;
ret.loc.y = mission_item_int.y;
return MAV_MISSION_ACCEPTED;
}
MAV_MISSION_RESULT MissionItemProtocol_Fence::replace_item(const mavlink_mission_item_int_t &mission_item_int)
{
if (_new_items == nullptr) {
AP::internalerror().error(AP_InternalError::error_t::flow_of_control);
return MAV_MISSION_ERROR;
}
if (mission_item_int.seq >= _new_items_count) {
return MAV_MISSION_INVALID_SEQUENCE;
}
const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_AC_PolyFenceItem(mission_item_int, _new_items[mission_item_int.seq]);
if (ret != MAV_MISSION_ACCEPTED) {
return ret;
}
if (_updated_mask != nullptr) {
_updated_mask[mission_item_int.seq/8] |= (1U<<(mission_item_int.seq%8));
}
return MAV_MISSION_ACCEPTED;
}
MAV_MISSION_RESULT MissionItemProtocol_Fence::append_item(const mavlink_mission_item_int_t &mission_item_int)
{
return replace_item(mission_item_int);
}
void MissionItemProtocol_Fence::free_upload_resources()
{
free(_new_items);
_new_items = nullptr;
delete[] _updated_mask;
_updated_mask = nullptr;
}
MAV_MISSION_RESULT MissionItemProtocol_Fence::complete(const GCS_MAVLINK &_link)
{
if (_updated_mask != nullptr) {
// get any points that weren't filled in
for (uint16_t i=0; i<_new_items_count; i++) {
if (!(_updated_mask[i/8] & (1U<<(i%8)))) {
if (!_fence.polyfence().get_item(i, _new_items[i])) {
_link.send_text(MAV_SEVERITY_INFO, "Error replacing item (%u)", i);
return MAV_MISSION_ERROR;
}
}
}
}
bool success = _fence.polyfence().write_fence(_new_items, _new_items_count);
if (!success) {
return MAV_MISSION_ERROR;
}
// AP::logger().Write_Fence();
return MAV_MISSION_ACCEPTED;
}
void MissionItemProtocol_Fence::timeout()
{
link->send_text(MAV_SEVERITY_WARNING, "Fence upload timeout");
}
uint16_t MissionItemProtocol_Fence::max_items() const
{
return _fence.polyfence().max_items();
}
void MissionItemProtocol_Fence::truncate(const mavlink_mission_count_t &packet)
{
// FIXME: validate packet.count is same as allocated number of items
}
bool MissionItemProtocol_Fence::clear_all_items()
{
return _fence.polyfence().write_fence(nullptr, 0);
}
MAV_MISSION_RESULT MissionItemProtocol_Fence::allocate_receive_resources(const uint16_t count)
{
if (_new_items != nullptr) {
// this is an error - the base class should have called
// free_upload_resources first
AP::internalerror().error(AP_InternalError::error_t::flow_of_control);
return MAV_MISSION_ERROR;
}
const uint16_t allocation_size = count * sizeof(AC_PolyFenceItem);
gcs().send_text(MAV_SEVERITY_DEBUG, "Allocating %u bytes for fence upload", allocation_size);
if (allocation_size != 0) {
_new_items = (AC_PolyFenceItem*)malloc(allocation_size);
if (_new_items == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING, "Out of memory for upload");
return MAV_MISSION_ERROR;
}
}
_new_items_count = count;
return MAV_MISSION_ACCEPTED;
}
MAV_MISSION_RESULT MissionItemProtocol_Fence::allocate_update_resources()
{
const uint16_t _item_count = _fence.polyfence().num_stored_items();
_updated_mask = new uint8_t[(_item_count+7/8)];
if (_updated_mask == nullptr) {
return MAV_MISSION_ERROR;
}
MAV_MISSION_RESULT ret = allocate_receive_resources(_item_count);
if (ret != MAV_MISSION_ACCEPTED) {
delete[] _updated_mask;
_updated_mask = nullptr;
return ret;
}
_new_items_count = _item_count;
return MAV_MISSION_ACCEPTED;
}

48
libraries/GCS_MAVLink/MissionItemProtocol_Fence.h Normal file
View File

@ -0,0 +1,48 @@
#pragma once
#include "MissionItemProtocol.h"
class AC_PolyFence_loader;
class MissionItemProtocol_Fence : public MissionItemProtocol {
public:
MissionItemProtocol_Fence(class AC_Fence &fence) :
_fence(fence) {}
MAV_MISSION_TYPE mission_type() const override {
return MAV_MISSION_TYPE_FENCE;
}
void truncate(const mavlink_mission_count_t &packet) override;
MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) override;
void timeout() override;
protected:
ap_message next_item_ap_message_id() const override {
return MSG_NEXT_MISSION_REQUEST_FENCE;
}
bool clear_all_items() override WARN_IF_UNUSED;
private:
class AC_Fence &_fence;
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;
void free_upload_resources() override;
MAV_MISSION_RESULT allocate_receive_resources(const uint16_t count) override WARN_IF_UNUSED;
MAV_MISSION_RESULT allocate_update_resources() override WARN_IF_UNUSED;
class AC_PolyFenceItem *_new_items;
uint16_t _new_items_count;
uint8_t *_updated_mask;
};

1
libraries/GCS_MAVLink/ap_message.h
View File

@ -35,6 +35,7 @@ enum ap_message : uint8_t {
MSG_SERVO_OUT,
MSG_NEXT_MISSION_REQUEST_WAYPOINTS,
MSG_NEXT_MISSION_REQUEST_RALLY,
MSG_NEXT_MISSION_REQUEST_FENCE,
MSG_NEXT_PARAM,
MSG_FENCE_STATUS,
MSG_AHRS,