/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ /* support for autotune of helicopters */ #pragma once #include "AC_AutoTune.h" class AC_AutoTune_Heli : public AC_AutoTune { public: // constructor AC_AutoTune_Heli(); // save gained, called on disarm void save_tuning_gains() override; // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; protected: // // methods to load and save gains // // backup original gains and prepare for start of tuning void backup_gains_and_initialise() override; // switch to use original gains void load_orig_gains() override; // switch to gains found by last successful autotune void load_tuned_gains() override; // load gains used between tests. called during testing mode's update-gains step to set gains ahead of return-to-level step void load_intra_test_gains() override; // load test gains void load_test_gains() override; // reset the test vaariables for heli void reset_vehicle_test_variables() override; // initializes test void test_init() override; // runs test void test_run(AxisType test_axis, const float dir_sign) override; // update gains for the rate p up tune type void updating_rate_p_up_all(AxisType test_axis) override; // update gains for the rate d up tune type void updating_rate_d_up_all(AxisType test_axis) override; // update gains for the rate d down tune type void updating_rate_d_down_all(AxisType test_axis) override {}; // update gains for the rate ff up tune type void updating_rate_ff_up_all(AxisType test_axis) override; // update gains for the angle p up tune type void updating_angle_p_up_all(AxisType test_axis) override; // update gains for the angle p down tune type void updating_angle_p_down_all(AxisType test_axis) override {}; // update gains for the max gain tune type void updating_max_gains_all(AxisType test_axis) override; // get minimum rate P (for any axis) float get_rp_min() const override; // get minimum angle P (for any axis) float get_sp_min() const override; // get minimum rate Yaw filter value float get_yaw_rate_filt_min() const override; // reverse direction for twitch test bool twitch_reverse_direction() override { return positive_direction; } // methods to log autotune summary data void Log_AutoTune() override; void Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp, float max_accel); // methods to log autotune time history results for command, angular rate, and attitude. void Log_AutoTuneDetails() override; void Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads, float tgt_ang_rad, float ang_rad); // methods to log autotune frequency response results void Log_AutoTuneSweep() override; void Log_Write_AutoTuneSweep(float freq, float gain, float phase); // returns true if rate P gain of zero is acceptable for this vehicle bool allow_zero_rate_p() override { return true; } // returns true if max tested accel is used for parameter bool set_accel_to_max_test_value() override { return false; } // send intermittant updates to user on status of tune void do_gcs_announcements() override; // set the tuning test sequence void set_tune_sequence() override; // tuning sequence bitmask AP_Int8 seq_bitmask; // minimum sweep frequency AP_Float min_sweep_freq; // maximum sweep frequency AP_Float max_sweep_freq; // maximum response gain AP_Float max_resp_gain; private: // max_gain_data type stores information from the max gain test struct max_gain_data { float freq; float phase; float gain; float max_allowed; }; // max gain data for rate p tuning max_gain_data max_rate_p; // max gain data for rate d tuning max_gain_data max_rate_d; // Feedforward test used to determine Rate FF gain void rate_ff_test_init(); void rate_ff_test_run(float max_angle_cds, float target_rate_cds, float dir_sign); // dwell test used to perform frequency dwells for rate gains void dwell_test_init(float filt_freq); void dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase); // dwell test used to perform frequency dwells for angle gains void angle_dwell_test_init(float filt_freq); void angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase); // generates waveform for frequency sweep excitations float waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax); // updating_rate_ff_up - adjust FF to ensure the target is reached // FF is adjusted until rate requested is acheived void updating_rate_ff_up(float &tune_ff, float rate_target, float meas_rate, float meas_command); // updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not exceed exceed max response gain void updating_rate_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p); // updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response gain is at a minimum void updating_rate_d_up(float &tune_d, float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_d); // updating_angle_p_up - determines maximum angle p gain for pitch and roll void updating_angle_p_up(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt); // updating_angle_p_up_yaw - determines maximum angle p gain for yaw void updating_angle_p_up_yaw(float &tune_p, float *freq, float *gain, float *phase, uint8_t &frq_cnt); // updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur void updating_max_gains(float *freq, float *gain, float *phase, uint8_t &frq_cnt, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d); uint8_t method; //0: determine freq, 1: use max gain method, 2: use phase 180 method // updating rate FF variables // flag for completion of the initial direction for the feedforward test bool first_dir_complete; // feedforward gain resulting from testing in the initial direction float first_dir_rff; // updating max gain variables // flag for finding maximum p gain bool found_max_p; // flag for finding maximum d gain bool found_max_d; // flag for interpolating to find max response gain bool find_middle; // updating angle P up variables // track the maximum phase float phase_max; // previous gain float sp_prev_gain; // flag for finding the peak of the gain response bool find_peak; // updating angle P up yaw // counter value of previous good frequency uint8_t sp_prev_good_frq_cnt; // updating rate P up // counter value of previous good frequency uint8_t rp_prev_good_frq_cnt; // previous gain float rp_prev_gain; // updating rate D up // counter value of previous good frequency uint8_t rd_prev_good_frq_cnt; // previous gain float rd_prev_gain; uint8_t ff_test_phase; // phase of feedforward test float test_command_filt; // filtered commanded output for FF test analysis float test_rate_filt; // filtered rate output for FF test analysis float command_out; // test axis command output float test_tgt_rate_filt; // filtered target rate for FF test analysis float filt_target_rate; // filtered target rate float test_gain[20]; // frequency response gain for each dwell test iteration float test_freq[20]; // frequency of each dwell test iteration float test_phase[20]; // frequency response phase for each dwell test iteration float dwell_start_time_ms; // start time in ms of dwell test uint8_t freq_cnt_max; // counter number for frequency that produced max gain response float curr_test_freq; // current test frequency float curr_test_gain; // current test frequency response gain float curr_test_phase; // current test frequency response phase Vector3f start_angles; // aircraft attitude at the start of test uint32_t settle_time; // time in ms for allowing aircraft to stabilize before initiating test uint32_t phase_out_time; // time in ms to phase out response float waveform_freq_rads; //current frequency for chirp waveform float trim_pff_out; // trim output of the PID rate controller for P, I and FF terms float trim_meas_rate; // trim measured gyro rate //variables from rate FF test float trim_command_reading; float trim_heading; float rate_request_cds; float angle_request_cd; // variables from rate dwell test Vector3f trim_attitude_cd; Vector3f filt_attitude_cd; Vector2f filt_att_fdbk_from_velxy_cd; float filt_command_reading; float filt_gyro_reading; float filt_tgt_rate_reading; float trim_command; // variables from angle dwell test float trim_yaw_tgt_reading; float trim_yaw_heading_reading; // Vector2f filt_att_fdbk_from_velxy_cd; // float filt_command_reading; // float filt_gyro_reading; // float filt_tgt_rate_reading; LowPassFilterFloat command_filt; // filtered command LowPassFilterFloat target_rate_filt; // filtered target rotation rate in radians/second // sweep_data tracks the overall characteristics in the response to the frequency sweep struct sweep_data { float maxgain_freq; float maxgain_gain; float maxgain_phase; float ph180_freq; float ph180_gain; float ph180_phase; float ph270_freq; float ph270_gain; float ph270_phase; uint8_t progress; // set based on phase of frequency response. 0 - start; 1 - reached 180 deg; 2 - reached 270 deg; }; sweep_data sweep; // freqresp object for the rate frequency response tests AC_AutoTune_FreqResp freqresp_rate; // freqresp object for the angle frequency response tests AC_AutoTune_FreqResp freqresp_angle; };