From 27fb35253c3fe447508cff8057e823c57945d18b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 Apr 2016 20:26:43 +1000 Subject: [PATCH] Plane: added in-flight transmitter tuning --- ArduPlane/Log.cpp | 4 +- ArduPlane/Parameters.cpp | 4 + ArduPlane/Parameters.h | 1 + ArduPlane/Plane.h | 7 ++ ArduPlane/defines.h | 3 +- ArduPlane/quadplane.h | 1 + ArduPlane/radio.cpp | 2 + ArduPlane/tuning.cpp | 223 +++++++++++++++++++++++++++++++++++++++ ArduPlane/tuning.h | 75 +++++++++++++ 9 files changed, 318 insertions(+), 2 deletions(-) create mode 100644 ArduPlane/tuning.cpp create mode 100644 ArduPlane/tuning.h diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index c6a79ad2e2..8d0825b4f4 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -482,7 +482,7 @@ void Plane::Log_Write_Home_And_Origin() } } -static const struct LogStructure log_structure[] = { +const struct LogStructure Plane::log_structure[] = { LOG_COMMON_STRUCTURES, { LOG_PERFORMANCE_MSG, sizeof(log_Performance), "PM", "QIHIhhhBH", "TimeUS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" }, @@ -506,6 +506,8 @@ static const struct LogStructure log_structure[] = { { LOG_OPTFLOW_MSG, sizeof(log_Optflow), "OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY" }, #endif + { LOG_PARAMTUNE_MSG, sizeof(Tuning::log_ParameterTuning), + "PTUN", "QBfff", "TimeUS,Param,TunVal,TunLo,TunHi" }, TECS_LOG_FORMAT(LOG_TECS_MSG) }; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index ec26b11ca3..d85ef86258 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1106,6 +1106,10 @@ const AP_Param::Info Plane::var_info[] = { // @Path: quadplane.cpp GOBJECT(quadplane, "Q_", QuadPlane), + // @Group: TUNE_ + // @Path: tuning.cpp + GOBJECT(tuning, "TUNE_", Tuning), + // @Group: Q_A_ // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp { AP_PARAM_GROUP, "Q_A_", Parameters::k_param_q_attitude_control, diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 2762ed9608..a6e80443bc 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -272,6 +272,7 @@ public: k_param_long_fs_timeout, k_param_rc_13, k_param_rc_14, + k_param_tuning, // // 200: Feed-forward gains diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1111d3dd41..cb875b3a0d 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -95,6 +95,7 @@ #include #include "quadplane.h" +#include "tuning.h" // Configuration #include "config.h" @@ -136,6 +137,7 @@ public: friend class Parameters; friend class AP_Arming_Plane; friend class QuadPlane; + friend class Tuning; Plane(void); @@ -729,6 +731,11 @@ private: // support for quadcopter-plane QuadPlane quadplane{ahrs}; + // support for transmitter tuning + Tuning tuning; + + static const struct LogStructure log_structure[]; + #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 // the crc of the last created PX4Mixer int32_t last_mixer_crc = -1; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index bbf35e2b5d..251aa32408 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -117,7 +117,8 @@ enum log_messages { LOG_ARM_DISARM_MSG, LOG_STATUS_MSG, LOG_OPTFLOW_MSG, - LOG_QTUN_MSG + LOG_QTUN_MSG, + LOG_PARAMTUNE_MSG }; #define MASK_LOG_ATTITUDE_FAST (1<<0) diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index cfa8b41d01..1acbe164bc 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -14,6 +14,7 @@ class QuadPlane { public: friend class Plane; + friend class Tuning; QuadPlane(AP_AHRS_NavEKF &_ahrs); // var_info for holding Parameter information diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index bcdc9eda41..81bc6acd91 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -215,6 +215,8 @@ void Plane::read_radio() } else { rudder_input = channel_rudder->control_in; } + + tuning.check_input(); } void Plane::control_failsafe(uint16_t pwm) diff --git a/ArduPlane/tuning.cpp b/ArduPlane/tuning.cpp new file mode 100644 index 0000000000..64d3fcc81b --- /dev/null +++ b/ArduPlane/tuning.cpp @@ -0,0 +1,223 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Plane.h" + +const AP_Param::GroupInfo Tuning::var_info[] = { + + // @Param: CHAN + // @DisplayName: Transmitter tuning channel + // @Description: This sets the channel for transmitter tuning + // @Values: 0:Disable,1:Chan1,2:Chan3,3:Chan3,4:Chan4,5:Chan5,6:Chan6,7:Chan7,8:Chan8,9:Chan9,10:Chan10,11:Chan11,12:Chan12,13:Chan13,14:Chan14,15:Chan15,16:Chan16 + // @User: Standard + AP_GROUPINFO_FLAGS("CHAN", 1, Tuning, channel, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: PARM + // @DisplayName: Transmitter tuning parameter + // @Description: This sets which parameter or combination of parameters will be tuned + // @User: Standard + AP_GROUPINFO("PARM", 2, Tuning, parm, 0), + + // @Param: MIN + // @DisplayName: Transmitter tuning minimum value + // @Description: This sets the minimum value + // @User: Standard + AP_GROUPINFO("MIN", 3, Tuning, minimum, 0), + + // @Param: MAX + // @DisplayName: Transmitter tuning maximum value + // @Description: This sets the maximum value + // @User: Standard + AP_GROUPINFO("MAX", 4, Tuning, maximum, 0), + + AP_GROUPEND +}; + +/* + constructor + */ +Tuning::Tuning(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +/* + check for changed tuning input + */ +void Tuning::check_input(void) +{ + if (channel <= 0 || parm <= 0) { + // disabled + return; + } + + // only adjust values at 4Hz + uint32_t now = AP_HAL::millis(); + if (now - last_check_ms < 250) { + return; + } + last_check_ms = now; + + if (channel > hal.rcin->num_channels()) { + return; + } + + RC_Channel *chan = RC_Channel::rc_channel(channel-1); + if (chan == nullptr) { + return; + } + uint8_t input = chan->percent_input(); + if (input == last_input_pct) { + // no change + return; + } + last_input_pct = input; + + float tuning_value = minimum + (maximum-minimum)*(input*0.01f); + + if (!plane.quadplane.available()) { + // quadplane tuning options not available + return; + } + + switch((enum tuning_func)parm.get()) { + + case TUNING_Q_RATE_ROLL_PITCH_KPI: + plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value); + plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value); + plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value); + plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value); + break; + + case TUNING_Q_RATE_ROLL_PITCH_KP: + plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value); + plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value); + break; + + case TUNING_Q_RATE_ROLL_PITCH_KI: + plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value); + plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value); + break; + + case TUNING_Q_RATE_ROLL_PITCH_KD: + plane.quadplane.attitude_control->get_rate_roll_pid().kD(tuning_value); + plane.quadplane.attitude_control->get_rate_pitch_pid().kD(tuning_value); + break; + + case TUNING_Q_RATE_ROLL_KPI: + plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value); + plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value); + break; + + case TUNING_Q_RATE_ROLL_KP: + plane.quadplane.attitude_control->get_rate_roll_pid().kP(tuning_value); + break; + + case TUNING_Q_RATE_ROLL_KI: + plane.quadplane.attitude_control->get_rate_roll_pid().kI(tuning_value); + break; + + case TUNING_Q_RATE_ROLL_KD: + plane.quadplane.attitude_control->get_rate_roll_pid().kD(tuning_value); + break; + + case TUNING_Q_RATE_PITCH_KPI: + plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value); + plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value); + break; + + case TUNING_Q_RATE_PITCH_KP: + plane.quadplane.attitude_control->get_rate_pitch_pid().kP(tuning_value); + break; + + case TUNING_Q_RATE_PITCH_KI: + plane.quadplane.attitude_control->get_rate_pitch_pid().kI(tuning_value); + break; + + case TUNING_Q_RATE_PITCH_KD: + plane.quadplane.attitude_control->get_rate_pitch_pid().kD(tuning_value); + break; + + case TUNING_Q_RATE_YAW_KPI: + plane.quadplane.attitude_control->get_rate_yaw_pid().kP(tuning_value); + plane.quadplane.attitude_control->get_rate_yaw_pid().kI(tuning_value); + break; + + case TUNING_Q_RATE_YAW_KP: + plane.quadplane.attitude_control->get_rate_yaw_pid().kP(tuning_value); + break; + + case TUNING_Q_RATE_YAW_KI: + plane.quadplane.attitude_control->get_rate_yaw_pid().kI(tuning_value); + break; + + case TUNING_Q_RATE_YAW_KD: + plane.quadplane.attitude_control->get_rate_yaw_pid().kD(tuning_value); + break; + + case TUNING_Q_ANG_ROLL_KP: + plane.quadplane.attitude_control->get_angle_roll_p().kP(tuning_value); + break; + + case TUNING_Q_ANG_PITCH_KP: + plane.quadplane.attitude_control->get_angle_pitch_p().kP(tuning_value); + break; + + case TUNING_Q_ANG_YAW_KP: + plane.quadplane.attitude_control->get_angle_yaw_p().kP(tuning_value); + break; + + case TUNING_Q_PXY_P: + plane.quadplane.p_pos_xy.kP(tuning_value); + break; + + case TUNING_Q_PZ_P: + plane.quadplane.p_alt_hold.kP(tuning_value); + break; + + case TUNING_Q_VXY_P: + plane.quadplane.pi_vel_xy.kP(tuning_value); + break; + + case TUNING_Q_VXY_I: + plane.quadplane.pi_vel_xy.kI(tuning_value); + break; + + case TUNING_Q_VZ_P: + plane.quadplane.p_vel_z.kP(tuning_value); + break; + + case TUNING_Q_AZ_P: + plane.quadplane.pid_accel_z.kP(tuning_value); + break; + + case TUNING_Q_AZ_I: + plane.quadplane.pid_accel_z.kI(tuning_value); + break; + + case TUNING_Q_AZ_D: + plane.quadplane.pid_accel_z.kD(tuning_value); + break; + + default: + return; + } + + Log_Write_Parameter_Tuning((uint8_t)parm.get(), tuning_value, minimum, maximum); +} + +/* + log a tuning change + */ +void Tuning::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_low, float tune_high) +{ + struct log_ParameterTuning pkt_tune = { + LOG_PACKET_HEADER_INIT(LOG_PARAMTUNE_MSG), + time_us : AP_HAL::micros64(), + parameter : param, + tuning_value : tuning_val, + tuning_low : tune_low, + tuning_high : tune_high + }; + + plane.DataFlash.WriteBlock(&pkt_tune, sizeof(pkt_tune)); +} diff --git a/ArduPlane/tuning.h b/ArduPlane/tuning.h new file mode 100644 index 0000000000..fbbd4eb312 --- /dev/null +++ b/ArduPlane/tuning.h @@ -0,0 +1,75 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + Plane transmitter tuning + */ +class Tuning +{ +public: + friend class Plane; + + Tuning(void); + + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + + void check_input(void); + +private: + AP_Int8 channel; + AP_Int8 parm; + AP_Float minimum; + AP_Float maximum; + + uint8_t last_input_pct = 255; + uint32_t last_check_ms; + + struct PACKED log_ParameterTuning { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t parameter; // parameter we are tuning, e.g. 39 is CH6_CIRCLE_RATE + float tuning_value; // normalized value used inside tuning() function + float tuning_low; // tuning low end value + float tuning_high; // tuning high end value + }; + + void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_low, float tune_high); + + enum tuning_func { + TUNING_NONE = 0, + TUNING_Q_RATE_ROLL_PITCH_KPI = 1, + TUNING_Q_RATE_ROLL_PITCH_KP = 2, + TUNING_Q_RATE_ROLL_PITCH_KI = 3, + TUNING_Q_RATE_ROLL_PITCH_KD = 4, + + TUNING_Q_RATE_ROLL_KPI = 5, + TUNING_Q_RATE_ROLL_KP = 6, + TUNING_Q_RATE_ROLL_KI = 7, + TUNING_Q_RATE_ROLL_KD = 8, + + TUNING_Q_RATE_PITCH_KPI = 9, + TUNING_Q_RATE_PITCH_KP = 10, + TUNING_Q_RATE_PITCH_KI = 11, + TUNING_Q_RATE_PITCH_KD = 12, + + TUNING_Q_RATE_YAW_KPI = 13, + TUNING_Q_RATE_YAW_KP = 14, + TUNING_Q_RATE_YAW_KI = 15, + TUNING_Q_RATE_YAW_KD = 16, + + TUNING_Q_ANG_ROLL_KP = 17, + TUNING_Q_ANG_PITCH_KP = 18, + TUNING_Q_ANG_YAW_KP = 19, + + TUNING_Q_PXY_P = 20, + TUNING_Q_PZ_P = 21, + + TUNING_Q_VXY_P = 22, + TUNING_Q_VXY_I = 23, + TUNING_Q_VZ_P = 24, + + TUNING_Q_AZ_P = 25, + TUNING_Q_AZ_I = 26, + TUNING_Q_AZ_D = 27, + }; +};