Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: add support for mavlink in-progress message

Browse Source
This commit is contained in:
Peter Barker 2022-12-08 17:47:13 +11:00 committed by Peter Barker
parent d7ec1e7b0c
commit 11ec22900b
2 changed files with 159 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/GCS_MAVLink/GCS.h
View File

@ -151,6 +151,39 @@ private:
};
#endif
class GCS_MAVLINK_InProgress
{
public:
enum class Type {
NONE,
AIRSPEED_CAL,
};
// these can fail if there's no space on the channel to send the ack:
bool conclude(MAV_RESULT result);
bool send_in_progress();
Type task;
MAV_CMD mav_cmd;
static class GCS_MAVLINK_InProgress *get_task(MAV_CMD cmd, Type t, uint8_t sysid, uint8_t compid);
static void check_tasks();
private:
uint8_t requesting_sysid;
uint8_t requesting_compid;
mavlink_channel_t chan;
bool send_ack(MAV_RESULT result);
static GCS_MAVLINK_InProgress in_progress_tasks[1];
// allocate a task-tracking ID
static uint32_t last_check_ms;
};
///
/// @class GCS_MAVLINK
/// @brief MAVLink transport control class
@ -571,10 +604,10 @@ protected:
// generally this should not be overridden; Plane overrides it to ensure
// failsafe isn't triggered during calibration
virtual MAV_RESULT handle_command_preflight_calibration(const mavlink_command_long_t &packet);
virtual MAV_RESULT handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
virtual MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet);
virtual MAV_RESULT _handle_command_preflight_calibration_baro();
virtual MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
virtual MAV_RESULT _handle_command_preflight_calibration_baro(const mavlink_message_t &msg);
MAV_RESULT handle_command_preflight_can(const mavlink_command_long_t &packet);

