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

AP_FileSystem: permit upload of fence/rally via ftp

This commit is contained in:
Peter Barker 2024-01-23 15:10:33 +11:00
parent 5fa8bfb187
commit c6e7ba8131
2 changed files with 125 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

129
libraries/AP_Filesystem/AP_Filesystem_Mission.cpp
View File

@ -27,6 +27,7 @@
#include <AP_Rally/AP_Rally.h>
#include <GCS_MAVLink/MissionItemProtocol_Rally.h>
#include <GCS_MAVLink/MissionItemProtocol_Fence.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
extern int errno;
@ -211,10 +212,12 @@ int AP_Filesystem_Mission::stat(const char *name, struct stat *stbuf)
*/
bool AP_Filesystem_Mission::check_file_name(const char *name, enum MAV_MISSION_TYPE &mtype)
{
#if AP_MISSION_ENABLED
if (strcmp(name, "mission.dat") == 0) {
mtype = MAV_MISSION_TYPE_MISSION;
return true;
}
#endif
#if AP_FENCE_ENABLED
if (strcmp(name, "fence.dat") == 0) {
mtype = MAV_MISSION_TYPE_FENCE;
@ -261,6 +264,7 @@ bool AP_Filesystem_Mission::get_item(uint32_t idx, enum MAV_MISSION_TYPE mtype,
uint32_t AP_Filesystem_Mission::get_num_items(enum MAV_MISSION_TYPE mtype) const
{
switch (mtype) {
#if AP_MISSION_ENABLED
case MAV_MISSION_TYPE_MISSION: {
auto *mission = AP::mission();
if (!mission) {
@ -268,18 +272,17 @@ uint32_t AP_Filesystem_Mission::get_num_items(enum MAV_MISSION_TYPE mtype) const
}
return mission->num_commands();
}
case MAV_MISSION_TYPE_FENCE: {
#endif
#if AP_FENCE_ENABLED
case MAV_MISSION_TYPE_FENCE: {
auto *fence = AP::fence();
if (fence == nullptr) {
return 0;
}
return fence->polyfence().num_stored_items();
#else
return 0;
#endif
}
#endif
#if HAL_RALLY_ENABLED
case MAV_MISSION_TYPE_RALLY: {
@ -381,6 +384,30 @@ bool AP_Filesystem_Mission::finish_upload(const rfile &r)
}
}
switch (r.mtype) {
#if AP_MISSION_ENABLED
case MAV_MISSION_TYPE_MISSION:
return finish_upload_mission(hdr, r, b);
#endif
#if HAL_RALLY_ENABLED
case MAV_MISSION_TYPE_RALLY:
return finish_upload_rally(hdr, r, b);
#endif
#if AP_FENCE_ENABLED
case MAV_MISSION_TYPE_FENCE:
return finish_upload_fence(hdr, r, b);
#endif
default:
// really should not get here....
break;
}
return false;
}
#if AP_MISSION_ENABLED
bool AP_Filesystem_Mission::finish_upload_mission(const struct header &hdr, const rfile &r, const uint8_t *b)
{
auto *mission = AP::mission();
if (mission == nullptr) {
return false;
@ -389,16 +416,17 @@ bool AP_Filesystem_Mission::finish_upload(const rfile &r)
if ((hdr.options & unsigned(Options::NO_CLEAR)) == 0) {
mission->clear();
}
for (uint32_t i=0; i<nitems; i++) {
for (uint32_t i=0; i<hdr.num_items; i++) {
mavlink_mission_item_int_t m {};
AP_Mission::Mission_Command cmd;
const uint8_t item_size = MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN;
memcpy(&m, &b[sizeof(hdr)+i*item_size], item_size);
const MAV_MISSION_RESULT res = AP_Mission::mavlink_int_to_mission_cmd(m, cmd);
if (res != MAV_MISSION_ACCEPTED) {
return false;
}
if (cmd.id == MAV_CMD_DO_JUMP &&
(cmd.content.jump.target >= nitems || cmd.content.jump.target == 0)) {
(cmd.content.jump.target >= hdr.num_items || cmd.content.jump.target == 0)) {
return false;
}
uint16_t idx = i + hdr.start;
@ -414,5 +442,92 @@ bool AP_Filesystem_Mission::finish_upload(const rfile &r)
}
return true;
}
#endif // AP_MISSION_ENABLED
#if AP_FENCE_ENABLED
bool AP_Filesystem_Mission::finish_upload_fence(const struct header &hdr, const rfile &r, const uint8_t *b)
{
bool success = false;
AC_PolyFenceItem *new_items = nullptr;
auto *fence = AP::fence();
if (fence == nullptr) {
goto OUT;
}
if ((hdr.options & unsigned(Options::NO_CLEAR)) != 0) {
// Only complete fences can be uploaded for now.
goto OUT;
}
// passing nullptr and 0 items through to Polyfence loader is
// absolutely OK:
if (hdr.num_items != 0) {
new_items = new AC_PolyFenceItem[hdr.num_items];
if (new_items == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Out of memory for upload");
goto OUT;
}
}
// convert from MISSION_ITEM_INT to AC_PolyFenceItem:
for (uint32_t i=0; i<hdr.num_items; i++) {
mavlink_mission_item_int_t m {};
const uint8_t item_size = MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN;
memcpy(&m, &b[sizeof(hdr)+i*item_size], item_size);
const MAV_MISSION_RESULT res = MissionItemProtocol_Fence::convert_MISSION_ITEM_INT_to_AC_PolyFenceItem(m, new_items[i]);
if (res != MAV_MISSION_ACCEPTED) {
goto OUT;
}
}
success = fence->polyfence().write_fence(new_items, hdr.num_items);
OUT:
delete[] new_items;
return success;
}
#endif // AP_FENCE_ENABLED
#if HAL_RALLY_ENABLED
bool AP_Filesystem_Mission::finish_upload_rally(const struct header &hdr, const rfile &r, const uint8_t *b)
{
bool success = false;
auto *rally = AP::rally();
if (rally == nullptr) {
goto OUT;
}
if ((hdr.options & unsigned(Options::NO_CLEAR)) != 0) {
//only complete sets of rally points can be added ATM
goto OUT;
}
rally->truncate(0);
for (uint32_t i=0; i<hdr.num_items; i++) {
mavlink_mission_item_int_t m {};
RallyLocation cmd;
const uint8_t item_size = MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN;
memcpy(&m, &b[sizeof(hdr)+i*item_size], item_size);
const MAV_MISSION_RESULT res = MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyLocation(m, cmd);
if (res != MAV_MISSION_ACCEPTED) {
goto OUT;
}
if (!rally->append(cmd)) {
goto OUT;
}
}
success = true;
OUT:
return success;
}
#endif // HAL_RALLY_ENABLED
#endif // AP_FILESYSTEM_MISSION_ENABLED

3
libraries/AP_Filesystem/AP_Filesystem_Mission.h
View File

@ -72,6 +72,9 @@ private:
// finish loading items
bool finish_upload(const rfile &r);
bool finish_upload_mission(const struct header &hdr, const rfile &r, const uint8_t *b);
bool finish_upload_fence(const struct header &hdr, const rfile &r, const uint8_t *b);
bool finish_upload_rally(const struct header &hdr, const rfile &r, const uint8_t *b);
// see if a block of memory is all zero
bool all_zero(const uint8_t *b, uint8_t size) const;