Line data Source code
1 : #include "Copter.h"
2 : #include <AP_Math/control.h>
3 :
4 : #if MODE_SYSTEMID_ENABLED
5 :
6 : /*
7 : * Init and run calls for systemId, flight mode
8 : */
9 :
10 : const AP_Param::GroupInfo ModeSystemId::var_info[] = {
11 :
12 : // @Param: _AXIS
13 : // @DisplayName: System identification axis
14 : // @Description: Controls which axis are being excited. Set to non-zero to see more parameters
15 : // @User: Standard
16 : // @Values: 0:None, 1:Input Roll Angle, 2:Input Pitch Angle, 3:Input Yaw Angle, 4:Recovery Roll Angle, 5:Recovery Pitch Angle, 6:Recovery Yaw Angle, 7:Rate Roll, 8:Rate Pitch, 9:Rate Yaw, 10:Mixer Roll, 11:Mixer Pitch, 12:Mixer Yaw, 13:Mixer Thrust, 14:Measured Lateral Position, 15:Measured Longitudinal Position, 16:Measured Lateral Velocity, 17:Measured Longitudinal Velocity, 18:Input Lateral Velocity, 19:Input Longitudinal Velocity
17 : AP_GROUPINFO_FLAGS("_AXIS", 1, ModeSystemId, axis, 0, AP_PARAM_FLAG_ENABLE),
18 :
19 : // @Param: _MAGNITUDE
20 : // @DisplayName: System identification Chirp Magnitude
21 : // @Description: Magnitude of sweep in deg, deg/s and 0-1 for mixer outputs.
22 : // @User: Standard
23 : AP_GROUPINFO("_MAGNITUDE", 2, ModeSystemId, waveform_magnitude, 15),
24 :
25 : // @Param: _F_START_HZ
26 : // @DisplayName: System identification Start Frequency
27 : // @Description: Frequency at the start of the sweep
28 : // @Range: 0.01 100
29 : // @Units: Hz
30 : // @User: Standard
31 : AP_GROUPINFO("_F_START_HZ", 3, ModeSystemId, frequency_start, 0.5f),
32 :
33 : // @Param: _F_STOP_HZ
34 : // @DisplayName: System identification Stop Frequency
35 : // @Description: Frequency at the end of the sweep
36 : // @Range: 0.01 100
37 : // @Units: Hz
38 : // @User: Standard
39 : AP_GROUPINFO("_F_STOP_HZ", 4, ModeSystemId, frequency_stop, 40),
40 :
41 : // @Param: _T_FADE_IN
42 : // @DisplayName: System identification Fade in time
43 : // @Description: Time to reach maximum amplitude of sweep
44 : // @Range: 0 20
45 : // @Units: s
46 : // @User: Standard
47 : AP_GROUPINFO("_T_FADE_IN", 5, ModeSystemId, time_fade_in, 15),
48 :
49 : // @Param: _T_REC
50 : // @DisplayName: System identification Total Sweep length
51 : // @Description: Time taken to complete the sweep
52 : // @Range: 0 255
53 : // @Units: s
54 : // @User: Standard
55 : AP_GROUPINFO("_T_REC", 6, ModeSystemId, time_record, 70),
56 :
57 : // @Param: _T_FADE_OUT
58 : // @DisplayName: System identification Fade out time
59 : // @Description: Time to reach zero amplitude at the end of the sweep
60 : // @Range: 0 5
61 : // @Units: s
62 : // @User: Standard
63 : AP_GROUPINFO("_T_FADE_OUT", 7, ModeSystemId, time_fade_out, 2),
64 :
65 : AP_GROUPEND
66 : };
67 :
68 1001 : ModeSystemId::ModeSystemId(void) : Mode()
69 : {
70 1001 : AP_Param::setup_object_defaults(this, var_info);
71 1001 : }
72 :
73 : #define SYSTEM_ID_DELAY 1.0f // time in seconds waited after system id mode change for frequency sweep injection
74 :
75 : // Return true if this mode is enabled, used by MAVLink available modes
76 75223 : bool ModeSystemId::enabled() const
77 : {
78 75223 : return axis != 0;
79 : }
80 :
81 : // systemId_init - initialise systemId controller
82 0 : bool ModeSystemId::init(bool ignore_checks)
83 : {
84 : // check if enabled
85 0 : if (!enabled()) {
86 0 : gcs().send_text(MAV_SEVERITY_WARNING, "No axis selected, SID_AXIS = 0");
87 0 : return false;
88 : }
89 :
90 : // ensure we are flying
91 0 : if (!copter.motors->armed() || !copter.ap.auto_armed || copter.ap.land_complete) {
92 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Aircraft must be flying");
93 0 : return false;
94 : }
95 :
96 0 : if (!is_poscontrol_axis_type()) {
97 :
98 : // System ID is being done on the attitude control loops
99 :
100 : // Can only switch into System ID Axes 1-13 with a flight mode that has manual throttle
101 0 : if (!copter.flightmode->has_manual_throttle()) {
102 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Axis requires manual throttle");
103 0 : return false;
104 : }
105 :
106 : #if FRAME_CONFIG == HELI_FRAME
107 0 : copter.input_manager.set_use_stab_col(true);
108 : #endif
109 :
110 : } else {
111 :
112 : // System ID is being done on the position control loops
113 :
114 : // Can only switch into System ID Axes 14-19 from Loiter flight mode
115 0 : if (copter.flightmode->mode_number() != Mode::Number::LOITER) {
116 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Axis requires switch from Loiter");
117 0 : return false;
118 : }
119 :
120 : // set horizontal speed and acceleration limits
121 0 : pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
122 0 : pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
123 :
124 : // initialise the horizontal position controller
125 0 : if (!pos_control->NE_is_active()) {
126 0 : pos_control->NE_init_controller();
127 : }
128 :
129 : // set vertical speed and acceleration limits
130 0 : pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
131 0 : pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
132 :
133 : // initialise the vertical position controller
134 0 : if (!pos_control->D_is_active()) {
135 0 : pos_control->D_init_controller();
136 : }
137 0 : target_pos_ne_m = pos_control->get_pos_estimate_NED_m().xy();
138 : }
139 :
140 0 : att_bf_feedforward = attitude_control->get_bf_feedforward();
141 0 : waveform_time = 0.0f;
142 0 : time_const_freq = 2.0f / frequency_start; // Two full cycles at the starting frequency
143 0 : systemid_state = SystemIDModeState::SYSTEMID_STATE_TESTING;
144 0 : log_subsample = 0;
145 :
146 0 : chirp_input.init(time_record, frequency_start, frequency_stop, time_fade_in, time_fade_out, time_const_freq);
147 :
148 0 : gcs().send_text(MAV_SEVERITY_INFO, "SystemID Starting: axis=%d", (unsigned)axis);
149 :
150 : #if HAL_LOGGING_ENABLED
151 0 : copter.Log_Write_SysID_Setup(axis, waveform_magnitude, frequency_start, frequency_stop, time_fade_in, time_const_freq, time_record, time_fade_out);
152 : #endif
153 :
154 0 : return true;
155 : }
156 :
157 : // systemId_exit - clean up systemId controller before exiting
158 0 : void ModeSystemId::exit()
159 : {
160 : // reset the feedforward enabled parameter to the initialized state
161 0 : attitude_control->bf_feedforward(att_bf_feedforward);
162 0 : }
163 :
164 : // systemId_run - runs the systemId controller
165 : // should be called at 100hz or more
166 0 : void ModeSystemId::run()
167 : {
168 0 : float target_roll_rad = 0.0f;
169 0 : float target_pitch_rad = 0.0f;
170 0 : float target_yaw_rate_rads = 0.0f;
171 0 : float pilot_throttle_scaled = 0.0f;
172 0 : float target_climb_rate_ms = 0.0f;
173 0 : Vector2f input_vel_ne_ms;
174 :
175 0 : if (!is_poscontrol_axis_type()) {
176 :
177 : // convert pilot input to lean angles
178 0 : get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, attitude_control->lean_angle_max_rad(), attitude_control->lean_angle_max_rad());
179 :
180 : // get pilot's desired yaw rate
181 0 : target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads();
182 :
183 0 : if (!motors->armed()) {
184 : // Motors should be Stopped
185 0 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
186 : // Tradheli doesn't set spool state to ground idle when throttle stick is zero. Ground idle only set when
187 : // motor interlock is disabled.
188 0 : } else if (copter.ap.throttle_zero && !copter.is_tradheli()) {
189 : // Attempting to Land
190 0 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
191 : } else {
192 0 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
193 : }
194 :
195 0 : switch (motors->get_spool_state()) {
196 0 : case AP_Motors::SpoolState::SHUT_DOWN:
197 : // Motors Stopped
198 0 : attitude_control->reset_yaw_target_and_rate();
199 0 : attitude_control->reset_rate_controller_I_terms();
200 0 : break;
201 :
202 0 : case AP_Motors::SpoolState::GROUND_IDLE:
203 : // Landed
204 : // Tradheli initializes targets when going from disarmed to armed state.
205 : // init_targets_on_arming is always set true for multicopter.
206 0 : if (motors->init_targets_on_arming()) {
207 0 : attitude_control->reset_yaw_target_and_rate();
208 0 : attitude_control->reset_rate_controller_I_terms_smoothly();
209 : }
210 0 : break;
211 :
212 0 : case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
213 : // clear landing flag above zero throttle
214 0 : if (!motors->limit.throttle_lower) {
215 0 : set_land_complete(false);
216 : }
217 0 : break;
218 :
219 0 : case AP_Motors::SpoolState::SPOOLING_UP:
220 : case AP_Motors::SpoolState::SPOOLING_DOWN:
221 : // do nothing
222 0 : break;
223 : }
224 :
225 : // get pilot's desired throttle
226 : #if FRAME_CONFIG == HELI_FRAME
227 0 : pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
228 : #else
229 0 : pilot_throttle_scaled = get_pilot_desired_throttle();
230 : #endif
231 :
232 : }
233 :
234 0 : if ((systemid_state == SystemIDModeState::SYSTEMID_STATE_TESTING) &&
235 0 : (!is_positive(frequency_start) || !is_positive(frequency_stop) || is_negative(time_fade_in) || !is_positive(time_record) || is_negative(time_fade_out) || (time_record <= time_const_freq))) {
236 0 : systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED;
237 0 : gcs().send_text(MAV_SEVERITY_INFO, "SystemID Parameter Error");
238 : }
239 :
240 0 : waveform_time += G_Dt;
241 0 : waveform_sample = chirp_input.update(waveform_time - SYSTEM_ID_DELAY, waveform_magnitude);
242 0 : waveform_freq_rads = chirp_input.get_frequency_rads();
243 0 : Vector2f disturb_state;
244 0 : switch (systemid_state) {
245 0 : case SystemIDModeState::SYSTEMID_STATE_STOPPED:
246 0 : attitude_control->bf_feedforward(att_bf_feedforward);
247 0 : break;
248 0 : case SystemIDModeState::SYSTEMID_STATE_TESTING:
249 :
250 0 : if (copter.ap.land_complete) {
251 0 : systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED;
252 0 : gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: Landed");
253 0 : break;
254 : }
255 0 : if (attitude_control->lean_angle_rad() > attitude_control->lean_angle_max_rad()) {
256 0 : systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED;
257 0 : gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: lean=%f max=%f", (double)degrees(attitude_control->lean_angle_rad()), (double)degrees(attitude_control->lean_angle_max_rad()));
258 0 : break;
259 : }
260 0 : if (waveform_time > SYSTEM_ID_DELAY + time_fade_in + time_const_freq + time_record + time_fade_out) {
261 0 : systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED;
262 0 : gcs().send_text(MAV_SEVERITY_INFO, "SystemID Finished");
263 0 : break;
264 : }
265 :
266 0 : switch ((AxisType)axis.get()) {
267 0 : case AxisType::NONE:
268 0 : systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED;
269 0 : gcs().send_text(MAV_SEVERITY_INFO, "SystemID Stopped: axis = 0");
270 0 : break;
271 0 : case AxisType::INPUT_ROLL:
272 0 : target_roll_rad += radians(waveform_sample);
273 0 : break;
274 0 : case AxisType::INPUT_PITCH:
275 0 : target_pitch_rad += radians(waveform_sample);
276 0 : break;
277 0 : case AxisType::INPUT_YAW:
278 0 : target_yaw_rate_rads += radians(waveform_sample);
279 0 : break;
280 0 : case AxisType::RECOVER_ROLL:
281 0 : target_roll_rad += radians(waveform_sample);
282 0 : attitude_control->bf_feedforward(false);
283 0 : break;
284 0 : case AxisType::RECOVER_PITCH:
285 0 : target_pitch_rad += radians(waveform_sample);
286 0 : attitude_control->bf_feedforward(false);
287 0 : break;
288 0 : case AxisType::RECOVER_YAW:
289 0 : target_yaw_rate_rads += radians(waveform_sample);
290 0 : attitude_control->bf_feedforward(false);
291 0 : break;
292 0 : case AxisType::RATE_ROLL:
293 0 : attitude_control->rate_bf_roll_sysid_rads(radians(waveform_sample));
294 0 : break;
295 0 : case AxisType::RATE_PITCH:
296 0 : attitude_control->rate_bf_pitch_sysid_rads(radians(waveform_sample));
297 0 : break;
298 0 : case AxisType::RATE_YAW:
299 0 : attitude_control->rate_bf_yaw_sysid_rads(radians(waveform_sample));
300 0 : break;
301 0 : case AxisType::MIX_ROLL:
302 0 : attitude_control->actuator_roll_sysid(waveform_sample);
303 0 : break;
304 0 : case AxisType::MIX_PITCH:
305 0 : attitude_control->actuator_pitch_sysid(waveform_sample);
306 0 : break;
307 0 : case AxisType::MIX_YAW:
308 0 : attitude_control->actuator_yaw_sysid(waveform_sample);
309 0 : break;
310 0 : case AxisType::MIX_THROTTLE:
311 0 : pilot_throttle_scaled += waveform_sample;
312 0 : break;
313 0 : case AxisType::DISTURB_POS_LAT:
314 0 : disturb_state.x = 0.0f;
315 0 : disturb_state.y = waveform_sample;
316 0 : disturb_state.rotate(attitude_control->get_att_target_euler_rad().z);
317 0 : pos_control->set_disturb_pos_NE_m(disturb_state);
318 0 : break;
319 0 : case AxisType::DISTURB_POS_LONG:
320 0 : disturb_state.x = waveform_sample;
321 0 : disturb_state.y = 0.0f;
322 0 : disturb_state.rotate(attitude_control->get_att_target_euler_rad().z);
323 0 : pos_control->set_disturb_pos_NE_m(disturb_state);
324 0 : break;
325 0 : case AxisType::DISTURB_VEL_LAT:
326 0 : disturb_state.x = 0.0f;
327 0 : disturb_state.y = waveform_sample;
328 0 : disturb_state.rotate(attitude_control->get_att_target_euler_rad().z);
329 0 : pos_control->set_disturb_vel_NE_ms(disturb_state);
330 0 : break;
331 0 : case AxisType::DISTURB_VEL_LONG:
332 0 : disturb_state.x = waveform_sample;
333 0 : disturb_state.y = 0.0f;
334 0 : disturb_state.rotate(attitude_control->get_att_target_euler_rad().z);
335 0 : pos_control->set_disturb_vel_NE_ms(disturb_state);
336 0 : break;
337 0 : case AxisType::INPUT_VEL_LAT:
338 0 : input_vel_ne_ms.x = 0.0f;
339 0 : input_vel_ne_ms.y = waveform_sample;
340 0 : input_vel_ne_ms.rotate(attitude_control->get_att_target_euler_rad().z);
341 0 : break;
342 0 : case AxisType::INPUT_VEL_LONG:
343 0 : input_vel_ne_ms.x = waveform_sample;
344 0 : input_vel_ne_ms.y = 0.0f;
345 0 : input_vel_ne_ms.rotate(attitude_control->get_att_target_euler_rad().z);
346 0 : break;
347 : }
348 0 : break;
349 : }
350 :
351 0 : if (!is_poscontrol_axis_type()) {
352 :
353 : // call attitude controller
354 0 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(target_roll_rad, target_pitch_rad, target_yaw_rate_rads);
355 :
356 : // output pilot's throttle
357 0 : attitude_control->set_throttle_out(pilot_throttle_scaled, !copter.is_tradheli(), g.throttle_filt);
358 :
359 : } else {
360 :
361 : // relax loiter target if we might be landed
362 0 : if (copter.ap.land_complete_maybe) {
363 0 : pos_control->NE_soften_for_landing();
364 : }
365 :
366 0 : Vector2f accel_ne_mss;
367 0 : target_pos_ne_m += input_vel_ne_ms.topostype() * G_Dt;
368 0 : if (is_positive(G_Dt)) {
369 0 : accel_ne_mss = (input_vel_ne_ms - input_vel_last_ne_ms) / G_Dt;
370 0 : input_vel_last_ne_ms = input_vel_ne_ms;
371 : }
372 0 : pos_control->set_pos_vel_accel_NE_m(target_pos_ne_m, input_vel_ne_ms, accel_ne_mss);
373 :
374 : // run pos controller
375 0 : pos_control->NE_update_controller();
376 :
377 : // call attitude controller
378 0 : attitude_control->input_thrust_vector_rate_heading_rads(pos_control->get_thrust_vector(), target_yaw_rate_rads, false);
379 :
380 : // Send the commanded climb rate to the position controller
381 0 : pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms);
382 :
383 : // run the vertical position controller and set output throttle
384 0 : pos_control->D_update_controller();
385 : }
386 :
387 0 : if (log_subsample <= 0) {
388 0 : log_data();
389 0 : if (copter.should_log(MASK_LOG_ATTITUDE_FAST) && copter.should_log(MASK_LOG_ATTITUDE_MED)) {
390 0 : log_subsample = 1;
391 0 : } else if (copter.should_log(MASK_LOG_ATTITUDE_FAST)) {
392 0 : log_subsample = 2;
393 0 : } else if (copter.should_log(MASK_LOG_ATTITUDE_MED)) {
394 0 : log_subsample = 4;
395 : } else {
396 0 : log_subsample = 8;
397 : }
398 : }
399 0 : log_subsample -= 1;
400 0 : }
401 :
402 : // log system id and attitude
403 0 : void ModeSystemId::log_data() const
404 : {
405 0 : Vector3f delta_angle;
406 : float delta_angle_dt;
407 0 : copter.ins.get_delta_angle(delta_angle, delta_angle_dt);
408 :
409 0 : Vector3f delta_velocity;
410 : float delta_velocity_dt;
411 0 : copter.ins.get_delta_velocity(delta_velocity, delta_velocity_dt);
412 :
413 0 : if (is_positive(delta_angle_dt) && is_positive(delta_velocity_dt)) {
414 0 : copter.Log_Write_SysID_Data(waveform_time, waveform_sample, waveform_freq_rads / (2 * M_PI), degrees(delta_angle.x / delta_angle_dt), degrees(delta_angle.y / delta_angle_dt), degrees(delta_angle.z / delta_angle_dt), delta_velocity.x / delta_velocity_dt, delta_velocity.y / delta_velocity_dt, delta_velocity.z / delta_velocity_dt);
415 : }
416 :
417 : // Full rate logging of attitude, rate and pid loops
418 0 : copter.Log_Write_Attitude();
419 0 : copter.Log_Write_Rate();
420 0 : copter.Log_Write_PIDS();
421 :
422 0 : if (is_poscontrol_axis_type()) {
423 0 : pos_control->write_log();
424 0 : copter.logger.Write_PID(LOG_PIDN_MSG, pos_control->NE_get_vel_pid().get_pid_info_x());
425 0 : copter.logger.Write_PID(LOG_PIDE_MSG, pos_control->NE_get_vel_pid().get_pid_info_y());
426 :
427 : }
428 0 : }
429 :
430 0 : bool ModeSystemId::is_poscontrol_axis_type() const
431 : {
432 0 : bool ret = false;
433 :
434 0 : switch ((AxisType)axis.get()) {
435 0 : case AxisType::DISTURB_POS_LAT:
436 : case AxisType::DISTURB_POS_LONG:
437 : case AxisType::DISTURB_VEL_LAT:
438 : case AxisType::DISTURB_VEL_LONG:
439 : case AxisType::INPUT_VEL_LAT:
440 : case AxisType::INPUT_VEL_LONG:
441 0 : ret = true;
442 0 : break;
443 0 : default:
444 0 : break;
445 : }
446 :
447 0 : return ret;
448 : }
449 :
450 : #endif
|
