mirror of https://github.com/ArduPilot/ardupilot
AP_FileSystem: permit upload of fence/rally via ftp
This commit is contained in:
parent
809a76d419
commit
95046e617e
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue