Browse Source
GCS_MAVLink: add support for fence inclusion circles GCS_MAVLink: factor out a transfer_is_complete; start commenting properlymission-4.1.18
Peter Barker
6 years ago
committed by
Randy Mackay
9 changed files with 336 additions and 8 deletions
@ -0,0 +1,211 @@
@@ -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; |
||||
} |
||||
@ -0,0 +1,48 @@
@@ -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; |
||||
}; |
||||
Loading…
Reference in new issue