aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: add in-progress to formatsdcard process

Browse Source
This commit is contained in:
Peter Barker 2022-12-08 17:47:18 +11:00 committed by Peter Barker
parent d633bd3f49
commit 8b38cc671a
2 changed files with 51 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/GCS_MAVLink/GCS.h
View File

@ -157,11 +157,14 @@ public:
enum class Type {
NONE,
AIRSPEED_CAL,
SD_FORMAT,
};
// these can fail if there's no space on the channel to send the ack:
bool conclude(MAV_RESULT result);
bool send_in_progress();
// abort task without sending any further ACKs:
void abort() { task = Type::NONE; }
Type task;
MAV_CMD mav_cmd;
@ -511,6 +514,7 @@ protected:
virtual MAV_RESULT handle_command_component_arm_disarm(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_set_home(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_aux_function(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_storage_format(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
void handle_mission_request_list(const mavlink_message_t &msg);
void handle_mission_request(const mavlink_message_t &msg) const;
void handle_mission_request_int(const mavlink_message_t &msg) const;

56
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1313,8 +1313,26 @@ void GCS_MAVLINK_InProgress::check_tasks()
break;
}
#endif
}
break;
case Type::SD_FORMAT:
switch (AP::FS().get_format_status()) {
case AP_Filesystem_Backend::FormatStatus::NOT_STARTED:
// we shouldn't get here
task.conclude(MAV_RESULT_FAILED);
break;
case AP_Filesystem_Backend::FormatStatus::IN_PROGRESS:
case AP_Filesystem_Backend::FormatStatus::PENDING:
task.send_in_progress();
break;
case AP_Filesystem_Backend::FormatStatus::SUCCESS:
task.conclude(MAV_RESULT_ACCEPTED);
break;
case AP_Filesystem_Backend::FormatStatus::FAILURE:
task.conclude(MAV_RESULT_FAILED);
break;
}
break;
}
}
}
}
@ -5077,6 +5095,23 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const mavlink_command_long_t &
return handle_command_do_set_roi(roi_loc);
}
MAV_RESULT GCS_MAVLINK::handle_command_storage_format(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
{
if (!is_equal(packet.param1, 1.0f) ||
!is_equal(packet.param2, 1.0f)) {
return MAV_RESULT_UNSUPPORTED;
}
GCS_MAVLINK_InProgress *task = GCS_MAVLINK_InProgress::get_task(MAV_CMD_STORAGE_FORMAT, GCS_MAVLINK_InProgress::Type::SD_FORMAT, msg.sysid, msg.compid);
if (task == nullptr) {
return MAV_RESULT_TEMPORARILY_REJECTED;
}
if (!AP::FS().format()) {
task->abort();
return MAV_RESULT_FAILED;
}
return MAV_RESULT_IN_PROGRESS;
}
MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &packet)
{
switch (packet.command) {
@ -5087,13 +5122,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
return handle_command_do_set_roi_sysid(packet);
case MAV_CMD_DO_SET_HOME:
return handle_command_int_do_set_home(packet);
case MAV_CMD_STORAGE_FORMAT: {
if (!is_equal(packet.param1, 1.0f) ||
!is_equal(packet.param2, 1.0f)) {
return MAV_RESULT_UNSUPPORTED;
}
return AP::FS().format() ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
}
#if AP_SCRIPTING_ENABLED
case MAV_CMD_SCRIPTING:
@ -5119,7 +5147,17 @@ void GCS_MAVLINK::handle_command_int(const mavlink_message_t &msg)
hal.util->persistent_data.last_mavlink_cmd = packet.command;
const MAV_RESULT result = handle_command_int_packet(packet);
MAV_RESULT result;
// special handling of messages that need the mavlink_message_t
switch (packet.command) {
case MAV_CMD_STORAGE_FORMAT:
result = handle_command_storage_format(packet, msg);
break;
default:
result = handle_command_int_packet(packet);
break;
}
// send ACK or NAK
mavlink_msg_command_ack_send(chan, packet.command, result,