From 86cddc6a52e20a51d6f9402b8fbfc0b9d65f98d0 Mon Sep 17 00:00:00 2001 From: chris1seto Date: Fri, 23 Sep 2022 16:17:51 -0700 Subject: [PATCH] fw_autotune_attitude_control : Add better support for RC TX control while in FW AT (#20069) Co-authored-by: Chris Seto --- .../fw_autotune_attitude_control.cpp | 138 ++++++++++++------ .../fw_autotune_attitude_control.hpp | 12 +- .../fw_autotune_attitude_control_params.c | 18 +++ 3 files changed, 117 insertions(+), 51 deletions(-) diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp index 9f67eb2f49..ffef587b27 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -65,6 +65,11 @@ bool FwAutotuneAttitudeControl::init() _signal_filter.setParameters(_publishing_dt_s, .2f); // runs in the slow publishing loop + if (!_actuator_controls_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + return true; } @@ -92,9 +97,12 @@ void FwAutotuneAttitudeControl::Run() updateStateMachine(hrt_absolute_time()); } + _aux_switch_en = isAuxEnableSwitchEnabled(); + // new control data needed every iteration - if (_state == state::idle + if ((_state == state::idle && !_aux_switch_en) || !_actuator_controls_sub.updated()) { + return; } @@ -103,6 +111,7 @@ void FwAutotuneAttitudeControl::Run() if (_vehicle_status_sub.copy(&vehicle_status)) { _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + _nav_state = vehicle_status.nav_state; } } @@ -230,6 +239,48 @@ void FwAutotuneAttitudeControl::checkFilters() } } +bool FwAutotuneAttitudeControl::isAuxEnableSwitchEnabled() +{ + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + + float aux_enable_channel = 0; + + switch (_param_fw_at_man_aux.get()) { + case 0: + return false; + + case 1: + aux_enable_channel = manual_control_setpoint.aux1; + break; + + case 2: + aux_enable_channel = manual_control_setpoint.aux2; + break; + + case 3: + aux_enable_channel = manual_control_setpoint.aux3; + break; + + case 4: + aux_enable_channel = manual_control_setpoint.aux4; + break; + + case 5: + aux_enable_channel = manual_control_setpoint.aux5; + break; + + case 6: + aux_enable_channel = manual_control_setpoint.aux6; + break; + + default: + return false; + } + + return aux_enable_channel > .5f; +} + void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) { // when identifying an axis, check if the estimate has converged @@ -240,15 +291,12 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) switch (_state) { case state::idle: - if (_param_fw_at_start.get()) { - if (registerActuatorControlsCallback()) { - _state = state::init; - - } else { - _state = state::fail; - } + if (_param_fw_at_start.get() || _aux_switch_en) { + mavlink_log_info(&_mavlink_log_pub, "Autotune started"); + _state = state::init; _state_start_time = now; + _start_flight_mode = _nav_state; } break; @@ -361,20 +409,23 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) _state_start_time = now; break; - case state::apply: - if ((_param_fw_at_apply.get() == 1)) { - _state = state::wait_for_disarm; + case state::apply: { + mavlink_log_info(&_mavlink_log_pub, "Autotune finished successfully"); - } else if (_param_fw_at_apply.get() == 2) { - backupAndSaveGainsToParams(); - _state = state::test; + if ((_param_fw_at_apply.get() == 1)) { + _state = state::wait_for_disarm; - } else { - _state = state::complete; + } else if (_param_fw_at_apply.get() == 2) { + backupAndSaveGainsToParams(); + _state = state::test; + + } else { + _state = state::complete; + } + + _state_start_time = now; } - _state_start_time = now; - break; case state::wait_for_disarm: @@ -409,38 +460,36 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) // Wait a bit in that state to make sure // the other components are aware of the final result if ((now - _state_start_time) > 2_s) { + + // Don't reset until aux switch is back to disabled + if (_param_fw_at_man_aux.get() && _aux_switch_en) { + break; + } + + orb_advert_t mavlink_log_pub = nullptr; + mavlink_log_info(&mavlink_log_pub, "Autotune returned to idle"); _state = state::idle; - stopAutotune(); + _param_fw_at_start.set(false); + _param_fw_at_start.commit(); } break; } - // In case of convergence timeout or pilot intervention, + // In case of convergence timeout // the identification sequence is aborted immediately - manual_control_setpoint_s manual_control_setpoint{}; - _manual_control_setpoint_sub.copy(&manual_control_setpoint); - - if (_state != state::wait_for_disarm - && _state != state::idle - && (((now - _state_start_time) > 20_s) - || (fabsf(manual_control_setpoint.x) > 0.2f) - || (fabsf(manual_control_setpoint.y) > 0.2f))) { - _state = state::fail; - _state_start_time = now; + if (_state != state::wait_for_disarm && _state != state::idle && _state != state::fail && _state != state::complete) { + if (now - _state_start_time > 20_s + || (_param_fw_at_man_aux.get() && !_aux_switch_en) + || _start_flight_mode != _nav_state) { + orb_advert_t mavlink_log_pub = nullptr; + mavlink_log_critical(&mavlink_log_pub, "Autotune aborted before finishing"); + _state = state::fail; + _state_start_time = now; + } } } -bool FwAutotuneAttitudeControl::registerActuatorControlsCallback() -{ - if (!_actuator_controls_sub.registerCallback()) { - PX4_ERR("callback registration failed"); - return false; - } - - return true; -} - void FwAutotuneAttitudeControl::copyGains(int index) { if (index <= 2) { @@ -564,13 +613,6 @@ void FwAutotuneAttitudeControl::saveGainsToParams() } } -void FwAutotuneAttitudeControl::stopAutotune() -{ - _param_fw_at_start.set(false); - _param_fw_at_start.commit(); - _actuator_controls_sub.unregisterCallback(); -} - const Vector3f FwAutotuneAttitudeControl::getIdentificationSignal() { if (_steps_counter > _max_steps) { diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp index 10fa470658..efe7159973 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -60,6 +60,7 @@ #include #include #include +#include using namespace time_literals; @@ -98,13 +99,12 @@ private: void checkFilters(); void updateStateMachine(hrt_abstime now); - bool registerActuatorControlsCallback(); - void stopAutotune(); void copyGains(int index); bool areGainsGood() const; void saveGainsToParams(); void backupAndSaveGainsToParams(); void revertParamGains(); + bool isAuxEnableSwitchEnabled(); const matrix::Vector3f getIdentificationSignal(); @@ -143,6 +143,11 @@ private: int8_t _signal_sign{0}; bool _armed{false}; + uint8_t _nav_state{0}; + uint8_t _start_flight_mode{0}; + bool _aux_switch_en{false}; + + orb_advert_t _mavlink_log_pub{nullptr}; matrix::Vector3f _kiff{}; matrix::Vector3f _rate_k{}; @@ -179,6 +184,7 @@ private: DEFINE_PARAMETERS( (ParamInt) _param_fw_at_axes, (ParamBool) _param_fw_at_start, + (ParamInt) _param_fw_at_man_aux, (ParamFloat) _param_fw_at_sysid_amp, (ParamInt) _param_fw_at_apply, diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c index 93421a0fcc..21a0d4e6db 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c @@ -103,3 +103,21 @@ PARAM_DEFINE_INT32(FW_AT_APPLY, 2); * @group Autotune */ PARAM_DEFINE_INT32(FW_AT_AXES, 3); + +/** + * Enable auto tuning enable on aux input + * + * Defines which aux input to enable auto tuning + * + * @value 0 Disable + * @value 1 Aux1 + * @value 2 Aux2 + * @value 3 Aux3 + * @value 4 Aux4 + * @value 5 Aux5 + * @value 6 Aux6 + * @min 0 + * @max 6 + * @group Autotune + */ +PARAM_DEFINE_INT32(FW_AT_MAN_AUX, 0);