fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink_receiver: Add mavlink handler with progress report

Browse Source
This commit is contained in:
bresch 2021-10-01 09:59:02 +02:00 committed by Daniel Agar
parent d463dfe2d8
commit 61dabca4c8
2 changed files with 15 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -97,7 +97,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
}
void
MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result)
MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result, uint8_t progress)
{
vehicle_command_ack_s command_ack{};
@ -106,6 +106,7 @@ MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, ui
command_ack.result = result;
command_ack.target_system = sysid;
command_ack.target_component = compid;
command_ack.result_param1 = progress;
_cmd_ack_pub.publish(command_ack);
}
@ -469,6 +470,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
bool send_ack = true;
uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
uint8_t progress = 0; // TODO: should be 255, 0 for backwards compatibility
if (!target_ok) {
// Reject alien commands only if there is no forwarding or we've never seen target component before
@ -630,48 +632,48 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
switch (status.state) {
case autotune_attitude_control_status_s::STATE_IDLE:
case autotune_attitude_control_status_s::STATE_INIT:
/* progress = 0; */
progress = 0;
break;
case autotune_attitude_control_status_s::STATE_ROLL:
case autotune_attitude_control_status_s::STATE_ROLL_PAUSE:
/* progress = 20; */
progress = 20;
break;
case autotune_attitude_control_status_s::STATE_PITCH:
case autotune_attitude_control_status_s::STATE_PITCH_PAUSE:
/* progress = 40; */
progress = 40;
break;
case autotune_attitude_control_status_s::STATE_YAW:
case autotune_attitude_control_status_s::STATE_YAW_PAUSE:
/* progress = 60; */
progress = 60;
break;
case autotune_attitude_control_status_s::STATE_VERIFICATION:
/* progress = 80; */
progress = 80;
break;
case autotune_attitude_control_status_s::STATE_APPLY:
/* progress = 85; */
progress = 85;
break;
case autotune_attitude_control_status_s::STATE_TEST:
/* progress = 90; */
progress = 90;
break;
case autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM:
/* progress = 95; */
progress = 95;
break;
case autotune_attitude_control_status_s::STATE_COMPLETE:
/* progress = 100; */
progress = 100;
// ack it properly with an ACCEPTED once we're done
result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
break;
case autotune_attitude_control_status_s::STATE_FAIL:
/* progress = 0; */
progress = 0;
result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
break;
}
@ -716,7 +718,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
}
if (send_ack) {
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, result);
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, result, progress);
}
}

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -135,7 +135,7 @@ private:
static void *start_trampoline(void *context);
void run();
void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result);
void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result, uint8_t progress = 0);
/**
* Common method to handle both mavlink command types. T is one of mavlink_command_int_t or mavlink_command_long_t.