134
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -105,6 +105,9 @@ uint8_t GCS_MAVLINK::mavlink_private = 0;
GCS *GCS::_singleton = nullptr;
GCS_MAVLINK_InProgress GCS_MAVLINK_InProgress::in_progress_tasks[1];
uint32_t GCS_MAVLINK_InProgress::last_check_ms;
GCS_MAVLINK::GCS_MAVLINK(GCS_MAVLINK_Parameters &parameters,
AP_HAL::UARTDriver &uart)
{
@ -1214,6 +1217,108 @@ int8_t GCS_MAVLINK::deferred_message_to_send_index(uint16_t now16_ms)
return next_deferred_message_to_send_cache;
}
bool GCS_MAVLINK_InProgress::send_ack(MAV_RESULT result)
{
if (!HAVE_PAYLOAD_SPACE(chan, COMMAND_ACK)) {
// can't fit the ACK, come back to this later
return false;
}
mavlink_msg_command_ack_send(
chan,
mav_cmd,
result,
0,
0,
requesting_sysid,
requesting_compid
);
return true;
}
bool GCS_MAVLINK_InProgress::send_in_progress()
{
return send_ack(MAV_RESULT_IN_PROGRESS);
}
bool GCS_MAVLINK_InProgress::conclude(MAV_RESULT result)
{
if (!send_ack(result)) {
return false;
}
task = Type::NONE;
return true;
}
GCS_MAVLINK_InProgress *GCS_MAVLINK_InProgress::get_task(MAV_CMD mav_cmd, GCS_MAVLINK_InProgress::Type t, uint8_t sysid, uint8_t compid)
{
// we can't have two outstanding tasks for the same command from
// the same mavlink node or the result is ambiguous:
for (auto &_task : in_progress_tasks) {
if (_task.task == Type::NONE) {
continue;
}
if (_task.mav_cmd == mav_cmd &&
_task.requesting_sysid == sysid &&
_task.requesting_compid == compid) {
return nullptr;
}
}
for (auto &_task : in_progress_tasks) {
if (_task.task != Type::NONE) {
continue;
}
_task.task = t;
_task.mav_cmd = mav_cmd;
_task.requesting_sysid = sysid;
_task.requesting_compid = compid;
return &_task;
}
return nullptr;
}
void GCS_MAVLINK_InProgress::check_tasks()
{
// run these checks only intermittently (rate-limits the
// MAV_RESULT_IN_PROGRESS messages):
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - last_check_ms < 1000) {
return;
}
last_check_ms = now_ms;
for (auto &task : in_progress_tasks) {
switch (task.task) {
case Type::NONE:
break;
case Type::AIRSPEED_CAL: {
#if AP_AIRSPEED_ENABLED
const AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
switch (airspeed->get_calibration_state()) {
case AP_Airspeed::CalibrationState::NOT_STARTED:
// we shouldn't get here
task.conclude(MAV_RESULT_FAILED);
break;
case AP_Airspeed::CalibrationState::IN_PROGRESS:
task.send_in_progress();
break;
case AP_Airspeed::CalibrationState::FAILED:
task.conclude(MAV_RESULT_FAILED);
break;
case AP_Airspeed::CalibrationState::SUCCESS:
task.conclude(MAV_RESULT_ACCEPTED);
break;
}
#endif
break;
}
}
}
}
void GCS_MAVLINK::update_send()
{
#if !defined(HAL_BUILD_AP_PERIPH) || HAL_LOGGING_ENABLED
@ -1235,6 +1340,9 @@ void GCS_MAVLINK::update_send()
uint32_t retry_deferred_body_start = AP_HAL::micros();
#endif
// check for any in-progress tasks; check_tasks does its own rate-limiting
GCS_MAVLINK_InProgress::check_tasks();
const uint32_t start = AP_HAL::millis();
const uint16_t start16 = start & 0xFFFF;
while (AP_HAL::millis() - start < 5) { // spend a max of 5ms sending messages. This should never trigger - out_of_time() should become true
@ -4122,7 +4230,7 @@ bool GCS_MAVLINK::calibrate_gyros()
#endif
}
MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration_baro()
MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration_baro(const mavlink_message_t &msg)
{
// fast barometer calibration
gcs().send_text(MAV_SEVERITY_INFO, "Updating barometer calibration");
@ -4130,16 +4238,21 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration_baro()
gcs().send_text(MAV_SEVERITY_INFO, "Barometer calibration complete");
#if AP_AIRSPEED_ENABLED
AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
if (airspeed != nullptr) {
GCS_MAVLINK_InProgress *task = GCS_MAVLINK_InProgress::get_task(MAV_CMD_PREFLIGHT_CALIBRATION, GCS_MAVLINK_InProgress::Type::AIRSPEED_CAL, msg.sysid, msg.compid);
if (task == nullptr) {
return MAV_RESULT_TEMPORARILY_REJECTED;
}
airspeed->calibrate(false);
}
#endif
return MAV_RESULT_ACCEPTED;
return MAV_RESULT_IN_PROGRESS;
}
MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
{
MAV_RESULT ret = MAV_RESULT_UNSUPPORTED;
@ -4152,7 +4265,7 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm
}
if (is_equal(packet.param3,1.0f)) {
return _handle_command_preflight_calibration_baro();
return _handle_command_preflight_calibration_baro(msg);
}
rc().calibrating(is_positive(packet.param4));
@ -4224,7 +4337,7 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm
return ret;
}
MAV_RESULT GCS_MAVLINK::handle_command_preflight_calibration(const mavlink_command_long_t &packet)
MAV_RESULT GCS_MAVLINK::handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
{
if (hal.util->get_soft_armed()) {
// *preflight*, remember?
@ -4232,7 +4345,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_calibration(const mavlink_comma
return MAV_RESULT_FAILED;
}
// now call subclass methods:
return _handle_command_preflight_calibration(packet);
return _handle_command_preflight_calibration(packet, msg);
}
MAV_RESULT GCS_MAVLINK::handle_command_run_prearm_checks(const mavlink_command_long_t &packet)
@ -4629,11 +4742,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
result = handle_command_do_set_roi(packet);
break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
result = handle_command_preflight_calibration(packet);
break;
case MAV_CMD_DO_SET_MISSION_CURRENT:
result = handle_command_do_set_mission_current(packet);
break;
@ -4823,6 +4931,10 @@ void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg)
result = handle_preflight_reboot(packet, msg);
break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
result = handle_command_preflight_calibration(packet, msg);
break;
default:
result = handle_command_long_packet(packet);
break;