Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: expose functions to convert fence and rally to mavlink

Browse Source 
this allows for the @MISSION filesystem to download data in the
current mavlink format
This commit is contained in:
Andrew Tridgell 2021-04-05 12:45:16 +10:00
parent 0ee0228883
commit c53c112691
4 changed files with 62 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp
View File

@ -2,20 +2,25 @@
#include <AC_Fence/AC_Fence.h> #include <AC_Fence/AC_Fence.h>
MAV_MISSION_RESULT MissionItemProtocol_Fence::get_item(const GCS_MAVLINK &_link, /*
const mavlink_message_t &msg, public function to format mission item as mavlink_mission_item_int_t
const mavlink_mission_request_int_t &packet, */
mavlink_mission_item_int_t &ret_packet) bool MissionItemProtocol_Fence::get_item_as_mission_item(uint16_t seq,
mavlink_mission_item_int_t &ret_packet)
{ {
const uint8_t num_stored_items = _fence.polyfence().num_stored_items(); AC_Fence *fence = AP::fence();
if (packet.seq > num_stored_items) { if (fence == nullptr) {
return MAV_MISSION_INVALID_SEQUENCE; return false;
}
const uint8_t num_stored_items = fence->polyfence().num_stored_items();
if (seq > num_stored_items) {
return false;
} }
AC_PolyFenceItem fenceitem; AC_PolyFenceItem fenceitem;
if (!_fence.polyfence().get_item(packet.seq, fenceitem)) { if (!fence->polyfence().get_item(seq, fenceitem)) {
return MAV_MISSION_ERROR; return false;
} }
MAV_CMD ret_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION; // initialised to avoid compiler warning MAV_CMD ret_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION; // initialised to avoid compiler warning
@ -41,7 +46,7 @@ MAV_MISSION_RESULT MissionItemProtocol_Fence::get_item(const GCS_MAVLINK &_link,
p1 = fenceitem.radius; p1 = fenceitem.radius;
break; break;
case AC_PolyFenceType::END_OF_STORAGE: case AC_PolyFenceType::END_OF_STORAGE:
return MAV_MISSION_ERROR; return false;
} }
ret_packet.command = ret_cmd; ret_packet.command = ret_cmd;
@ -50,6 +55,23 @@ MAV_MISSION_RESULT MissionItemProtocol_Fence::get_item(const GCS_MAVLINK &_link,
ret_packet.y = fenceitem.loc.y; ret_packet.y = fenceitem.loc.y;
ret_packet.z = 0; ret_packet.z = 0;
return true;
}
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;
}
if (!get_item_as_mission_item(packet.seq, ret_packet)) {
return MAV_MISSION_ERROR;
}
return MAV_MISSION_ACCEPTED; return MAV_MISSION_ACCEPTED;
} }

5
libraries/GCS_MAVLink/MissionItemProtocol_Fence.h
View File

@ -17,6 +17,11 @@ public:
MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) override; MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) override;
void timeout() override; void timeout() override;
/*
static function to format mission item as mavlink_mission_item_int_t
*/
static bool get_item_as_mission_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet);
protected: protected:
ap_message next_item_ap_message_id() const override { ap_message next_item_ap_message_id() const override {

26
libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp
View File

@ -73,15 +73,18 @@ MAV_MISSION_RESULT MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyL
return MAV_MISSION_ACCEPTED; return MAV_MISSION_ACCEPTED;
} }
MAV_MISSION_RESULT MissionItemProtocol_Rally::get_item(const GCS_MAVLINK &_link, /*
const mavlink_message_t &msg, static function to get rally item as mavlink_mission_item_int_t
const mavlink_mission_request_int_t &packet, */
mavlink_mission_item_int_t &ret_packet) bool MissionItemProtocol_Rally::get_item_as_mission_item(uint16_t seq,
mavlink_mission_item_int_t &ret_packet)
{ {
auto *rallyp = AP::rally();
RallyLocation rallypoint; RallyLocation rallypoint;
if (!rally.get_rally_point_with_index(packet.seq, rallypoint)) { if (rallyp == nullptr || !rallyp->get_rally_point_with_index(seq, rallypoint)) {
return MAV_MISSION_INVALID_SEQUENCE; return false;
} }
ret_packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; ret_packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
@ -90,6 +93,17 @@ MAV_MISSION_RESULT MissionItemProtocol_Rally::get_item(const GCS_MAVLINK &_link,
ret_packet.y = rallypoint.lng; ret_packet.y = rallypoint.lng;
ret_packet.z = rallypoint.alt; ret_packet.z = rallypoint.alt;
return true;
}
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)
{
if (!get_item_as_mission_item(packet.seq, ret_packet)) {
return MAV_MISSION_INVALID_SEQUENCE;
}
return MAV_MISSION_ACCEPTED; return MAV_MISSION_ACCEPTED;
} }

5
libraries/GCS_MAVLink/MissionItemProtocol_Rally.h
View File

@ -12,6 +12,11 @@ public:
MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) override; MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) override;
void timeout() override; void timeout() override;
/*
static function to get rally item as mavlink_mission_item_int_t
*/
static bool get_item_as_mission_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet);
protected: protected:
ap_message next_item_ap_message_id() const override { ap_message next_item_ap_message_id() const override {