From 70109f968c48f7a12f3621ec27ab5043367b193e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 29 Aug 2023 15:17:09 +1000 Subject: [PATCH] AC_AttitudeControl: tidy AC_PID construction --- .../AC_AttitudeControl_Multi.cpp | 6 +-- .../AC_AttitudeControl_Multi.h | 46 +++++++++++++++++-- 2 files changed, 45 insertions(+), 7 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index d17b2ec17d..174ce28078 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -1,6 +1,7 @@ #include "AC_AttitudeControl_Multi.h" #include #include +#include // table of user settable parameters const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { @@ -244,10 +245,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors) : AC_AttitudeControl(ahrs, aparm, motors), - _motors_multi(motors), - _pid_rate_roll(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ), - _pid_rate_pitch(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ), - _pid_rate_yaw(AC_ATC_MULTI_RATE_YAW_P, AC_ATC_MULTI_RATE_YAW_I, AC_ATC_MULTI_RATE_YAW_D, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, AC_ATC_MULTI_RATE_YAW_FILT_HZ, 0.0f) + _motors_multi(motors) { AP_Param::setup_object_defaults(this, var_info); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 883c6193bf..eb2ad45791 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -93,9 +93,49 @@ protected: float get_throttle_avg_max(float throttle_in); AP_MotorsMulticopter& _motors_multi; - AC_PID _pid_rate_roll; - AC_PID _pid_rate_pitch; - AC_PID _pid_rate_yaw; + AC_PID _pid_rate_roll { + AC_PID::Defaults{ + .p = AC_ATC_MULTI_RATE_RP_P, + .i = AC_ATC_MULTI_RATE_RP_I, + .d = AC_ATC_MULTI_RATE_RP_D, + .ff = 0.0f, + .imax = AC_ATC_MULTI_RATE_RP_IMAX, + .filt_T_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .filt_E_hz = 0.0f, + .filt_D_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .srmax = 0, + .srtau = 1.0 + } + }; + AC_PID _pid_rate_pitch{ + AC_PID::Defaults{ + .p = AC_ATC_MULTI_RATE_RP_P, + .i = AC_ATC_MULTI_RATE_RP_I, + .d = AC_ATC_MULTI_RATE_RP_D, + .ff = 0.0f, + .imax = AC_ATC_MULTI_RATE_RP_IMAX, + .filt_T_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .filt_E_hz = 0.0f, + .filt_D_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .srmax = 0, + .srtau = 1.0 + } + }; + + AC_PID _pid_rate_yaw{ + AC_PID::Defaults{ + .p = AC_ATC_MULTI_RATE_YAW_P, + .i = AC_ATC_MULTI_RATE_YAW_I, + .d = AC_ATC_MULTI_RATE_YAW_D, + .ff = 0.0f, + .imax = AC_ATC_MULTI_RATE_YAW_IMAX, + .filt_T_hz = AC_ATC_MULTI_RATE_RP_FILT_HZ, + .filt_E_hz = AC_ATC_MULTI_RATE_YAW_FILT_HZ, + .filt_D_hz = 0.0f, + .srmax = 0, + .srtau = 1.0 + } + }; AP_Float _thr_mix_man; // throttle vs attitude control prioritisation used when using manual throttle (higher values mean we prioritise attitude control over throttle) AP_Float _thr_mix_min; // throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)