Line data Source code
1 : #include "systemid.h"
2 :
3 : #if AP_PLANE_SYSTEMID_ENABLED
4 :
5 : #include <AP_Math/control.h>
6 : #include "Plane.h"
7 :
8 : /*
9 : handle systemid via an auxiliary switch
10 : */
11 :
12 : const AP_Param::GroupInfo AP_SystemID::var_info[] = {
13 :
14 : // @Param: _AXIS
15 : // @DisplayName: System identification axis
16 : // @Description: Controls which axis are being excited. Set to non-zero to see more parameters
17 : // @User: Standard
18 : // @Values: 0:None, 1:VTOL Input Roll Angle, 2:VTOL Input Pitch Angle, 3:VTOL Input Yaw Angle, 4:VTOL Recovery Roll Angle, 5:VTOL Recovery Pitch Angle, 6:VTOL Recovery Yaw Angle, 7:VTOL Rate Roll, 8:VTOL Rate Pitch, 9:VTOL Rate Yaw, 10:VTOL Mixer Roll, 11:VTOL Mixer Pitch, 12:VTOL Mixer Yaw, 13:VTOL Mixer Thrust, 20:FW Input Roll Angle, 21:FW Input Pitch Angle, 22:FW Mixer Roll, 23:FW Mixer Pitch
19 : AP_GROUPINFO_FLAGS("_AXIS", 1, AP_SystemID, axis, 0, AP_PARAM_FLAG_ENABLE),
20 :
21 : // @Param: _MAGNITUDE
22 : // @DisplayName: System identification Chirp Magnitude
23 : // @Description: Magnitude of sweep in deg, deg/s and 0-1 for mixer outputs.
24 : // @User: Standard
25 : AP_GROUPINFO("_MAGNITUDE", 2, AP_SystemID, waveform_magnitude, 5),
26 :
27 : // @Param: _F_START_HZ
28 : // @DisplayName: System identification Start Frequency
29 : // @Description: Frequency at the start of the sweep
30 : // @Range: 0.01 100
31 : // @Units: Hz
32 : // @User: Standard
33 : AP_GROUPINFO("_F_START_HZ", 3, AP_SystemID, frequency_start, 0.5f),
34 :
35 : // @Param: _F_STOP_HZ
36 : // @DisplayName: System identification Stop Frequency
37 : // @Description: Frequency at the end of the sweep
38 : // @Range: 0.01 100
39 : // @Units: Hz
40 : // @User: Standard
41 : AP_GROUPINFO("_F_STOP_HZ", 4, AP_SystemID, frequency_stop, 15),
42 :
43 : // @Param: _T_FADE_IN
44 : // @DisplayName: System identification Fade in time
45 : // @Description: Time to reach maximum amplitude of sweep
46 : // @Range: 0 20
47 : // @Units: s
48 : // @User: Standard
49 : AP_GROUPINFO("_T_FADE_IN", 5, AP_SystemID, time_fade_in, 5),
50 :
51 : // @Param: _T_REC
52 : // @DisplayName: System identification Total Sweep length
53 : // @Description: Time taken to complete the sweep
54 : // @Range: 0 255
55 : // @Units: s
56 : // @User: Standard
57 : AP_GROUPINFO("_T_REC", 6, AP_SystemID, time_record, 70),
58 :
59 : // @Param: _T_FADE_OUT
60 : // @DisplayName: System identification Fade out time
61 : // @Description: Time to reach zero amplitude at the end of the sweep
62 : // @Range: 0 5
63 : // @Units: s
64 : // @User: Standard
65 : AP_GROUPINFO("_T_FADE_OUT", 7, AP_SystemID, time_fade_out, 1),
66 :
67 : // @Param: _XY_CTRL_MUL
68 : // @DisplayName: System identification XY control multiplier
69 : // @Description: A multiplier for the XY velocity and position controller when using systemID in VTOL modes that do horizontal position and velocity control
70 : // @Range: 0.05 1.0
71 : // @User: Standard
72 : AP_GROUPINFO("_XY_CTRL_MUL", 8, AP_SystemID, xy_control_mul, 0.1),
73 :
74 : AP_GROUPEND
75 : };
76 :
77 692 : AP_SystemID::AP_SystemID(void)
78 : {
79 692 : AP_Param::setup_object_defaults(this, var_info);
80 692 : }
81 :
82 : // @LoggerMessage: SIDS
83 : // @Description: System ID settings
84 : // @Field: TimeUS: Time since system startup
85 : // @Field: Ax: The axis which is being excited
86 : // @Field: Mag: Magnitude of the chirp waveform
87 : // @Field: FSt: Frequency at the start of chirp
88 : // @Field: FSp: Frequency at the end of chirp
89 : // @Field: TFin: Time to reach maximum amplitude of chirp
90 : // @Field: TC: Time at constant frequency before chirp starts
91 : // @Field: TR: Time taken to complete chirp waveform
92 : // @Field: TFout: Time to reach zero amplitude after chirp finishes
93 :
94 : /*
95 : start systemid
96 : */
97 0 : void AP_SystemID::start()
98 : {
99 0 : start_axis = axis;
100 :
101 0 : switch (start_axis) {
102 0 : case AxisType::NONE:
103 : // check if enabled
104 0 : gcs().send_text(MAV_SEVERITY_WARNING, "SystemID: No axis selected");
105 0 : return;
106 0 : case AxisType::INPUT_ROLL:
107 : case AxisType::INPUT_PITCH:
108 : case AxisType::INPUT_YAW:
109 : case AxisType::RECOVER_ROLL:
110 : case AxisType::RECOVER_PITCH:
111 : case AxisType::RECOVER_YAW:
112 : case AxisType::RATE_ROLL:
113 : case AxisType::RATE_PITCH:
114 : case AxisType::RATE_YAW:
115 : case AxisType::MIX_ROLL:
116 : case AxisType::MIX_PITCH:
117 : case AxisType::MIX_YAW:
118 : case AxisType::MIX_THROTTLE:
119 : // Exits if the current flight mode or phase does not support system ID axis.
120 0 : if (!plane.control_mode->supports_vtol_systemid()) {
121 : #if HAL_QUADPLANE_ENABLED
122 0 : gcs().send_text(MAV_SEVERITY_WARNING, "SystemID: Axis not supported for this flight mode");
123 : #else
124 : gcs().send_text(MAV_SEVERITY_WARNING, "SystemID: Axis not supported in Plane");
125 : #endif
126 0 : return;
127 : }
128 0 : break;
129 0 : case AxisType::FW_INPUT_ROLL:
130 : case AxisType::FW_INPUT_PITCH:
131 : case AxisType::FW_MIX_ROLL:
132 : case AxisType::FW_MIX_PITCH:
133 : // Exits if the currently flight mode or phase does not support system ID axis.
134 0 : if (!plane.control_mode->supports_fw_systemid()) {
135 0 : gcs().send_text(MAV_SEVERITY_WARNING, "SystemID: Axis not supported for this flight mode");
136 0 : return;
137 : }
138 0 : if (!plane.control_mode->allow_fw_systemid()) {
139 0 : gcs().send_text(MAV_SEVERITY_WARNING, "SystemID: Axis not supported for this flight phase");
140 0 : return;
141 : }
142 0 : break;
143 : }
144 :
145 0 : if (!hal.util->get_soft_armed()) {
146 0 : gcs().send_text(MAV_SEVERITY_WARNING, "SystemID: must be armed");
147 0 : return;
148 : }
149 :
150 0 : attitude_offset_deg.zero();
151 0 : throttle_offset = 0;
152 :
153 : #if HAL_QUADPLANE_ENABLED
154 0 : if (plane.quadplane.available()) {
155 0 : restore.att_bf_feedforward = plane.quadplane.attitude_control->get_bf_feedforward();
156 : }
157 : #endif
158 :
159 0 : waveform_time = 0;
160 0 : time_const_freq = 2.0 / frequency_start; // Two full cycles at the starting frequency
161 :
162 0 : chirp_input.init(time_record, frequency_start, frequency_stop, time_fade_in, time_fade_out, time_const_freq);
163 :
164 0 : gcs().send_text(MAV_SEVERITY_INFO, "SystemID Starting: axis=%d", (unsigned)axis);
165 :
166 : #if HAL_LOGGING_ENABLED
167 0 : AP::logger().WriteStreaming("SIDS", "TimeUS,Ax,Mag,FSt,FSp,TFin,TC,TR,TFout",
168 : "s--ssssss", "F--------", "QBfffffff",
169 : AP_HAL::micros64(),
170 0 : uint8_t(start_axis),
171 0 : waveform_magnitude.get(),
172 0 : frequency_start.get(),
173 0 : frequency_stop.get(),
174 0 : time_fade_in.get(),
175 0 : time_const_freq,
176 0 : time_record.get(),
177 0 : time_fade_out.get());
178 : #endif // HAL_LOGGING_ENABLED
179 :
180 0 : running = true;
181 : }
182 :
183 : /*
184 : stop systemid
185 : */
186 1930 : void AP_SystemID::stop()
187 : {
188 1930 : if (running) {
189 0 : running = false;
190 0 : attitude_offset_deg.zero();
191 0 : throttle_offset = 0;
192 :
193 : #if HAL_QUADPLANE_ENABLED
194 0 : if (plane.quadplane.available()) {
195 0 : auto *attitude_control = plane.quadplane.attitude_control;
196 0 : attitude_control->bf_feedforward(restore.att_bf_feedforward);
197 0 : attitude_control->rate_bf_roll_sysid_rads(0);
198 0 : attitude_control->rate_bf_pitch_sysid_rads(0);
199 0 : attitude_control->rate_bf_yaw_sysid_rads(0);
200 0 : plane.quadplane.pos_control->NE_set_control_scale_factor(1);
201 :
202 : // re-initialise the XY controller so we take current position as target
203 0 : plane.quadplane.pos_control->NE_init_controller();
204 : }
205 : #endif
206 0 : gcs().send_text(MAV_SEVERITY_INFO, "SystemID stopped");
207 : }
208 1930 : }
209 :
210 : /*
211 : update systemid - needs to be called at main loop rate
212 : */
213 810697 : void AP_SystemID::vtol_update()
214 : {
215 : #if HAL_QUADPLANE_ENABLED
216 810697 : if (!running) {
217 810697 : return;
218 : }
219 0 : if (chirp_input.completed()) {
220 0 : stop();
221 0 : return;
222 : }
223 :
224 0 : float const last_loop_time_s = AP::scheduler().get_last_loop_time_s();
225 :
226 0 : waveform_time += last_loop_time_s;
227 0 : waveform_sample = chirp_input.update(waveform_time, waveform_magnitude);
228 0 : waveform_freq_rads = chirp_input.get_frequency_rads();
229 :
230 0 : auto *attitude_control = plane.quadplane.attitude_control;
231 :
232 0 : switch (start_axis) {
233 0 : case AxisType::NONE:
234 : // not possible, see start()
235 0 : break;
236 0 : case AxisType::INPUT_ROLL:
237 0 : attitude_offset_deg.x = waveform_sample;
238 0 : break;
239 0 : case AxisType::INPUT_PITCH:
240 0 : attitude_offset_deg.y = waveform_sample;
241 0 : break;
242 0 : case AxisType::INPUT_YAW:
243 0 : attitude_offset_deg.z = waveform_sample;
244 0 : break;
245 0 : case AxisType::RECOVER_ROLL:
246 0 : attitude_offset_deg.x = waveform_sample;
247 0 : attitude_control->bf_feedforward(false);
248 0 : break;
249 0 : case AxisType::RECOVER_PITCH:
250 0 : attitude_offset_deg.y = waveform_sample;
251 0 : attitude_control->bf_feedforward(false);
252 0 : break;
253 0 : case AxisType::RECOVER_YAW:
254 0 : attitude_offset_deg.z = waveform_sample;
255 0 : attitude_control->bf_feedforward(false);
256 0 : break;
257 0 : case AxisType::RATE_ROLL:
258 0 : attitude_control->rate_bf_roll_sysid_rads(radians(waveform_sample));
259 0 : break;
260 0 : case AxisType::RATE_PITCH:
261 0 : attitude_control->rate_bf_pitch_sysid_rads(radians(waveform_sample));
262 0 : break;
263 0 : case AxisType::RATE_YAW:
264 0 : attitude_control->rate_bf_yaw_sysid_rads(radians(waveform_sample));
265 0 : break;
266 0 : case AxisType::MIX_ROLL:
267 0 : attitude_control->actuator_roll_sysid(waveform_sample);
268 0 : break;
269 0 : case AxisType::MIX_PITCH:
270 0 : attitude_control->actuator_pitch_sysid(waveform_sample);
271 0 : break;
272 0 : case AxisType::MIX_YAW:
273 0 : attitude_control->actuator_yaw_sysid(waveform_sample);
274 0 : break;
275 0 : case AxisType::MIX_THROTTLE:
276 0 : throttle_offset = waveform_sample;
277 0 : break;
278 0 : default:
279 0 : break;
280 : }
281 :
282 : // reduce control in NE axis when in position controlled modes
283 0 : plane.quadplane.pos_control->NE_set_control_scale_factor(xy_control_mul);
284 :
285 0 : if (log_subsample <= 0) {
286 0 : log_data();
287 : // log attitude controller at the same rate
288 0 : plane.quadplane.Log_Write_AttRate();
289 :
290 0 : if (plane.should_log(MASK_LOG_ATTITUDE_FAST) && plane.should_log(MASK_LOG_ATTITUDE_MED)) {
291 0 : log_subsample = 1;
292 0 : } else if (plane.should_log(MASK_LOG_ATTITUDE_FAST)) {
293 0 : log_subsample = 2;
294 0 : } else if (plane.should_log(MASK_LOG_ATTITUDE_MED)) {
295 0 : log_subsample = 4;
296 : } else {
297 0 : log_subsample = 8;
298 : }
299 : }
300 0 : log_subsample -= 1;
301 :
302 : #endif
303 : }
304 :
305 : // Return true if a fixed wing system ID is currently running
306 23460105 : bool AP_SystemID::is_running_fw() const
307 : {
308 23460105 : if (!is_running()) {
309 23460105 : return false;
310 : }
311 :
312 0 : switch (start_axis) {
313 0 : case AxisType::NONE:
314 : case AxisType::INPUT_ROLL:
315 : case AxisType::INPUT_PITCH:
316 : case AxisType::INPUT_YAW:
317 : case AxisType::RECOVER_ROLL:
318 : case AxisType::RECOVER_PITCH:
319 : case AxisType::RECOVER_YAW:
320 : case AxisType::RATE_ROLL:
321 : case AxisType::RATE_PITCH:
322 : case AxisType::RATE_YAW:
323 : case AxisType::MIX_ROLL:
324 : case AxisType::MIX_PITCH:
325 : case AxisType::MIX_YAW:
326 : case AxisType::MIX_THROTTLE:
327 0 : break;
328 :
329 0 : case AxisType::FW_INPUT_ROLL:
330 : case AxisType::FW_INPUT_PITCH:
331 : case AxisType::FW_MIX_ROLL:
332 : case AxisType::FW_MIX_PITCH:
333 0 : return true;
334 : }
335 :
336 0 : return false;
337 : }
338 :
339 : /*
340 : update systemid - needs to be called at main loop rate
341 : */
342 0 : void AP_SystemID::fw_update()
343 : {
344 0 : if (!plane.control_mode->allow_fw_systemid() || chirp_input.completed()) {
345 : // Control mode change means chirp should be stopped, or
346 : // Chirp is complete
347 0 : stop();
348 0 : return;
349 : }
350 :
351 0 : float const last_loop_time_s = AP::scheduler().get_last_loop_time_s();
352 :
353 0 : waveform_time += last_loop_time_s;
354 0 : waveform_sample = chirp_input.update(waveform_time, waveform_magnitude);
355 0 : waveform_freq_rads = chirp_input.get_frequency_rads();
356 :
357 0 : switch (start_axis) {
358 0 : case AxisType::NONE:
359 : // not possible, see start()
360 0 : break;
361 0 : case AxisType::FW_INPUT_ROLL:
362 0 : plane.nav_roll_cd += waveform_sample * 100.0f;
363 0 : break;
364 0 : case AxisType::FW_INPUT_PITCH:
365 0 : plane.nav_pitch_cd += waveform_sample * 100.0f;
366 0 : break;
367 0 : case AxisType::FW_MIX_ROLL:
368 0 : output_offset.x = waveform_sample;
369 0 : break;
370 0 : case AxisType::FW_MIX_PITCH:
371 0 : output_offset.y = waveform_sample;
372 0 : break;
373 0 : default:
374 0 : break;
375 : }
376 :
377 0 : if (log_subsample <= 0) {
378 0 : log_data();
379 0 : log_plane_data();
380 :
381 0 : if (plane.should_log(MASK_LOG_ATTITUDE_FAST) && plane.should_log(MASK_LOG_ATTITUDE_MED)) {
382 0 : log_subsample = 1;
383 0 : } else if (plane.should_log(MASK_LOG_ATTITUDE_FAST)) {
384 0 : log_subsample = 2;
385 0 : } else if (plane.should_log(MASK_LOG_ATTITUDE_MED)) {
386 0 : log_subsample = 4;
387 : } else {
388 0 : log_subsample = 8;
389 : }
390 : }
391 0 : log_subsample -= 1;
392 : }
393 :
394 : // @LoggerMessage: SIDD
395 : // @Description: System ID data
396 : // @Field: TimeUS: Time since system startup
397 : // @Field: Time: Time reference for waveform
398 : // @Field: Targ: Current waveform sample
399 : // @Field: F: Instantaneous waveform frequency
400 : // @Field: Gx: Delta angle, X-Axis
401 : // @Field: Gy: Delta angle, Y-Axis
402 : // @Field: Gz: Delta angle, Z-Axis
403 : // @Field: Ax: Delta velocity, X-Axis
404 : // @Field: Ay: Delta velocity, Y-Axis
405 : // @Field: Az: Delta velocity, Z-Axis
406 :
407 : // log system id
408 0 : void AP_SystemID::log_data() const
409 : {
410 : #if HAL_LOGGING_ENABLED
411 0 : Vector3f delta_angle;
412 : float delta_angle_dt;
413 0 : plane.ins.get_delta_angle(delta_angle, delta_angle_dt);
414 :
415 0 : Vector3f delta_velocity;
416 : float delta_velocity_dt;
417 0 : plane.ins.get_delta_velocity(delta_velocity, delta_velocity_dt);
418 :
419 0 : if (is_positive(delta_angle_dt) && is_positive(delta_velocity_dt)) {
420 0 : const float dt_ang_inv = 1.0 / delta_angle_dt;
421 0 : const float dt_vel_inv = 1.0 / delta_velocity_dt;
422 0 : AP::logger().WriteStreaming("SIDD", "TimeUS,Time,Targ,F,Gx,Gy,Gz,Ax,Ay,Az",
423 : "ss-zkkkooo", "F---------", "Qfffffffff",
424 : AP_HAL::micros64(),
425 0 : waveform_time, waveform_sample, waveform_freq_rads / (2 * M_PI),
426 0 : degrees(delta_angle.x * dt_ang_inv),
427 0 : degrees(delta_angle.y * dt_ang_inv),
428 0 : degrees(delta_angle.z * dt_ang_inv),
429 0 : delta_velocity.x * dt_vel_inv,
430 0 : delta_velocity.y * dt_vel_inv,
431 0 : delta_velocity.z * dt_vel_inv);
432 : }
433 : #endif // HAL_LOGGING_ENABLED
434 0 : }
435 :
436 : // @LoggerMessage: SIDP
437 : // @Description: System ID data for Plane
438 : // @Field: TimeUS: Time since system startup
439 : // @Field: DRll: Desired Roll Angle
440 : // @Field: Rll: Roll Angle
441 : // @Field: DPit: Desired Pitch Angle
442 : // @Field: Pit: Pitch Angle
443 : // @Field: rdes: Desired Roll Rate
444 : // @Field: r: Measured Roll Rate
445 : // @Field: pdes: Desired Pitch Rate
446 : // @Field: p: Measured Pitch Rate
447 : // @Field: Aile: Aileron
448 : // @Field: Elev: Elevator
449 : // @Field: aspd: Speed_Scalar
450 : // @Field: eastas: EAS2TAS
451 :
452 0 : void AP_SystemID::log_plane_data() const
453 : {
454 : #if HAL_LOGGING_ENABLED
455 : // int16_t pitch = plane.ahrs.pitch_sensor - plane.g.pitch_trim * 100;
456 0 : float speed_scaler = plane.get_speed_scaler();
457 0 : const auto &pitch_pid_info = plane.pitchController.get_pid_info();
458 0 : const auto &roll_pid_info = plane.rollController.get_pid_info();
459 :
460 0 : int16_t demanded_pitch = plane.nav_pitch_cd + int32_t(plane.g.pitch_trim * 100.0) + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * plane.g.kff_throttle_to_pitch;
461 0 : AP::logger().WriteStreaming("SIDP", "TimeUS,DRll,Rll,DPit,Pit,rdes,r,pdes,p,Aile,Elev,aspd,eastas",
462 : "soooooooooooo", "F------------", "Qffffffffffff",
463 : AP_HAL::micros64(),
464 0 : plane.nav_roll_cd * 0.01f,
465 0 : plane.ahrs.roll_sensor * 0.01f,
466 0 : demanded_pitch * 0.01f,
467 0 : plane.ahrs.pitch_sensor * 0.01f,
468 0 : roll_pid_info.target,
469 0 : degrees(plane.ahrs.get_gyro().x),
470 0 : pitch_pid_info.target,
471 0 : degrees(plane.ahrs.get_gyro().y),
472 0 : SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) * 0.01f,
473 0 : SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) * 0.01f,
474 : speed_scaler,
475 0 : plane.ahrs.get_EAS2TAS());
476 :
477 : #endif // HAL_LOGGING_ENABLED
478 0 : }
479 : #endif // AP_PLANE_SYSTEMID_ENABLED
480 :
|
