diff --git a/AnalyticTune/AnalyticTune.js b/AnalyticTune/AnalyticTune.js index 306e53fd..b181c763 100644 --- a/AnalyticTune/AnalyticTune.js +++ b/AnalyticTune/AnalyticTune.js @@ -2,395 +2,12 @@ var DataflashParser import('../modules/JsDataflashParser/parser.js').then((mod) => { DataflashParser = mod.default }) - -function PID(sample_rate,kP,kI,kD,filtE,filtD) { - this.sample_rate = sample_rate - - this._kP = kP; - this._kI = kI; - this._kD = kD; - - this.E_filter = new LPF_1P(sample_rate, filtE) - this.D_filter = new LPF_1P(sample_rate, filtD) - - this.transfer = function(Z, Z1, Z2, use_dB, unwrap_phase) { - const E_trans = this.E_filter.transfer(Z, Z1, Z2, false, false) - const D_trans = complex_mul(E_trans, this.D_filter.transfer(Z, Z1, Z2, false, false)) - - // I term is k*z / (z - 1) - const Z_less_one = [array_offset(Z[0], -1), Z[1].slice()] - const I_comp = complex_mul(complex_div(Z,Z_less_one), E_trans) - const kI = this._kI/this.sample_rate - - // D term is k * (1 - Z^-1) - const one_less_Z1 = [array_offset(array_scale(Z1[0],-1), 1), array_scale(Z1[1],-1)] - const D_comp = complex_mul(one_less_Z1, D_trans) - const kD = this._kD*this.sample_rate - - - const len = Z1[0].length - let ret = [new Array(len), new Array(len)] - let P = [new Array(len), new Array(len)] - let I = [new Array(len), new Array(len)] - let D = [new Array(len), new Array(len)] - for (let i = 0; i 0.0) && (this.center_freq_hz < 0.5 * this.sample_rate) && (this.Q > 0.0)) { - this.A = Math.pow(10.0, -this.attenuation_dB / 40.0); - var omega = 2.0 * Math.PI * this.center_freq_hz / this.sample_rate; - var alpha = Math.sin(omega) / (2 * this.Q); - this.b0 = 1.0 + alpha*(this.A**2); - this.b1 = -2.0 * Math.cos(omega); - this.b2 = 1.0 - alpha*(this.A**2); - this.a0_inv = 1.0/(1.0 + alpha); - this.a1 = this.b1; - this.a2 = 1.0 - alpha; - this.initialised = true; - } else { - this.initialised = false; - } - - this.transfer = function(Z, Z1, Z2, use_dB, unwrap_phase) { - if (!this.initialised) { - const len = Z1[0].length - return [new Array(len).fill(1), new Array(len).fill(0)] - } - - const a0 = 1 / this.a0_inv - - const len = Z1[0].length - let numerator = [new Array(len), new Array(len)] - let denominator = [new Array(len), new Array(len)] - for (let i = 0; i 0.5 * this.bandwidth_hz) { - var octaves = Math.log2(this.center_freq_hz / (this.center_freq_hz - this.bandwidth_hz / 2.0)) * 2.0; - this.Q = Math.sqrt(Math.pow(2.0, octaves)) / (Math.pow(2.0, octaves) - 1.0); - } else { - this.Q = 0.0; - } - } - - this.init_with_A_and_Q = function() { - if ((this.center_freq_hz > 0.0) && (this.center_freq_hz < 0.5 * this.sample_freq) && (this.Q > 0.0)) { - var omega = 2.0 * Math.PI * this.center_freq_hz / this.sample_freq; - var alpha = Math.sin(omega) / (2 * this.Q); - this.b0 = 1.0 + alpha*(this.A**2); - this.b1 = -2.0 * Math.cos(omega); - this.b2 = 1.0 - alpha*(this.A**2); - this.a0_inv = 1.0/(1.0 + alpha); - this.a1 = this.b1; - this.a2 = 1.0 - alpha; - this.initialised = true; - } else { - this.initialised = false; - } - } - - // check center frequency is in the allowable range - if ((center_freq_hz > 0.5 * bandwidth_hz) && (center_freq_hz < 0.5 * sample_freq)) { - this.calculate_A_and_Q(); - this.init_with_A_and_Q(); - } else { - this.initialised = false; - } - - this.transfer = function(Z, Z1, Z2) { - if (!this.initialised) { - const len = Z1[0].length - return [new Array(len).fill(1), new Array(len).fill(0)] - } - - const a0 = 1 / this.a0_inv - - const len = Z1[0].length - let numerator = [new Array(len), new Array(len)] - let denominator = [new Array(len), new Array(len)] - for (let i = 0; i 1) { - var notch_center_double; - // only enable the filter if its center frequency is below the nyquist frequency - notch_center_double = notch_center * (1.0 - notch_spread); - if (notch_center_double < nyquist_limit) { - this.notches.push(new NotchFilter(sample_freq,notch_center_double,bandwidth_hz/composite_notches,att)); - } - // only enable the filter if its center frequency is below the nyquist frequency - notch_center_double = notch_center * (1.0 + notch_spread); - if (notch_center_double < nyquist_limit) { - this.notches.push(new NotchFilter(sample_freq,notch_center_double,bandwidth_hz/composite_notches,att)); - } - } - } - } - } - - this.transfer = function(Z, Z1, Z2, use_dB, unwrap_phase) { - const len = Z1[0].length - var H_total = [new Array(len).fill(1), new Array(len).fill(0)] - for (n in this.notches) { - const H = this.notches[n].transfer(Z, Z1, Z2); - H_total = complex_mul(H_total, H) - } - - this.attenuation = complex_abs(H_total) - this.phase = array_scale(complex_phase(H_total), 180/Math.PI) - if (use_dB) { - this.attenuation = array_scale(array_log10(this.attenuation), 20.0) - } - if (unwrap_phase) { - this.phase = unwrap(this.phase) - } - - return H_total; - } -} - function get_form(vname) { var v = parseFloat(document.getElementById(vname).value); return v; } -function get_filters(sample_rate) { - var filters = [] - filters.push(new HarmonicNotchFilter(sample_rate, - get_form("INS_HNTCH_ENABLE"), - get_form("INS_HNTCH_MODE"), - get_form("INS_HNTCH_FREQ"), - get_form("INS_HNTCH_BW"), - get_form("INS_HNTCH_ATT"), - get_form("INS_HNTCH_REF"), - get_form("INS_HNTCH_FM_RAT"), - get_form("INS_HNTCH_HMNCS"), - get_form("INS_HNTCH_OPTS"))); - filters.push(new HarmonicNotchFilter(sample_rate, - get_form("INS_HNTC2_ENABLE"), - get_form("INS_HNTC2_MODE"), - get_form("INS_HNTC2_FREQ"), - get_form("INS_HNTC2_BW"), - get_form("INS_HNTC2_ATT"), - get_form("INS_HNTC2_REF"), - get_form("INS_HNTC2_FM_RAT"), - get_form("INS_HNTC2_HMNCS"), - get_form("INS_HNTC2_OPTS"))); - filters.push(new DigitalBiquadFilter(sample_rate,get_form("INS_GYRO_FILTER"))); - - return filters; -} - -// Unwrap phase by looking for jumps of larger than 180 deg -function unwrap(phase) { - const len = phase.length - - // Notches result in large positive phase changes, bias the unwrap to do a better job - const neg_threshold = 45 - const pos_threshold = 360 - neg_threshold - - let unwrapped = new Array(len) - - unwrapped[0] = phase[0] - for (let i = 1; i < len; i++) { - let phase_diff = phase[i] - phase[i-1]; - if (phase_diff > pos_threshold) { - phase_diff -= 360.0; - } else if (phase_diff < -neg_threshold) { - phase_diff += 360.0; - } - unwrapped[i] = unwrapped[i-1] + phase_diff - } - - return unwrapped -} - -function evaluate_transfer_functions(filter_groups, freq_max, freq_step, use_dB, unwrap_phase) { - - // Not sure why range does not return expected array, _data gets us the array - const freq = array_from_range(freq_step, freq_max, freq_step) - - // Start with unity transfer function, input = output - const len = freq.length - var H_total = [new Array(len).fill(1), new Array(len).fill(0)] - - for (let i = 0; i < filter_groups.length; i++) { - // Allow for batches at different sample rates - const filters = filter_groups[i] - - const sample_rate = filters[0].sample_rate - for (let j = 1; j < filters.length; j++) { - if (filters[0].sample_rate != sample_rate) { - error("Sample rate miss match") - } - } - - // Calculate Z for transfer function - // Z = e^jw - const Z = exp_jw(freq, sample_rate) - - // Z^-1 - const Z1 = complex_inverse(Z) - - // Z^-2 - const Z2 = complex_inverse(complex_square(Z)) - - // Apply all transfer functions - for (let filter of filters) { - const H = filter.transfer(Z, Z1, Z2, use_dB, unwrap_phase) - H_total = complex_mul(H_total, H) - } - } - - // Calculate total filter transfer function - let attenuation = complex_abs(H_total) - let phase = array_scale(complex_phase(H_total), 180/Math.PI) - if (use_dB) { - attenuation = array_scale(array_log10(attenuation), 20.0) - } - if (unwrap_phase) { - phase = unwrap(phase) - } - - // Return attenuation and phase - return { attenuation: attenuation, phase: phase, freq: freq, H_total: H_total} -} - var flight_data = {} var fft_plot = {} var fft_plot_Phase = {} @@ -669,7 +78,7 @@ function link_plots() { } -function setup_plots() { +function setup_time_history_plots() { const time_scale_label = "Time (s)" @@ -745,6 +154,9 @@ function setup_plots() { }) +} + +function setup_freq_response_plots() { amplitude_scale = get_amplitude_scale() frequency_scale = get_frequency_scale() @@ -805,7 +217,7 @@ function get_axis_prefix() { return "" } -function calculate_predicted_TF(H_acft, sample_rate, window_size) { +function calculate_attctrl_predicted_TF(H_acft, sample_rate, window_size) { //this will have to be the sample rate of time history data var freq_max = sample_rate * 0.5 @@ -895,60 +307,290 @@ function calculate_predicted_TF(H_acft, sample_rate, window_size) { if (vehicle_type == "ArduPlane_FW") { Angle_P = 1 / get_form(get_angle_param_prefix() + "TCONST") } else { - Angle_P = get_form(get_angle_param_prefix() + "P") + Angle_P = get_form(get_angle_param_prefix() + "P") + } + Ang_P_filter.push(new Ang_P(PID_rate, Angle_P)) + const Ang_P_H = evaluate_transfer_functions([Ang_P_filter], freq_max, freq_step, use_dB, unwrap_phase) + + // calculate transfer function for attitude controller with feedforward enabled (includes intermediate steps) + const rate_ANGP = complex_mul(Ret_rate, Ang_P_H.H_total) + var rate_ANGP_plus_one = [new Array(H_acft[0].length).fill(0), new Array(H_acft[0].length).fill(0)] + var ANGP_plus_one = [new Array(H_acft[0].length).fill(0), new Array(H_acft[0].length).fill(0)] + for (let k=0;k 0) { nef_freq = get_form("FILT" + nef_num + "_NOTCH_FREQ") } + if (nef_num > 0 && nef_freq > 0.0) { + var E_notch_filter = [] + E_notch_filter.push(new NotchFilterusingQ(PID_rate, nef_freq, get_form("FILT" + nef_num + "_NOTCH_Q"), get_form("FILT" + nef_num + "_NOTCH_ATT"))) + const NEF_H = evaluate_transfer_functions([E_notch_filter], freq_max, freq_step, use_dB, unwrap_phase) + PID_H_TOT = complex_mul(NEF_H.H_total, PID_H.H_total) + } else { + PID_H_TOT = PID_H.H_total + } + + // calculate transfer function for FF and DFF + var FF_filter = [] + FF_filter.push(new feedforward(PID_rate, get_form(param_prefix + "FF"),get_form(param_prefix + "D_FF"))) + const FF_H = evaluate_transfer_functions([FF_filter], freq_max, freq_step, use_dB, unwrap_phase) + var FFPID_H = [new Array(H_acft[0].length).fill(0), new Array(H_acft[0].length).fill(0)] + for (let k=0;k 0) { ntf_freq = get_form("FILT" + ntf_num + "_NOTCH_FREQ") } + if (ntf_num > 0 && ntf_freq > 0.0) { + var T_notch_filter = [] + T_notch_filter.push(new NotchFilterusingQ(PID_rate, ntf_freq, get_form("FILT" + ntf_num + "_NOTCH_Q"), get_form("FILT" + ntf_num + "_NOTCH_ATT"))) + const NTF_H = evaluate_transfer_functions([T_notch_filter], freq_max, freq_step, use_dB, unwrap_phase) + TGT_FILT_H = complex_mul(NTF_H.H_total, FLTT_H.H_total) + } else { + TGT_FILT_H = FLTT_H.H_total } - Ang_P_filter.push(new Ang_P(PID_rate, Angle_P)) - const Ang_P_H = evaluate_transfer_functions([Ang_P_filter], freq_max, freq_step, use_dB, unwrap_phase) - // calculate transfer function for attitude controller with feedforward enabled (includes intermediate steps) - const rate_ANGP = complex_mul(Ret_rate, Ang_P_H.H_total) - var rate_ANGP_plus_one = [new Array(H_acft[0].length).fill(0), new Array(H_acft[0].length).fill(0)] - var ANGP_plus_one = [new Array(H_acft[0].length).fill(0), new Array(H_acft[0].length).fill(0)] - for (let k=0;k 0) { - document.getElementById('FILT' + NTF_num).style.display = 'block'; - } - const NEF_num = document.getElementById(get_rate_param_prefix() + 'NEF').value; - if (NEF_num > 0 && NEF_num != NTF_num) { - document.getElementById('FILT' + NEF_num).style.display = 'block'; + if (page_axis != "Lateral" && page_axis != "Longitudinal" && page_axis != "Vertical") { + document.getElementById('RollNOTCH').style.display = 'none'; + document.getElementById('PitchNOTCH').style.display = 'none'; + document.getElementById('YawNOTCH').style.display = 'none'; + for (let i = 1; i<9; i++) { + document.getElementById('FILT' + i).style.display = 'none'; + } + } + if (page_axis == "Roll" || page_axis == "Lateral" || page_axis == "Longitudinal") { + if (vehicle_type != "ArduPlane_FW" && page_axis != "Lateral" && page_axis != "Longitudinal") { + document.getElementById('RollPitchTC').style.display = 'block'; + } + document.getElementById('RollPIDS').style.display = 'block'; + if (page_axis != "Lateral" && page_axis != "Longitudinal") { + document.getElementById('RollNOTCH').style.display = 'block'; + const NTF_num = document.getElementById(get_rate_param_prefix() + 'NTF').value; + if (NTF_num > 0) { + document.getElementById('FILT' + NTF_num).style.display = 'block'; + } + const NEF_num = document.getElementById(get_rate_param_prefix() + 'NEF').value; + if (NEF_num > 0 && NEF_num != NTF_num) { + document.getElementById('FILT' + NEF_num).style.display = 'block'; + } } } else if (page_axis == "Pitch") { if (vehicle_type != "ArduPlane_FW") { - document.getElementById(ele_prefix + 'RollPitchTC').style.display = 'block'; + document.getElementById('RollPitchTC').style.display = 'block'; } - document.getElementById(ele_prefix + 'PitchPIDS').style.display = 'block'; - document.getElementById(ele_prefix + 'PitchNOTCH').style.display = 'block'; - console.log(ele_prefix + 'PitchPIDS') + document.getElementById('PitchPIDS').style.display = 'block'; + document.getElementById('PitchNOTCH').style.display = 'block'; + const NTF_num = document.getElementById(get_rate_param_prefix() + 'NTF').value; if (NTF_num > 0) { document.getElementById('FILT' + NTF_num).style.display = 'block'; @@ -1384,15 +1029,34 @@ function update_PID_filters() { } } else if (page_axis == "Yaw") { if (vehicle_type != "ArduPlane_FW") { - document.getElementById(ele_prefix + 'YawTC').style.display = 'block'; + document.getElementById('YawTC').style.display = 'block'; + } + document.getElementById('YawPIDS').style.display = 'block'; + if (page_axis != "Vertical") { + document.getElementById('YawNOTCH').style.display = 'block'; + console.log(get_rate_param_prefix() + 'NTF') + const NTF_num = document.getElementById(get_rate_param_prefix() + 'NTF').value; + if (NTF_num > 0) { + document.getElementById('FILT' + NTF_num).style.display = 'block'; + } + const NEF_num = document.getElementById(get_rate_param_prefix() + 'NEF').value; + if (NEF_num > 0 && NEF_num != NTF_num) { + document.getElementById('FILT' + NEF_num).style.display = 'block'; + } } - document.getElementById(ele_prefix + 'YawPIDS').style.display = 'block'; - document.getElementById(ele_prefix + 'YawNOTCH').style.display = 'block'; - const NTF_num = document.getElementById(get_rate_param_prefix() + 'NTF').value; + } else if (page_axis == "Vertical-Accel" || page_axis == "Vertical") { + document.getElementById('PitchPIDS').style.display = 'block'; + document.getElementById('YawPIDS').style.display = 'block'; + document.getElementById('PitchNOTCH').style.display = 'block'; + var param_prefix = "PSC_ACCZ_"; + if (vehicle_type == "ArduPlane_VTOL") { + param_prefix = "Q_P_ACCZ_"; + } + const NTF_num = document.getElementById(param_prefix + 'NTF').value; if (NTF_num > 0) { document.getElementById('FILT' + NTF_num).style.display = 'block'; } - const NEF_num = document.getElementById(get_rate_param_prefix() + 'NEF').value; + const NEF_num = document.getElementById(param_prefix + 'NEF').value; if (NEF_num > 0 && NEF_num != NTF_num) { document.getElementById('FILT' + NEF_num).style.display = 'block'; } @@ -1405,6 +1069,7 @@ var calc_freq_resp var pred_freq_resp function calculate_freq_resp() { const start = performance.now() + console.log(params) // Window size from user const window_size = parseInt(document.getElementById("FFTWindow_size").value) @@ -1430,9 +1095,12 @@ function calculate_freq_resp() { var sample_rate if (vehicle_type == "ArduPlane_FW") { [data_set, sample_rate] = load_fw_time_history_data(t_start, t_end, page_axis) + } else if (page_axis == "Lateral" || page_axis == "Longitudinal" || page_axis == "Vertical" || page_axis == "Vertical-Accel") { + [data_set, sample_rate] = load_posctrl_time_history_data(t_start, t_end, page_axis) } else { [data_set, sample_rate] = load_vtol_time_history_data(t_start, t_end, page_axis) } + console.log("dataset", data_set) data_set.FFT = run_fft(data_set, Object.keys(data_set), window_size, window_spacing, windowing_function, fft) // Get bins and other useful stuff @@ -1452,11 +1120,10 @@ function calculate_freq_resp() { // Number of windows averaged const mean_length = end_index - start_index -// console.log(mean_length) var H_pilot var coh_pilot - if (page_axis == "Yaw") { + if (page_axis == "Yaw" || page_axis == "Vertical") { [H_pilot, coh_pilot] = calculate_freq_resp_from_FFT(data_set.FFT.PilotInput, data_set.FFT.Rate, start_index, end_index, mean_length, window_size, sample_rate) } else { [H_pilot, coh_pilot] = calculate_freq_resp_from_FFT(data_set.FFT.PilotInput, data_set.FFT.Att, start_index, end_index, mean_length, window_size, sample_rate) @@ -1464,7 +1131,7 @@ function calculate_freq_resp() { var H_acft var coh_acft - if (document.getElementById('UseAttitude').checked) { + if (document.getElementById('UseAttitude' + get_page_suffix()).checked && page_axis != "Vertical") { [H_acft, coh_acft] = calculate_freq_resp_from_FFT(data_set.FFT.ActInput, data_set.FFT.Att, start_index, end_index, mean_length, window_size, sample_rate) } else { [H_acft, coh_acft] = calculate_freq_resp_from_FFT(data_set.FFT.ActInput, data_set.FFT.GyroRaw, start_index, end_index, mean_length, window_size, sample_rate) @@ -1472,7 +1139,7 @@ function calculate_freq_resp() { var H_rate var coh_rate - if (document.getElementById('UseAttitude').checked) { + if (document.getElementById('UseAttitude' + get_page_suffix()).checked && page_axis != "Vertical") { [H_rate, coh_rate] = calculate_freq_resp_from_FFT(data_set.FFT.RateTgt, data_set.FFT.Att, start_index, end_index, mean_length, window_size, sample_rate) } else { [H_rate, coh_rate] = calculate_freq_resp_from_FFT(data_set.FFT.RateTgt, data_set.FFT.GyroRaw, start_index, end_index, mean_length, window_size, sample_rate) @@ -1527,7 +1194,7 @@ function calculate_freq_resp() { freq_tf[k-1] = data_set.FFT.bins[k] } - if (document.getElementById('UseAttitude').checked) { + if (document.getElementById('UseAttitude' + get_page_suffix()).checked && page_axis != "Vertical") { var loop_rate = get_form("SCHED_LOOP_RATE") // determine transfer function for s var der_filter = [] @@ -1547,7 +1214,21 @@ function calculate_freq_resp() { var H_att_bl_pred var H_rate_bl_pred var H_sys_bl_pred - [H_rate_pred, H_att_ff_pred, H_pilot_pred, H_DRB_pred, H_att_nff_pred, H_att_bl_pred, H_rate_bl_pred, H_sys_bl_pred] = calculate_predicted_TF(H_acft_tf, sample_rate, window_size) + console.log("page_axis: ", page_axis) + if (page_axis == "Lateral" || page_axis == "Longitudinal") { + [H_rate_pred, H_att_ff_pred, H_pilot_pred, H_DRB_pred, H_att_nff_pred, H_att_bl_pred, H_rate_bl_pred, H_sys_bl_pred] = calculate_posctrl_predicted_TF(H_acft_tf, sample_rate, window_size) + } else if (page_axis == "Vertical-Accel" || page_axis == "Vertical") { + [H_rate_pred, H_att_ff_pred, H_pilot_pred, H_DRB_pred, H_att_nff_pred, H_att_bl_pred, H_rate_bl_pred, H_sys_bl_pred] = calculate_posctrl_vert_predicted_TF(H_acft_tf, sample_rate, window_size) + if (document.getElementById('UseAttitude' + get_page_suffix()).checked) { + var H_dummy + [H_dummy, H_att_ff_pred, H_pilot_pred, H_DRB_pred, H_att_nff_pred, H_att_bl_pred, H_rate_bl_pred, H_sys_bl_pred] = calculate_posctrl_predicted_TF(H_pilot, sample_rate, window_size) + } else { + var H_dummy + [H_dummy, H_att_ff_pred, H_pilot_pred, H_DRB_pred, H_att_nff_pred, H_att_bl_pred, H_rate_bl_pred, H_sys_bl_pred] = calculate_posctrl_predicted_TF(H_pilot_pred, sample_rate, window_size) + } + } else { + [H_rate_pred, H_att_ff_pred, H_pilot_pred, H_DRB_pred, H_att_nff_pred, H_att_bl_pred, H_rate_bl_pred, H_sys_bl_pred] = calculate_attctrl_predicted_TF(H_acft_tf, sample_rate, window_size) + } calc_freq_resp = { pilotctrl_H: H_pilot_tf, @@ -1574,7 +1255,7 @@ function calculate_freq_resp() { ratebl_H: H_rate_bl_pred, sysbl_H: H_sys_bl_pred } - + console.log(pred_freq_resp) redraw_freq_resp() @@ -1712,6 +1393,185 @@ function load_vtol_time_history_data(t_start, t_end, axis) { } +function load_posctrl_time_history_data(t_start, t_end, axis) { + + var timeRate + var ind1_r + var ind2_r + timeRate = log.get("RATE", "TimeUS") + ind1_r = nearestIndex(timeRate, t_start*1000000) + ind2_r = nearestIndex(timeRate, t_end*1000000) + + timeRate = timeRate.slice(ind1_r, ind2_r) + // Determine average sample rate + const trecord = (timeRate[timeRate.length - 1] - timeRate[0]) / 1000000 + const samplerate = (timeRate.length)/ trecord + console.log("sample rate: ", samplerate, t_start, t_end, trecord) + var timeATT + var ind1_a + var ind2_a + var heading + if (use_ANG_message) { + timeATT = log.get("ANG", "TimeUS") + ind1_a = nearestIndex(timeATT, t_start*1000000) + ind2_a = nearestIndex(timeATT, t_end*1000000) + heading = Array.from(log.get("ANG", "DesYaw")) + } else { + timeATT = log.get("ATT", "TimeUS") + ind1_a = nearestIndex(timeATT, t_start*1000000) + ind2_a = nearestIndex(timeATT, t_end*1000000) + heading = Array.from(log.get("ATT", "DesYaw")) + } + heading = heading.slice(ind1_a, ind2_a) + const sinhdg = Math.sin(array_mean(heading)*Math.PI/180) + const coshdg = Math.cos(array_mean(heading)*Math.PI/180) + + const timeSIDD = log.get("SIDD", "TimeUS") + const ind1_s = nearestIndex(timeSIDD, t_start*1000000) + const ind2_s = nearestIndex(timeSIDD, t_end*1000000) + + console.log(array_mean(heading)) + + var ActInputData + var RateTgtData + var RateData + var AttTgtData + var AttData + var GyroRawData + + if (axis == "Vertical-Accel") { + ActInputData = Array.from(log.get("RATE", "AOut")) + ActInputData = ActInputData.slice(ind1_r, ind2_r) + ActInputData = array_scale(ActInputData, 1000) + RateTgtData = Array.from(log.get("RATE", "ADes")) + RateTgtData = RateTgtData.slice(ind1_r, ind2_r) + RateData = Array.from(log.get("RATE", "A")) + RateData = RateData.slice(ind1_r, ind2_r) + AttTgtData = Array.from(log.get("RATE", "ADes")) + AttTgtData = AttTgtData.slice(ind1_r, ind2_r) + AttData = Array.from(log.get("RATE", "A")) + AttData = AttData.slice(ind1_r, ind2_r) + GyroRawData = Array.from(log.get("RATE", "A")) + GyroRawData = GyroRawData.slice(ind1_r, ind2_r) + } else if (axis == "Vertical") { + var timePSCD + var ind1_d + var ind2_d + timePSCD = log.get("PSCD", "TimeUS") + ind1_d = nearestIndex(timePSCD, t_start*1000000) + ind2_d = nearestIndex(timePSCD, t_end*1000000) + + ActInputData = Array.from(log.get("RATE", "AOut")) + ActInputData = ActInputData.slice(ind1_r, ind2_r) + ActInputData = array_scale(ActInputData, 1000) + RateTgtData = Array.from(log.get("RATE", "ADes")) + RateTgtData = RateTgtData.slice(ind1_r, ind2_r) + RateData = Array.from(log.get("PSCD", "VD")) + RateData = RateData.slice(ind1_d, ind2_d) + AttTgtData = Array.from(log.get("PSCD", "TPD")) + AttTgtData = AttTgtData.slice(ind1_d, ind2_d) + AttData = Array.from(log.get("PSCD", "PD")) + AttData = AttData.slice(ind1_d, ind2_d) + GyroRawData = Array.from(log.get("RATE", "A")) + GyroRawData = GyroRawData.slice(ind1_r, ind2_r) + } else { + var timePSCN_arr + timePSCN_arr = log.get("PSCN", "TimeUS") + const ind1_n = nearestIndex(timePSCN_arr, t_start*1000000) + const ind2_n = nearestIndex(timePSCN_arr, t_end*1000000) + + var timePSCE + var ind1_e + var ind2_e + timePSCE = log.get("PSCE", "TimeUS") + ind1_e = nearestIndex(timePSCE, t_start*1000000) + ind2_e = nearestIndex(timePSCE, t_end*1000000) + + // Rotate North/East to body frame for lateral and longitudinal axes + let PSCN_TAN = Array.from(log.get("PSCN", "TAN")) + PSCN_TAN = PSCN_TAN.slice(ind1_n, ind2_n) + let PSCN_TVN = Array.from(log.get("PSCN", "DVN")) + PSCN_TVN = PSCN_TVN.slice(ind1_n, ind2_n) + let PSCN_VN = Array.from(log.get("PSCN", "VN")) + PSCN_VN = PSCN_VN.slice(ind1_n, ind2_n) + let PSCN_TPN = Array.from(log.get("PSCN", "DVN")) + PSCN_TPN = PSCN_TPN.slice(ind1_n, ind2_n) + let PSCN_PN = Array.from(log.get("PSCN", "VN")) + PSCN_PN = PSCN_PN.slice(ind1_n, ind2_n) + + let PSCE_TAE = Array.from(log.get("PSCE", "TAE")) + PSCE_TAE = PSCE_TAE.slice(ind1_e, ind2_e) + let PSCE_TVE = Array.from(log.get("PSCE", "DVE")) + PSCE_TVE = PSCE_TVE.slice(ind1_e, ind2_e) + let PSCE_VE = Array.from(log.get("PSCE", "VE")) + PSCE_VE = PSCE_VE.slice(ind1_e, ind2_e) + let PSCE_TPE = Array.from(log.get("PSCE", "DVE")) + PSCE_TPE = PSCE_TPE.slice(ind1_e, ind2_e) + let PSCE_PE = Array.from(log.get("PSCE", "VE")) + PSCE_PE = PSCE_PE.slice(ind1_e, ind2_e) + + if (axis == "Lateral") { + // Lateral axis + ActInputData = array_sub(array_scale(PSCE_TAE, coshdg), array_scale(PSCN_TAN, sinhdg)) + RateTgtData = array_sub(array_scale(PSCE_TVE, coshdg), array_scale(PSCN_TVN, sinhdg)) + RateData = array_sub(array_scale(PSCE_VE, coshdg), array_scale(PSCN_VN, sinhdg)) + AttTgtData = array_sub(array_scale(PSCE_TPE, coshdg), array_scale(PSCN_TPN, sinhdg)) + AttData = array_sub(array_scale(PSCE_PE, coshdg), array_scale(PSCN_PN, sinhdg)) + GyroRawData = array_sub(array_scale(PSCE_VE, coshdg), array_scale(PSCN_VN, sinhdg)) + } else { + // Longitudinal axis + ActInputData = array_add(array_scale(PSCN_TAN, coshdg), array_scale(PSCE_TAE, sinhdg)) + RateTgtData = array_add(array_scale(PSCN_TVN, coshdg), array_scale(PSCE_TVE, sinhdg)) + RateData = array_add(array_scale(PSCN_VN, coshdg), array_scale(PSCE_VE, sinhdg)) + AttTgtData = array_add(array_scale(PSCN_TPN, coshdg), array_scale(PSCE_TPE, sinhdg)) + AttData = array_add(array_scale(PSCN_PN, coshdg), array_scale(PSCE_PE, sinhdg)) + GyroRawData = array_add(array_scale(PSCN_VN, coshdg), array_scale(PSCE_VE, sinhdg)) + } + } + var PilotInputData + if (axis == "Vertical-Accel") { + PilotInputData = Array.from(log.get("RATE", "ADes")) + PilotInputData = PilotInputData.slice(ind1_r, ind2_r) + } else if (axis == "Vertical") { + PilotInputData = Array.from(log.get("PSCD", "TAD")) + PilotInputData = PilotInputData.slice(ind1_d, ind2_d) + } else { + PilotInputData = Array.from(log.get("SIDD", "Targ")) + PilotInputData = PilotInputData.slice(ind1_s, ind2_s) + } + + // Pull Targ for input to Attitude Disturbance Rejection Transfer Function + DRBInputData = PilotInputData + // use integrated velocity to remove bias from position data + let intrate = new Array(RateData.length).fill(0); + for (let k=1;k 0) { - if (name.startsWith("FILT" + NEF_num + "_")) { - var value = inputs[v].value; - params += name + "," + param_to_string(value) + "\n"; + if (page_axis != "Longitudinal" && page_axis != "Lateral" && page_axis != "Vertical" && page_axis != "Vertical-Accel") { + NEF_num = document.getElementById(get_rate_param_prefix() + 'NEF').value + NTF_num = document.getElementById(get_rate_param_prefix() + 'NTF').value + if (NEF_num > 0) { + if (name.startsWith("FILT" + NEF_num + "_")) { + var value = inputs[v].value; + params += name + "," + param_to_string(value) + "\n"; + } } - } - if (NTF_num > 0 && NEF_num != NTF_num) { - if (name.startsWith("FILT" + NTF_num + "_")) { + if (NTF_num > 0 && NEF_num != NTF_num) { + if (name.startsWith("FILT" + NTF_num + "_")) { + var value = inputs[v].value; + params += name + "," + param_to_string(value) + "\n"; + } + } + if (name.startsWith("INS_")) { var value = inputs[v].value; params += name + "," + param_to_string(value) + "\n"; } } - if (name.startsWith("INS_")) { - var value = inputs[v].value; - params += name + "," + param_to_string(value) + "\n"; - } if (name.startsWith("SCHED_")) { var value = inputs[v].value; params += name + "," + param_to_string(value) + "\n"; @@ -2009,6 +1871,10 @@ async function load_parameters(file) { // update all hidden params, to be called at init function update_all_hidden() { + // skip if position controller tuning is being conducted + if (page_axis == "Lateral" || page_axis == "Longitudinal" || page_axis == "Vertical" || page_axis == "Vertical-Accel") { + return; + } var enable_params = ["INS_HNTCH_ENABLE", "INS_HNTC2_ENABLE"]; for (var i=-0;i 3) { - show_set_calc = false - } - pred_data = pred_freq_resp.pilotctrl_H - pred_data_coh = calc_freq_resp.bareAC_coh - show_set_pred = true - } else if (document.getElementById("type_Sys_Stab").checked) { - calc_data = calc_freq_resp.sysbl_H // entire control system stability - calc_data_coh = calc_freq_resp.sysbl_coh - if (sid_axis < 10 || sid_axis > 12) { - show_set_calc = false - } - pred_data = pred_freq_resp.sysbl_H // attitude stability - pred_data_coh = calc_freq_resp.bareAC_coh - show_set_pred = true - } else if (document.getElementById("type_Att_Stab").checked) { - calc_data = calc_freq_resp.sysbl_H // entire control system stability - calc_data_coh = calc_freq_resp.sysbl_coh - show_set_calc = false - pred_data = pred_freq_resp.attbl_H // attitude stability - pred_data_coh = calc_freq_resp.bareAC_coh - show_set_pred = true - } else if (document.getElementById("type_Rate_Stab").checked) { - calc_data = calc_freq_resp.sysbl_H // entire control system stability - calc_data_coh = calc_freq_resp.sysbl_coh - show_set_calc = false - pred_data = pred_freq_resp.ratebl_H // attitude stability - pred_data_coh = calc_freq_resp.bareAC_coh - show_set_pred = true - } else if (document.getElementById("type_Att_DRB").checked) { - calc_data = calc_freq_resp.DRB_H // calculated disturbance rejection - calc_data_coh = calc_freq_resp.DRB_coh // calculated disturbance rejection coherence - if (sid_axis < 4 || sid_axis > 6) { - show_set_calc = false - } - pred_data = pred_freq_resp.DRB_H // predicted disturbance rejection - pred_data_coh = calc_freq_resp.bareAC_coh - show_set_pred = true - } else if (document.getElementById("type_Att_Ctrlr_nff").checked) { - calc_data = calc_freq_resp.attctrl_H - calc_data_coh = calc_freq_resp.attctrl_coh - if (sid_axis < 4 || (sid_axis > 6 && sid_axis < 20) || sid_axis > 22) { - show_set_calc = false - } - pred_data = pred_freq_resp.attctrl_nff_H // attitude controller without feedforward - pred_data_coh = calc_freq_resp.bareAC_coh - show_set_pred = true - } else if (document.getElementById("type_Att_Ctrlr").checked) { - calc_data = calc_freq_resp.attctrl_H - calc_data_coh = calc_freq_resp.attctrl_coh - if ((sid_axis > 3 && sid_axis < 7) || (sid_axis > 9 && sid_axis < 20) ) { //|| sid_axis > 22) { - show_set_calc = false - } - pred_data = pred_freq_resp.attctrl_ff_H // attitude controller with feedforward - pred_data_coh = calc_freq_resp.bareAC_coh - show_set_pred = true - } else if (document.getElementById("type_Rate_Ctrlr").checked) { - calc_data = calc_freq_resp.ratectrl_H - calc_data_coh = calc_freq_resp.ratectrl_coh - if (sid_axis > 9 && sid_axis < 20) { - show_set_calc = false - } - pred_data = pred_freq_resp.ratectrl_H - pred_data_coh = calc_freq_resp.bareAC_coh - show_set_pred = true - } else { - calc_data = calc_freq_resp.bareAC_H - calc_data_coh = calc_freq_resp.bareAC_coh - show_set_calc = true - pred_data = pred_freq_resp.ratectrl_H - show_set_pred = false - } + [calc_data, calc_data_coh, pred_data, pred_data_coh, show_set_calc, show_set_pred] = get_plotted_frequency_response() + console.log(pred_data, show_set_pred) // Apply selected scale, set to y axis fft_plot.data[0].y = amplitude_scale.scale(complex_abs(calc_data)) @@ -2265,6 +2058,110 @@ function redraw_freq_resp() { console.log(`freq response redraw took: ${end - start} ms`); } +function get_plotted_frequency_response() { + var calc_fr + var calc_fr_coh + var pred_fr + var pred_fr_coh + var show_calc = true + var show_pred = true + + + if (document.getElementById("type_Pilot_Ctrlr" + get_page_suffix()).checked) { + calc_fr = calc_freq_resp.pilotctrl_H + calc_fr_coh = calc_freq_resp.pilotctrl_coh + if (sid_axis > 3) { + show_calc = false + } + pred_fr = pred_freq_resp.pilotctrl_H + pred_fr_coh = calc_freq_resp.bareAC_coh + show_pred = true + console.log("pilot ctrlr selected") + } else if (document.getElementById("type_Sys_Stab" + get_page_suffix()).checked) { + calc_fr = calc_freq_resp.sysbl_H // entire control system stability + calc_fr_coh = calc_freq_resp.sysbl_coh + if (sid_axis < 10 || sid_axis > 12) { + show_calc = false + } + pred_fr = pred_freq_resp.sysbl_H // attitude stability + pred_fr_coh = calc_freq_resp.bareAC_coh + show_pred = true + console.log("sys stab selected") + } else if (document.getElementById("type_Att_Stab" + get_page_suffix()).checked) { + calc_fr = calc_freq_resp.sysbl_H // entire control system stability + calc_fr_coh = calc_freq_resp.sysbl_coh + show_calc = false + pred_fr = pred_freq_resp.attbl_H // attitude stability + pred_fr_coh = calc_freq_resp.bareAC_coh + show_pred = true + console.log("att stab selected") + } else if (document.getElementById("type_Rate_Stab" + get_page_suffix()).checked) { + calc_fr = calc_freq_resp.sysbl_H // entire control system stability + calc_fr_coh = calc_freq_resp.sysbl_coh + show_calc = false + pred_fr = pred_freq_resp.ratebl_H // attitude stability + pred_fr_coh = calc_freq_resp.bareAC_coh + show_pred = true + console.log("rate stab selected") + } else if (document.getElementById("type_Att_DRB" + get_page_suffix()).checked) { + calc_fr = calc_freq_resp.DRB_H // calculated disturbance rejection + calc_fr_coh = calc_freq_resp.DRB_coh // calculated disturbance rejection coherence + if (sid_axis < 4 || (sid_axis > 6 && sid_axis < 14) || sid_axis > 17) { + show_calc = false + } + pred_fr = pred_freq_resp.DRB_H // predicted disturbance rejection + pred_fr_coh = calc_freq_resp.bareAC_coh + show_pred = true + console.log("att DRB selected") + } else if (document.getElementById("type_Att_Ctrlr" + get_page_suffix()).checked) { + calc_fr = calc_freq_resp.attctrl_H + calc_fr_coh = calc_freq_resp.attctrl_coh + if ((sid_axis > 3 && sid_axis < 7) || (sid_axis > 9 && sid_axis < 20) ) { //|| sid_axis > 22) { + show_calc = false + } + pred_fr = pred_freq_resp.attctrl_ff_H // attitude controller with feedforward + pred_fr_coh = calc_freq_resp.bareAC_coh + show_pred = true + console.log("att ctrlr selected") + } else if (document.getElementById("type_Rate_Ctrlr" + get_page_suffix()).checked) { + calc_fr = calc_freq_resp.ratectrl_H + calc_fr_coh = calc_freq_resp.ratectrl_coh + if (sid_axis > 9 && sid_axis < 14) { + show_calc = false + } + pred_fr = pred_freq_resp.ratectrl_H + pred_fr_coh = calc_freq_resp.bareAC_coh + show_pred = true + console.log("rate ctrlr selected") + } else if (document.getElementById("type_Bare_AC" + get_page_suffix()).checked) { + calc_fr = calc_freq_resp.bareAC_H + calc_fr_coh = calc_freq_resp.bareAC_coh + show_calc = true + pred_fr = pred_freq_resp.ratectrl_H + show_pred = false + console.log("bare AC selected") + } else if (document.getElementById("type_Att_Ctrlr_nff" + get_page_suffix()).checked) { + calc_fr = calc_freq_resp.attctrl_H + calc_fr_coh = calc_freq_resp.attctrl_coh + if (sid_axis < 4 || (sid_axis > 6 && sid_axis < 18) || (sid_axis > 21 && sid_axis < 24)) { + show_calc = false + } + pred_fr = pred_freq_resp.attctrl_nff_H // attitude controller without feedforward + pred_fr_coh = calc_freq_resp.bareAC_coh + show_pred = true + console.log("att ctrlr nff selected") + } else { + calc_fr = calc_freq_resp.bareAC_H + calc_fr_coh = calc_freq_resp.bareAC_coh + show_calc = true + pred_fr = pred_freq_resp.ratectrl_H + show_pred = false + console.log("default selected") + } + return [calc_fr, calc_fr_coh, pred_fr, pred_fr_coh, show_calc, show_pred] + +} + function add_sid_sets() { let fieldset = document.getElementById("sid_sets") @@ -2342,11 +2239,9 @@ function add_sid_sets() { 19: "Input Longitudinal Velocity", 20: "FW Input Roll Angle", 21: "FW Input Pitch Angle", - 22: "FW Input Yaw Angle", - 23: "FW Mixer Roll", - 24: "FW Mixer Pitch", - 25: "FW Mixer Yaw", - 26: "FW Mixer Thrust" + 22: "FW Mixer Roll", + 23: "FW Mixer Pitch", + 24: "Input Vertical Velocity" } const num_sets = sid_sets.axis.length @@ -2428,15 +2323,22 @@ function update_time_range() { function set_sid_axis(axis) { - if (axis == 1 || axis == 4 || axis == 7 || axis == 10 || axis == 20 || axis == 23) { + if (axis == 1 || axis == 4 || axis == 7 || axis == 10 || axis == 20 || axis == 22) { page_axis = "Roll" - } else if (axis == 2 || axis == 5 || axis == 8 || axis == 11 || axis == 21 || axis == 24) { + } else if (axis == 2 || axis == 5 || axis == 8 || axis == 11 || axis == 21 || axis == 23) { page_axis = "Pitch" - } else if (axis == 3 || axis == 6 || axis == 9 || axis == 12 || axis == 22 || axis == 25) { + } else if (axis == 3 || axis == 6 || axis == 9 || axis == 12) { page_axis = "Yaw" + } else if (axis == 24) { + page_axis = "Vertical" + } else if (axis == 13) { + page_axis = "Vertical-Accel" + } else if (axis == 14 || axis == 16 || axis == 18) { + page_axis = "Lateral" + } else if (axis == 15 || axis == 17 || axis == 19) { + page_axis = "Longitudinal" } sid_axis = axis - axis_changed() } function get_vehicle_atc_prefix() { @@ -2457,10 +2359,36 @@ function get_vehicle_plt_prefix() { return "" } +function get_vehicle_type() { + return vehicle_type +} + +function get_page_axis() { + return page_axis +} + function get_rate_param_prefix() { var prefix = "" if (vehicle_type == "ArduPlane_FW") { prefix = get_axis_prefix() + "_RATE_"; + } else if (page_axis == "Lateral" || page_axis == "Longitudinal") { + if (vehicle_type == "ArduPlane_VTOL") { + prefix = "Q_P_VELXY_" + } else { + prefix = "PSC_VELXY_" + } + } else if (page_axis == "Vertical") { + if (vehicle_type == "ArduPlane_VTOL") { + prefix = "Q_P_VELZ_" + } else { + prefix = "PSC_VELZ_" + } + } else if (page_axis == "Vertical-Accel") { + if (vehicle_type == "ArduPlane_VTOL") { + prefix = "Q_P_ACCZ_" + } else { + prefix = "PSC_ACCZ_" + } } else { prefix = get_vehicle_atc_prefix() + "RAT_" + get_axis_prefix() + "_"; } @@ -2471,8 +2399,32 @@ function get_angle_param_prefix() { var prefix = "" if (vehicle_type == "ArduPlane_FW") { prefix = get_axis_prefix() + "2SRV_"; + } else if (page_axis == "Lateral" || page_axis == "Longitudinal") { + if (vehicle_type == "ArduPlane_VTOL") { + prefix = "Q_P_POSXY_" + } else { + prefix = "PSC_POSXY_"; + } + } else if (page_axis == "Vertical" || page_axis == "Vertical-Accel") { + if (vehicle_type == "ArduPlane_VTOL") { + prefix = "Q_P_POSZ_" + } else { + prefix = "PSC_POSZ_"; + } } else { prefix = get_vehicle_atc_prefix() + "ANG_" + get_axis_prefix() + "_"; } return prefix -} \ No newline at end of file +} + +function get_page_suffix() { + var suffix = "" + if (vehicle_type == "ArduPlane_FW") { + suffix = "_FW"; + } else if (page_axis == "Lateral" || page_axis == "Longitudinal") { + suffix = "_POS"; + } else if (page_axis == "Vertical-Accel" || page_axis == "Vertical") { + suffix = "_POS_Acc"; + } + return suffix +} diff --git a/AnalyticTune/DiscreteFilter.js b/AnalyticTune/DiscreteFilter.js new file mode 100644 index 00000000..931288f2 --- /dev/null +++ b/AnalyticTune/DiscreteFilter.js @@ -0,0 +1,590 @@ +function PID(sample_rate,kP,kI,kD,filtE,filtD) { + this.sample_rate = sample_rate + + this._kP = kP; + this._kI = kI; + this._kD = kD; + + this.E_filter = new LPF_1P(sample_rate, filtE) + this.D_filter = new LPF_1P(sample_rate, filtD) + + this.transfer = function(Z, Z1, Z2, use_dB, unwrap_phase) { + const E_trans = this.E_filter.transfer(Z, Z1, Z2, false, false) + const D_trans = complex_mul(E_trans, this.D_filter.transfer(Z, Z1, Z2, false, false)) + + // I term is k*z / (z - 1) + const Z_less_one = [array_offset(Z[0], -1), Z[1].slice()] + const I_comp = complex_mul(complex_div(Z,Z_less_one), E_trans) + const kI = this._kI/this.sample_rate + + // D term is k * (1 - Z^-1) + const one_less_Z1 = [array_offset(array_scale(Z1[0],-1), 1), array_scale(Z1[1],-1)] + const D_comp = complex_mul(one_less_Z1, D_trans) + const kD = this._kD*this.sample_rate + + + const len = Z1[0].length + let ret = [new Array(len), new Array(len)] + let P = [new Array(len), new Array(len)] + let I = [new Array(len), new Array(len)] + let D = [new Array(len), new Array(len)] + for (let i = 0; i 0.0) && (this.center_freq_hz < 0.5 * this.sample_rate) && (this.Q > 0.0)) { + this.A = Math.pow(10.0, -this.attenuation_dB / 40.0); + var omega = 2.0 * Math.PI * this.center_freq_hz / this.sample_rate; + var alpha = Math.sin(omega) / (2 * this.Q); + this.b0 = 1.0 + alpha*(this.A**2); + this.b1 = -2.0 * Math.cos(omega); + this.b2 = 1.0 - alpha*(this.A**2); + this.a0_inv = 1.0/(1.0 + alpha); + this.a1 = this.b1; + this.a2 = 1.0 - alpha; + this.initialised = true; + } else { + this.initialised = false; + } + + this.transfer = function(Z, Z1, Z2, use_dB, unwrap_phase) { + if (!this.initialised) { + const len = Z1[0].length + return [new Array(len).fill(1), new Array(len).fill(0)] + } + + const a0 = 1 / this.a0_inv + + const len = Z1[0].length + let numerator = [new Array(len), new Array(len)] + let denominator = [new Array(len), new Array(len)] + for (let i = 0; i 0.5 * this.bandwidth_hz) { + var octaves = Math.log2(this.center_freq_hz / (this.center_freq_hz - this.bandwidth_hz / 2.0)) * 2.0; + this.Q = Math.sqrt(Math.pow(2.0, octaves)) / (Math.pow(2.0, octaves) - 1.0); + } else { + this.Q = 0.0; + } + } + + this.init_with_A_and_Q = function() { + if ((this.center_freq_hz > 0.0) && (this.center_freq_hz < 0.5 * this.sample_freq) && (this.Q > 0.0)) { + var omega = 2.0 * Math.PI * this.center_freq_hz / this.sample_freq; + var alpha = Math.sin(omega) / (2 * this.Q); + this.b0 = 1.0 + alpha*(this.A**2); + this.b1 = -2.0 * Math.cos(omega); + this.b2 = 1.0 - alpha*(this.A**2); + this.a0_inv = 1.0/(1.0 + alpha); + this.a1 = this.b1; + this.a2 = 1.0 - alpha; + this.initialised = true; + } else { + this.initialised = false; + } + } + + // check center frequency is in the allowable range + if ((center_freq_hz > 0.5 * bandwidth_hz) && (center_freq_hz < 0.5 * sample_freq)) { + this.calculate_A_and_Q(); + this.init_with_A_and_Q(); + } else { + this.initialised = false; + } + + this.transfer = function(Z, Z1, Z2) { + if (!this.initialised) { + const len = Z1[0].length + return [new Array(len).fill(1), new Array(len).fill(0)] + } + + const a0 = 1 / this.a0_inv + + const len = Z1[0].length + let numerator = [new Array(len), new Array(len)] + let denominator = [new Array(len), new Array(len)] + for (let i = 0; i 1) { + var notch_center_double; + // only enable the filter if its center frequency is below the nyquist frequency + notch_center_double = notch_center * (1.0 - notch_spread); + if (notch_center_double < nyquist_limit) { + this.notches.push(new NotchFilter(sample_freq,notch_center_double,bandwidth_hz/composite_notches,att)); + } + // only enable the filter if its center frequency is below the nyquist frequency + notch_center_double = notch_center * (1.0 + notch_spread); + if (notch_center_double < nyquist_limit) { + this.notches.push(new NotchFilter(sample_freq,notch_center_double,bandwidth_hz/composite_notches,att)); + } + } + } + } + } + + this.transfer = function(Z, Z1, Z2, use_dB, unwrap_phase) { + const len = Z1[0].length + var H_total = [new Array(len).fill(1), new Array(len).fill(0)] + for (n in this.notches) { + const H = this.notches[n].transfer(Z, Z1, Z2); + H_total = complex_mul(H_total, H) + } + + this.attenuation = complex_abs(H_total) + this.phase = array_scale(complex_phase(H_total), 180/Math.PI) + if (use_dB) { + this.attenuation = array_scale(array_log10(this.attenuation), 20.0) + } + if (unwrap_phase) { + this.phase = unwrap(this.phase) + } + + return H_total; + } +} + +function get_filters(sample_rate) { + var filters = [] + filters.push(new HarmonicNotchFilter(sample_rate, + get_form("INS_HNTCH_ENABLE"), + get_form("INS_HNTCH_MODE"), + get_form("INS_HNTCH_FREQ"), + get_form("INS_HNTCH_BW"), + get_form("INS_HNTCH_ATT"), + get_form("INS_HNTCH_REF"), + get_form("INS_HNTCH_FM_RAT"), + get_form("INS_HNTCH_HMNCS"), + get_form("INS_HNTCH_OPTS"))); + filters.push(new HarmonicNotchFilter(sample_rate, + get_form("INS_HNTC2_ENABLE"), + get_form("INS_HNTC2_MODE"), + get_form("INS_HNTC2_FREQ"), + get_form("INS_HNTC2_BW"), + get_form("INS_HNTC2_ATT"), + get_form("INS_HNTC2_REF"), + get_form("INS_HNTC2_FM_RAT"), + get_form("INS_HNTC2_HMNCS"), + get_form("INS_HNTC2_OPTS"))); + filters.push(new DigitalBiquadFilter(sample_rate,get_form("INS_GYRO_FILTER"))); + + return filters; +} + +// Unwrap phase by looking for jumps of larger than 180 deg +function unwrap(phase) { + const len = phase.length + + // Notches result in large positive phase changes, bias the unwrap to do a better job + const neg_threshold = 45 + const pos_threshold = 360 - neg_threshold + + let unwrapped = new Array(len) + + unwrapped[0] = phase[0] + for (let i = 1; i < len; i++) { + let phase_diff = phase[i] - phase[i-1]; + if (phase_diff > pos_threshold) { + phase_diff -= 360.0; + } else if (phase_diff < -neg_threshold) { + phase_diff += 360.0; + } + unwrapped[i] = unwrapped[i-1] + phase_diff + } + + return unwrapped +} + +function evaluate_transfer_functions(filter_groups, freq_max, freq_step, use_dB, unwrap_phase) { + + console.log("Evaluating transfer functions",freq_step,freq_max) + // Not sure why range does not return expected array, _data gets us the array + const freq = array_from_range(freq_step, freq_max, freq_step) + + // Start with unity transfer function, input = output + const len = freq.length + var H_total = [new Array(len).fill(1), new Array(len).fill(0)] + + for (let i = 0; i < filter_groups.length; i++) { + // Allow for batches at different sample rates + const filters = filter_groups[i] + + const sample_rate = filters[0].sample_rate + for (let j = 1; j < filters.length; j++) { + if (filters[0].sample_rate != sample_rate) { + error("Sample rate miss match") + } + } + + // Calculate Z for transfer function + // Z = e^jw + const Z = exp_jw(freq, sample_rate) + + // Z^-1 + const Z1 = complex_inverse(Z) + + // Z^-2 + const Z2 = complex_inverse(complex_square(Z)) + + // Apply all transfer functions + for (let filter of filters) { + const H = filter.transfer(Z, Z1, Z2, use_dB, unwrap_phase) + H_total = complex_mul(H_total, H) + } + } + + // Calculate total filter transfer function + let attenuation = complex_abs(H_total) + let phase = array_scale(complex_phase(H_total), 180/Math.PI) + if (use_dB) { + attenuation = array_scale(array_log10(attenuation), 20.0) + } + if (unwrap_phase) { + phase = unwrap(phase) + } + + // Return attenuation and phase + return { attenuation: attenuation, phase: phase, freq: freq, H_total: H_total} +} diff --git a/AnalyticTune/index.html b/AnalyticTune/index.html index cde68ce3..68625f3e 100644 --- a/AnalyticTune/index.html +++ b/AnalyticTune/index.html @@ -5,6 +5,7 @@ ArduCopter Analytic Tune Tool + @@ -37,7 +38,7 @@

ArduCopter Analytic Tune Tool

- + @@ -91,13 +92,13 @@

ArduCopter Analytic

- +

Flight Data

- +

@@ -105,709 +106,1130 @@

Flight Data

-

+ +
-
-
- INS Settings -

- - -

-

- -

-
- - - - - -
-
- First Notch Filter -

- -

-

- -

-

- -

-

- -

-

- -

-

- -

-

- -

-

- -

-

- -

-
- 		-
- Second Notch Filter -

- -

-

- -

-

- -

-

- -

-

- -

-

- -

-

- -

-

- -

-

- -

-
-
-
- Loop Rate -

- -

-
+ - + \ No newline at end of file diff --git a/AnalyticTune/params.json b/AnalyticTune/params.json index 0aa7cc80..e5de7db7 100644 --- a/AnalyticTune/params.json +++ b/AnalyticTune/params.json @@ -1,4 +1,644 @@ { + "PSC": { + "PSC_ACCZ_D": { + "Description": "Acceleration (vertical) controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration", + "DisplayName": "Acceleration (vertical) controller D gain", + "Range": { + "high": "0.400", + "low": "0.000" + }, + "User": "Standard" + }, + "PSC_ACCZ_D_FF": { + "Description": "FF D Gain which produces an output that is proportional to the rate of change of the target", + "DisplayName": "Accel (vertical) Derivative FeedForward Gain", + "Increment": "0.0001", + "Range": { + "high": "0.02", + "low": "0" + }, + "User": "Advanced" + }, + "PSC_ACCZ_FF": { + "Description": "Acceleration (vertical) controller feed forward", + "DisplayName": "Acceleration (vertical) controller feed forward", + "Increment": "0.001", + "Range": { + "high": "0.5", + "low": "0" + }, + "User": "Standard" + }, + "PSC_ACCZ_FLTD": { + "Description": "Acceleration (vertical) controller derivative frequency in Hz", + "DisplayName": "Acceleration (vertical) controller derivative frequency in Hz", + "Increment": "1", + "Range": { + "high": "100", + "low": "1" + }, + "Units": "Hz", + "User": "Standard" + }, + "PSC_ACCZ_FLTE": { + "Description": "Acceleration (vertical) controller error frequency in Hz", + "DisplayName": "Acceleration (vertical) controller error frequency in Hz", + "Increment": "1", + "Range": { + "high": "100", + "low": "1" + }, + "Units": "Hz", + "User": "Standard" + }, + "PSC_ACCZ_FLTT": { + "Description": "Acceleration (vertical) controller target frequency in Hz", + "DisplayName": "Acceleration (vertical) controller target frequency in Hz", + "Increment": "1", + "Range": { + "high": "50", + "low": "1" + }, + "Units": "Hz", + "User": "Standard" + }, + "PSC_ACCZ_I": { + "Description": "Acceleration (vertical) controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration", + "DisplayName": "Acceleration (vertical) controller I gain", + "Range": { + "high": "3.000", + "low": "0.000" + }, + "User": "Standard" + }, + "PSC_ACCZ_IMAX": { + "Description": "Acceleration (vertical) controller I gain maximum. Constrains the maximum pwm that the I term will generate", + "DisplayName": "Acceleration (vertical) controller I gain maximum", + "Range": { + "high": "1000", + "low": "0" + }, + "Units": "d%", + "User": "Standard" + }, + "PSC_ACCZ_NEF": { + "Description": "Accel (vertical) Error notch filter index", + "DisplayName": "Accel (vertical) Error notch filter index", + "Range": { + "high": "8", + "low": "1" + }, + "User": "Advanced" + }, + "PSC_ACCZ_NTF": { + "Description": "Accel (vertical) Target notch filter index", + "DisplayName": "Accel (vertical) Target notch filter index", + "Range": { + "high": "8", + "low": "1" + }, + "User": "Advanced" + }, + "PSC_ACCZ_P": { + "Description": "Acceleration (vertical) controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output", + "DisplayName": "Acceleration (vertical) controller P gain", + "Increment": "0.05", + "Range": { + "high": "1.500", + "low": "0.200" + }, + "User": "Standard" + }, + "PSC_ACCZ_PDMX": { + "Description": "Acceleration (vertical) controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output", + "DisplayName": "Acceleration (vertical) controller PD sum maximum", + "Range": { + "high": "1000", + "low": "0" + }, + "Units": "d%" + }, + "PSC_ACCZ_SMAX": { + "Description": "Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.", + "DisplayName": "Accel (vertical) slew rate limit", + "Increment": "0.5", + "Range": { + "high": "200", + "low": "0" + }, + "User": "Advanced" + }, + "PSC_ANGLE_MAX": { + "Description": "Maximum lean angle autopilot can request. Set to zero to use ANGLE_MAX parameter value", + "DisplayName": "Position Control Angle Max", + "Increment": "1", + "Range": { + "high": "45", + "low": "0" + }, + "Units": "deg", + "User": "Advanced" + }, + "PSC_JERK_XY": { + "Description": "Jerk limit of the horizontal kinematic path generation used to determine how quickly the aircraft varies the acceleration target", + "DisplayName": "Jerk limit for the horizontal kinematic input shaping", + "Increment": "1", + "Range": { + "high": "20", + "low": "1" + }, + "Units": "m/s/s/s", + "User": "Advanced" + }, + "PSC_JERK_Z": { + "Description": "Jerk limit of the vertical kinematic path generation used to determine how quickly the aircraft varies the acceleration target", + "DisplayName": "Jerk limit for the vertical kinematic input shaping", + "Increment": "1", + "Range": { + "high": "50", + "low": "5" + }, + "Units": "m/s/s/s", + "User": "Advanced" + }, + "PSC_POSXY_P": { + "Description": "Position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller", + "DisplayName": "Position (horizontal) controller P gain", + "Range": { + "high": "2.000", + "low": "0.500" + }, + "User": "Standard" + }, + "PSC_POSZ_P": { + "Description": "Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller", + "DisplayName": "Position (vertical) controller P gain", + "Range": { + "high": "3.000", + "low": "1.000" + }, + "User": "Standard" + }, + "PSC_VELXY_D": { + "Description": "Velocity (horizontal) D gain. Corrects short-term changes in velocity", + "DisplayName": "Velocity (horizontal) D gain", + "Increment": "0.001", + "Range": { + "high": "1.00", + "low": "0.00" + }, + "User": "Advanced" + }, + "PSC_VELXY_FF": { + "Description": "Velocity (horizontal) feed forward gain. Converts the difference between desired velocity to a target acceleration", + "DisplayName": "Velocity (horizontal) feed forward gain", + "Increment": "0.01", + "Range": { + "high": "6", + "low": "0" + }, + "User": "Advanced" + }, + "PSC_VELXY_FLTD": { + "Description": "Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for D term", + "DisplayName": "Velocity (horizontal) input filter", + "Range": { + "high": "100", + "low": "0" + }, + "Units": "Hz", + "User": "Advanced" + }, + "PSC_VELXY_FLTE": { + "Description": "Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms", + "DisplayName": "Velocity (horizontal) input filter", + "Range": { + "high": "100", + "low": "0" + }, + "Units": "Hz", + "User": "Advanced" + }, + "PSC_VELXY_I": { + "Description": "Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration", + "DisplayName": "Velocity (horizontal) I gain", + "Increment": "0.01", + "Range": { + "high": "1.00", + "low": "0.02" + }, + "User": "Advanced" + }, + "PSC_VELXY_IMAX": { + "Description": "Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output", + "DisplayName": "Velocity (horizontal) integrator maximum", + "Increment": "10", + "Range": { + "high": "4500", + "low": "0" + }, + "Units": "cm/s/s", + "User": "Advanced" + }, + "PSC_VELXY_P": { + "Description": "Velocity (horizontal) P gain. Converts the difference between desired and actual velocity to a target acceleration", + "DisplayName": "Velocity (horizontal) P gain", + "Increment": "0.1", + "Range": { + "high": "6.0", + "low": "0.1" + }, + "User": "Advanced" + }, + "PSC_VELZ_D": { + "Description": "Velocity (vertical) controller D gain. Corrects short-term changes in velocity", + "DisplayName": "Velocity (vertical) controller D gain", + "Increment": "0.001", + "Range": { + "high": "1.00", + "low": "0.00" + }, + "User": "Advanced" + }, + "PSC_VELZ_FF": { + "Description": "Velocity (vertical) controller Feed Forward gain. Produces an output that is proportional to the magnitude of the target", + "DisplayName": "Velocity (vertical) controller Feed Forward gain", + "Increment": "0.01", + "Range": { + "high": "1", + "low": "0" + }, + "User": "Advanced" + }, + "PSC_VELZ_FLTD": { + "Description": "Velocity (vertical) input filter for D term. This filter (in Hz) is applied to the input for D terms", + "DisplayName": "Velocity (vertical) input filter for D term", + "Range": { + "high": "100", + "low": "0" + }, + "Units": "Hz", + "User": "Advanced" + }, + "PSC_VELZ_FLTE": { + "Description": "Velocity (vertical) error filter. This filter (in Hz) is applied to the input for P and I terms", + "DisplayName": "Velocity (vertical) error filter", + "Range": { + "high": "100", + "low": "0" + }, + "Units": "Hz", + "User": "Advanced" + }, + "PSC_VELZ_I": { + "Description": "Velocity (vertical) controller I gain. Corrects long-term difference in desired velocity to a target acceleration", + "DisplayName": "Velocity (vertical) controller I gain", + "Increment": "0.01", + "Range": { + "high": "1.00", + "low": "0.02" + }, + "User": "Advanced" + }, + "PSC_VELZ_IMAX": { + "Description": "Velocity (vertical) controller I gain maximum. Constrains the target acceleration that the I gain will output", + "DisplayName": "Velocity (vertical) controller I gain maximum", + "Range": { + "high": "8.000", + "low": "1.000" + }, + "User": "Standard" + }, + "PSC_VELZ_P": { + "Description": "Velocity (vertical) controller P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller", + "DisplayName": "Velocity (vertical) controller P gain", + "Range": { + "high": "8.000", + "low": "1.000" + }, + "User": "Standard" + } + }, + "Q_P": { + "Q_P_ACCZ_D": { + "Description": "Acceleration (vertical) controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration", + "DisplayName": "Acceleration (vertical) controller D gain", + "Range": { + "high": "0.400", + "low": "0.000" + }, + "User": "Standard" + }, + "Q_P_ACCZ_D_FF": { + "Description": "FF D Gain which produces an output that is proportional to the rate of change of the target", + "DisplayName": "Accel (vertical) Derivative FeedForward Gain", + "Increment": "0.0001", + "Range": { + "high": "0.02", + "low": "0" + }, + "User": "Advanced" + }, + "Q_P_ACCZ_FF": { + "Description": "Acceleration (vertical) controller feed forward", + "DisplayName": "Acceleration (vertical) controller feed forward", + "Increment": "0.001", + "Range": { + "high": "0.5", + "low": "0" + }, + "User": "Standard" + }, + "Q_P_ACCZ_FLTD": { + "Description": "Acceleration (vertical) controller derivative frequency in Hz", + "DisplayName": "Acceleration (vertical) controller derivative frequency in Hz", + "Increment": "1", + "Range": { + "high": "100", + "low": "1" + }, + "Units": "Hz", + "User": "Standard" + }, + "Q_P_ACCZ_FLTE": { + "Description": "Acceleration (vertical) controller error frequency in Hz", + "DisplayName": "Acceleration (vertical) controller error frequency in Hz", + "Increment": "1", + "Range": { + "high": "100", + "low": "1" + }, + "Units": "Hz", + "User": "Standard" + }, + "Q_P_ACCZ_FLTT": { + "Description": "Acceleration (vertical) controller target frequency in Hz", + "DisplayName": "Acceleration (vertical) controller target frequency in Hz", + "Increment": "1", + "Range": { + "high": "50", + "low": "1" + }, + "Units": "Hz", + "User": "Standard" + }, + "Q_P_ACCZ_I": { + "Description": "Acceleration (vertical) controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration", + "DisplayName": "Acceleration (vertical) controller I gain", + "Range": { + "high": "3.000", + "low": "0.000" + }, + "User": "Standard" + }, + "Q_P_ACCZ_IMAX": { + "Description": "Acceleration (vertical) controller I gain maximum. Constrains the maximum pwm that the I term will generate", + "DisplayName": "Acceleration (vertical) controller I gain maximum", + "Range": { + "high": "1000", + "low": "0" + }, + "Units": "d%", + "User": "Standard" + }, + "Q_P_ACCZ_NEF": { + "Description": "Accel (vertical) Error notch filter index", + "DisplayName": "Accel (vertical) Error notch filter index", + "Range": { + "high": "8", + "low": "1" + }, + "User": "Advanced" + }, + "Q_P_ACCZ_NTF": { + "Description": "Accel (vertical) Target notch filter index", + "DisplayName": "Accel (vertical) Target notch filter index", + "Range": { + "high": "8", + "low": "1" + }, + "User": "Advanced" + }, + "Q_P_ACCZ_P": { + "Description": "Acceleration (vertical) controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output", + "DisplayName": "Acceleration (vertical) controller P gain", + "Increment": "0.05", + "Range": { + "high": "1.500", + "low": "0.200" + }, + "User": "Standard" + }, + "Q_P_ACCZ_PDMX": { + "Description": "Acceleration (vertical) controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output", + "DisplayName": "Acceleration (vertical) controller PD sum maximum", + "Range": { + "high": "1000", + "low": "0" + }, + "Units": "d%" + }, + "Q_P_ACCZ_SMAX": { + "Description": "Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.", + "DisplayName": "Accel (vertical) slew rate limit", + "Increment": "0.5", + "Range": { + "high": "200", + "low": "0" + }, + "User": "Advanced" + }, + "Q_P_ANGLE_MAX": { + "Description": "Maximum lean angle autopilot can request. Set to zero to use ANGLE_MAX parameter value", + "DisplayName": "Position Control Angle Max", + "Increment": "1", + "Range": { + "high": "45", + "low": "0" + }, + "Units": "deg", + "User": "Advanced" + }, + "Q_P_JERK_XY": { + "Description": "Jerk limit of the horizontal kinematic path generation used to determine how quickly the aircraft varies the acceleration target", + "DisplayName": "Jerk limit for the horizontal kinematic input shaping", + "Increment": "1", + "Range": { + "high": "20", + "low": "1" + }, + "Units": "m/s/s/s", + "User": "Advanced" + }, + "Q_P_JERK_Z": { + "Description": "Jerk limit of the vertical kinematic path generation used to determine how quickly the aircraft varies the acceleration target", + "DisplayName": "Jerk limit for the vertical kinematic input shaping", + "Increment": "1", + "Range": { + "high": "50", + "low": "5" + }, + "Units": "m/s/s/s", + "User": "Advanced" + }, + "Q_P_POSXY_P": { + "Description": "Position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller", + "DisplayName": "Position (horizontal) controller P gain", + "Range": { + "high": "2.000", + "low": "0.500" + }, + "User": "Standard" + }, + "Q_P_POSZ_P": { + "Description": "Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller", + "DisplayName": "Position (vertical) controller P gain", + "Range": { + "high": "3.000", + "low": "1.000" + }, + "User": "Standard" + }, + "Q_P_VELXY_D": { + "Description": "Velocity (horizontal) D gain. Corrects short-term changes in velocity", + "DisplayName": "Velocity (horizontal) D gain", + "Increment": "0.001", + "Range": { + "high": "1.00", + "low": "0.00" + }, + "User": "Advanced" + }, + "Q_P_VELXY_FF": { + "Description": "Velocity (horizontal) feed forward gain. Converts the difference between desired velocity to a target acceleration", + "DisplayName": "Velocity (horizontal) feed forward gain", + "Increment": "0.01", + "Range": { + "high": "6", + "low": "0" + }, + "User": "Advanced" + }, + "Q_P_VELXY_FLTD": { + "Description": "Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for D term", + "DisplayName": "Velocity (horizontal) input filter", + "Range": { + "high": "100", + "low": "0" + }, + "Units": "Hz", + "User": "Advanced" + }, + "Q_P_VELXY_FLTE": { + "Description": "Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms", + "DisplayName": "Velocity (horizontal) input filter", + "Range": { + "high": "100", + "low": "0" + }, + "Units": "Hz", + "User": "Advanced" + }, + "Q_P_VELXY_I": { + "Description": "Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration", + "DisplayName": "Velocity (horizontal) I gain", + "Increment": "0.01", + "Range": { + "high": "1.00", + "low": "0.02" + }, + "User": "Advanced" + }, + "Q_P_VELXY_IMAX": { + "Description": "Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output", + "DisplayName": "Velocity (horizontal) integrator maximum", + "Increment": "10", + "Range": { + "high": "4500", + "low": "0" + }, + "Units": "cm/s/s", + "User": "Advanced" + }, + "Q_P_VELXY_P": { + "Description": "Velocity (horizontal) P gain. Converts the difference between desired and actual velocity to a target acceleration", + "DisplayName": "Velocity (horizontal) P gain", + "Increment": "0.1", + "Range": { + "high": "6.0", + "low": "0.1" + }, + "User": "Advanced" + }, + "Q_P_VELZ_D": { + "Description": "Velocity (vertical) controller D gain. Corrects short-term changes in velocity", + "DisplayName": "Velocity (vertical) controller D gain", + "Increment": "0.001", + "Range": { + "high": "1.00", + "low": "0.00" + }, + "User": "Advanced" + }, + "Q_P_VELZ_FF": { + "Description": "Velocity (vertical) controller Feed Forward gain. Produces an output that is proportional to the magnitude of the target", + "DisplayName": "Velocity (vertical) controller Feed Forward gain", + "Increment": "0.01", + "Range": { + "high": "1", + "low": "0" + }, + "User": "Advanced" + }, + "Q_P_VELZ_FLTD": { + "Description": "Velocity (vertical) input filter for D term. This filter (in Hz) is applied to the input for D terms", + "DisplayName": "Velocity (vertical) input filter for D term", + "Range": { + "high": "100", + "low": "0" + }, + "Units": "Hz", + "User": "Advanced" + }, + "Q_P_VELZ_FLTE": { + "Description": "Velocity (vertical) error filter. This filter (in Hz) is applied to the input for P and I terms", + "DisplayName": "Velocity (vertical) error filter", + "Range": { + "high": "100", + "low": "0" + }, + "Units": "Hz", + "User": "Advanced" + }, + "Q_P_VELZ_I": { + "Description": "Velocity (vertical) controller I gain. Corrects long-term difference in desired velocity to a target acceleration", + "DisplayName": "Velocity (vertical) controller I gain", + "Increment": "0.01", + "Range": { + "high": "1.00", + "low": "0.02" + }, + "User": "Advanced" + }, + "Q_P_VELZ_IMAX": { + "Description": "Velocity (vertical) controller I gain maximum. Constrains the target acceleration that the I gain will output", + "DisplayName": "Velocity (vertical) controller I gain maximum", + "Range": { + "high": "8.000", + "low": "1.000" + }, + "User": "Standard" + }, + "Q_P_VELZ_P": { + "Description": "Velocity (vertical) controller P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller", + "DisplayName": "Velocity (vertical) controller P gain", + "Range": { + "high": "8.000", + "low": "1.000" + }, + "User": "Standard" + } + }, "ATC_": { "ATC_ACCEL_P_MAX": { "Description": "Maximum acceleration in pitch axis", @@ -742,6 +1382,20 @@ "__field_text": "\n // @DisplayName: Pilot yaw rate control input time constant\n // @Description: Pilot yaw rate control input time constant\n // @Range: 0.01 0.5\n // @User: Advanced" } }, + "Q_PLT": { + "Q_PLT_Y_RATE_TC": { + "Description": "Pilot yaw rate control input time constant. Low numbers lead to sharper response, higher numbers to softer response.", + "DisplayName": "Pilot yaw rate control input time constant", + "Increment": "0.01", + "Range": { + "high": "1", + "low": "0" + }, + "Units": "s", + "User": "Standard", + "__field_text": "\n // @DisplayName: Pilot yaw rate control input time constant\n // @Description: Pilot yaw rate control input time constant\n // @Range: 0.01 0.5\n // @User: Advanced" + } + }, "INS": { "INS_ACC1_CALTEMP": { "Calibration": "1", @@ -2680,17 +3334,6 @@ "__field_text": "\n // @DisplayName: Throttle Mix Minimum\n // @Description: Throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)\n // @Range: 0.1 0.25\n // @User: Advanced" } }, - "Q_PLT_Y_RATE_TC": { - "Description": "Pilot yaw rate control input time constant. Low numbers lead to sharper response, higher numbers to softer response.", - "DisplayName": "Pilot yaw rate control input time constant", - "Increment": "0.01", - "Range": { - "high": "1", - "low": "0" - }, - "Units": "s", - "User": "Standard" - }, "PTCH": { "PTCH2SRV_RLL": { "Description": "Gain added to pitch to keep aircraft from descending or ascending in turns. Increase in increments of 0.05 to reduce altitude loss. Decrease for altitude gain.", @@ -3212,6 +3855,6 @@ } }, "json": { - "version": 0 + "version": 1 } } \ No newline at end of file