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: handle auto-tuning command

Browse Source
This commit is contained in:
bresch 2021-09-29 14:36:29 +02:00 committed by Daniel Agar
parent cd61065aea
commit d463dfe2d8
2 changed files with 108 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -576,6 +576,112 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
}
}
} else if (cmd_mavlink.command == MAV_CMD_DO_AUTOTUNE_ENABLE) {
bool has_module = true;
autotune_attitude_control_status_s status{};
_autotune_attitude_control_status_sub.copy(&status);
// if not busy enable via the parameter
// do not check the return value of the uORB copy above because the module
// starts publishing only when MC_AT_START is set
if (status.state == autotune_attitude_control_status_s::STATE_IDLE) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
if (!vehicle_status.in_transition_mode) {
param_t atune_start;
switch (vehicle_status.vehicle_type) {
case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
/* atune_start = param_find("FW_AT_START"); */
atune_start = PARAM_INVALID;
break;
case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
atune_start = param_find("MC_AT_START");
break;
default:
atune_start = PARAM_INVALID;
break;
}
if (atune_start == PARAM_INVALID) {
has_module = false;
} else {
int32_t start = 1;
param_set(atune_start, &start);
}
} else {
has_module = false;
}
}
if (has_module) {
// most are in progress
result = vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS;
switch (status.state) {
case autotune_attitude_control_status_s::STATE_IDLE:
case autotune_attitude_control_status_s::STATE_INIT:
/* progress = 0; */
break;
case autotune_attitude_control_status_s::STATE_ROLL:
case autotune_attitude_control_status_s::STATE_ROLL_PAUSE:
/* progress = 20; */
break;
case autotune_attitude_control_status_s::STATE_PITCH:
case autotune_attitude_control_status_s::STATE_PITCH_PAUSE:
/* progress = 40; */
break;
case autotune_attitude_control_status_s::STATE_YAW:
case autotune_attitude_control_status_s::STATE_YAW_PAUSE:
/* progress = 60; */
break;
case autotune_attitude_control_status_s::STATE_VERIFICATION:
/* progress = 80; */
break;
case autotune_attitude_control_status_s::STATE_APPLY:
/* progress = 85; */
break;
case autotune_attitude_control_status_s::STATE_TEST:
/* progress = 90; */
break;
case autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM:
/* progress = 95; */
break;
case autotune_attitude_control_status_s::STATE_COMPLETE:
/* 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; */
result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
break;
}
} else {
result = vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
}
send_ack = true;
} else {
send_ack = false;

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

@ -62,6 +62,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/autotune_attitude_control_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/camera_status.h>
#include <uORB/topics/cellular_status.h>
@ -346,6 +347,7 @@ private:
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)};
uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};