forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtuning.cpp
228 lines (183 loc) · 6.96 KB
/
tuning.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
#include "Copter.h"
/*
* Function to update various parameters in flight using the ch6 tuning knob
* This should not be confused with the AutoTune feature which can bve found in control_autotune.cpp
*/
// tuning - updates parameters based on the ch6 tuning knob's position
// should be called at 3.3hz
void Copter::tuning() {
RC_Channel *rc6 = RC_Channels::rc_channel(CH_6);
// exit immediately if not using tuning function, or when radio failsafe is invoked, so tuning values are not set to zero
if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0 || rc6->get_radio_in() == 0) {
return;
}
uint16_t radio_in = rc6->get_radio_in();
float v = constrain_float((radio_in - rc6->get_radio_min()) / float(rc6->get_radio_max() - rc6->get_radio_min()), 0, 1);
int16_t control_in = g.radio_tuning_low + v * (g.radio_tuning_high - g.radio_tuning_low);
float tuning_value = control_in / 1000.0f;
// Tuning Value should never be outside the bounds of the specified low and high value
tuning_value = constrain_float(tuning_value, g.radio_tuning_low/1000.0f, g.radio_tuning_high/1000.0f);
Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, control_in, g.radio_tuning_low, g.radio_tuning_high);
switch(g.radio_tuning) {
// Roll, Pitch tuning
case TUNING_STABILIZE_ROLL_PITCH_KP:
attitude_control->get_angle_roll_p().kP(tuning_value);
attitude_control->get_angle_pitch_p().kP(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KP:
attitude_control->get_rate_roll_pid().kP(tuning_value);
attitude_control->get_rate_pitch_pid().kP(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KI:
attitude_control->get_rate_roll_pid().kI(tuning_value);
attitude_control->get_rate_pitch_pid().kI(tuning_value);
break;
case TUNING_RATE_ROLL_PITCH_KD:
attitude_control->get_rate_roll_pid().kD(tuning_value);
attitude_control->get_rate_pitch_pid().kD(tuning_value);
break;
// Yaw tuning
case TUNING_STABILIZE_YAW_KP:
attitude_control->get_angle_yaw_p().kP(tuning_value);
break;
case TUNING_YAW_RATE_KP:
attitude_control->get_rate_yaw_pid().kP(tuning_value);
break;
case TUNING_YAW_RATE_KD:
attitude_control->get_rate_yaw_pid().kD(tuning_value);
break;
// Altitude and throttle tuning
case TUNING_ALTITUDE_HOLD_KP:
g.p_alt_hold.kP(tuning_value);
break;
case TUNING_THROTTLE_RATE_KP:
g.p_vel_z.kP(tuning_value);
break;
case TUNING_ACCEL_Z_KP:
g.pid_accel_z.kP(tuning_value);
break;
case TUNING_ACCEL_Z_KI:
g.pid_accel_z.kI(tuning_value);
break;
case TUNING_ACCEL_Z_KD:
g.pid_accel_z.kD(tuning_value);
break;
// Loiter and navigation tuning
case TUNING_LOITER_POSITION_KP:
g.p_pos_xy.kP(tuning_value);
break;
case TUNING_VEL_XY_KP:
g.pi_vel_xy.kP(tuning_value);
break;
case TUNING_VEL_XY_KI:
g.pi_vel_xy.kI(tuning_value);
break;
case TUNING_WP_SPEED:
// set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
wp_nav->set_speed_xy(control_in);
break;
// Acro roll pitch gain
case TUNING_ACRO_RP_KP:
g.acro_rp_p = tuning_value;
break;
// Acro yaw gain
case TUNING_ACRO_YAW_KP:
g.acro_yaw_p = tuning_value;
break;
#if FRAME_CONFIG == HELI_FRAME
case TUNING_HELI_EXTERNAL_GYRO:
motors->ext_gyro_gain((float)control_in / 1000.0f);
break;
case TUNING_RATE_PITCH_FF:
attitude_control->get_rate_pitch_pid().ff(tuning_value);
break;
case TUNING_RATE_ROLL_FF:
attitude_control->get_rate_roll_pid().ff(tuning_value);
break;
case TUNING_RATE_YAW_FF:
attitude_control->get_rate_yaw_pid().ff(tuning_value);
break;
#endif
case TUNING_DECLINATION:
// set declination to +-20degrees
compass.set_declination(ToRad((2.0f * control_in - g.radio_tuning_high)/100.0f), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
break;
case TUNING_CIRCLE_RATE:
// set circle rate up to approximately 45 deg/sec in either direction
circle_nav->set_rate((float)control_in/25.0f-20.0f);
break;
case TUNING_RANGEFINDER_GAIN:
// set rangefinder gain
g.rangefinder_gain.set(tuning_value);
break;
#if 0
// disabled for now - we need accessor functions
case TUNING_EKF_VERTICAL_POS:
// Tune the EKF that is being used
// EKF's baro vs accel (higher rely on accels more, baro impact is reduced)
if (!ahrs.get_NavEKF2().enabled()) {
ahrs.get_NavEKF()._gpsVertPosNoise = tuning_value;
} else {
ahrs.get_NavEKF2()._gpsVertPosNoise = tuning_value;
}
break;
case TUNING_EKF_HORIZONTAL_POS:
// EKF's gps vs accel (higher rely on accels more, gps impact is reduced)
if (!ahrs.get_NavEKF2().enabled()) {
ahrs.get_NavEKF()._gpsHorizPosNoise = tuning_value;
} else {
ahrs.get_NavEKF2()._gpsHorizPosNoise = tuning_value;
}
break;
case TUNING_EKF_ACCEL_NOISE:
// EKF's accel noise (lower means trust accels more, gps & baro less)
if (!ahrs.get_NavEKF2().enabled()) {
ahrs.get_NavEKF()._accNoise = tuning_value;
} else {
ahrs.get_NavEKF2()._accNoise = tuning_value;
}
break;
#endif
case TUNING_RC_FEEL_RP:
// roll-pitch input smoothing
g.rc_feel_rp = control_in / 10;
break;
case TUNING_RATE_PITCH_KP:
attitude_control->get_rate_pitch_pid().kP(tuning_value);
break;
case TUNING_RATE_PITCH_KI:
attitude_control->get_rate_pitch_pid().kI(tuning_value);
break;
case TUNING_RATE_PITCH_KD:
attitude_control->get_rate_pitch_pid().kD(tuning_value);
break;
case TUNING_RATE_ROLL_KP:
attitude_control->get_rate_roll_pid().kP(tuning_value);
break;
case TUNING_RATE_ROLL_KI:
attitude_control->get_rate_roll_pid().kI(tuning_value);
break;
case TUNING_RATE_ROLL_KD:
attitude_control->get_rate_roll_pid().kD(tuning_value);
break;
#if FRAME_CONFIG != HELI_FRAME
case TUNING_RATE_MOT_YAW_HEADROOM:
motors->set_yaw_headroom(tuning_value*1000);
break;
#endif
case TUNING_RATE_YAW_FILT:
attitude_control->get_rate_yaw_pid().filt_hz(tuning_value);
break;
case TUNING_WINCH: {
float desired_rate = 0.0f;
if (v > 0.6f) {
desired_rate = g2.winch.get_rate_max() * (v - 0.6f) / 0.4f;
}
if (v < 0.4f) {
desired_rate = g2.winch.get_rate_max() * (v - 0.4) / 0.4f;
}
g2.winch.set_desired_rate(desired_rate);
break;
}
}
}