Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : #include "Copter.h"
16 : #include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
17 : #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
18 :
19 : #pragma GCC optimize("O2")
20 :
21 : /*
22 : Attitude Rate controller thread design.
23 :
24 : Rationale: running rate outputs linked to fast gyro outputs achieves two goals:
25 :
26 : 1. High frequency gyro processing allows filters to be applied with high sample rates
27 : which is advantageous in removing high frequency noise and associated aliasing
28 : 2. High frequency rate control reduces the latency between control and action leading to
29 : better disturbance rejection and faster responses which generally means higher
30 : PIDs can be used without introducing control oscillation
31 :
32 : (1) is already mostly achieved through the higher gyro rates that are available via
33 : INS_GYRO_RATE. (2) requires running the rate controller at higher rates via a separate thread
34 :
35 :
36 : Goal: the ideal scenario is to run in a single cycle:
37 :
38 : gyro read->filter->publish->rate control->motor output
39 :
40 : This ensures the minimum latency between gyro sample and motor output. Other functions need
41 : to also run faster than they would normally most notably logging and filter frequencies - most
42 : notably the harmonic notch frequency.
43 :
44 : Design assumptions:
45 :
46 : 1. The sample rate of the IMUs is consistent and accurate.
47 : This is the most basic underlying assumption. An alternative approach would be to rely on
48 : the timing of when samples are received but this proves to not work in practice due to
49 : scheduling delays. Thus the dt used by the attitude controller is the delta between IMU
50 : measurements, not the delta between processing cycles in the rate thread.
51 : 2. Every IMU reading must be processed or consistently sub-sampled.
52 : This is an assumption that follows from (1) - so it means that attitude control should
53 : process every sample or every other sample or every third sample etc. Note that these are
54 : filtered samples - all incoming samples are processed for filtering purposes, it is only
55 : for the purposes of rate control that we are sub-sampling.
56 : 3. The data that the rate loop requires is timely, consistent and accurate.
57 : Rate control essentially requires two components - the target and the actuals. The actuals
58 : come from the incoming gyro sample combined with the state of the PIDs. The target comes
59 : from attitude controller which is running at a slower rate in the main loop. Since the rate
60 : thread can read the attitude target at any time it is important that this is always available
61 : consistently and is updated consistently.
62 : 4. The data that the rest of the vehicle uses is the same data that the rate thread uses.
63 : Put another way any gyro value that the vehicle uses (e.g. in the EKF etc), must have already
64 : been processed by the rate thread. Where this becomes important is with sub-sampling - if
65 : rate gyro values are sub-sampled we need to make sure that the vehicle is also only using
66 : the sub-sampled values.
67 :
68 : Design:
69 :
70 : 1. Filtered gyro samples are (sub-sampled and) pushed into an ObjectBuffer from the INS backend.
71 : 2. The pushed sample is published to the INS front-end so that the rest of the vehicle only
72 : sees published values that have been used by the rate controller. When the rate thread is not
73 : in use the filtered samples are effectively sub-sampled at the main loop rate. The EKF is unaffected
74 : as it uses delta angles calculated from the raw gyro values. (It might be possible to avoid publishing
75 : from the rate thread by only updating _gyro_filtered when a value is pushed).
76 : 3. A notification is sent that a sample is available
77 : 4. The rate thread is blocked waiting for a sample. When it receives a notification it:
78 : 4a. Runs the rate controller
79 : 4b. Pushes the new pwm values. Periodically at the main loop rate all of the SRV_Channels::push()
80 : functionality is run as well.
81 : 5. The rcout dshot thread is blocked waiting for a new pwm value. When it is signalled by the
82 : rate thread it wakes up and runs the dshot motor output logic.
83 : 6. Periodically the rate thread:
84 : 6a. Logs the rate outputs (1Khz)
85 : 6b. Updates the notch filter centers (Gyro rate/2)
86 : 6c. Checks the ObjectBuffer length and main loop delay (10Hz)
87 : If the ObjectBuffer length has been longer than 2 for the last 5 cycles or the main loop has
88 : been slowed down then the rate thread is slowed down by telling the INS to sub-sample. This
89 : mechanism is continued until the rate thread is able to keep up with the sub-sample rate.
90 : The inverse of this mechanism is run if the rate thread is able to keep up but is running slower
91 : than the gyro sample rate.
92 : 6d. Updates the PID notch centers (1Hz)
93 : 7. When the rate rate changes through sub-sampling the following values are updated:
94 : 7a. The PID notch sample rate
95 : 7b. The dshot rate is constrained to be never greater than the gyro rate or rate rate
96 : 7c. The motors dt
97 : 8. Independently of the rate thread the attitude control target is updated in the main loop. In order
98 : for target values to be consistent all updates are processed using local variables and the final
99 : target is only written at the end of the update as a vector. Direct control of the target (e.g. in
100 : autotune) is also constrained to be on all axes simultaneously using the new desired value. The
101 : target makes use of the current PIDs and the "latest" gyro, it might be possible to use a loop
102 : delayed gyro value, but that is currently out-of-scope.
103 :
104 : Performance considerations:
105 :
106 : On an H754 using ICM42688 and gyro sampling at 4KHz and rate thread at 4Khz the main CPU users are:
107 :
108 : ArduCopter PRI=182 sp=0x30000600 STACK=4392/7168 LOAD=18.6%
109 : idle PRI= 1 sp=0x300217B0 STACK= 296/ 504 LOAD= 4.3%
110 : rcout PRI=181 sp=0x3001DAF0 STACK= 504/ 952 LOAD=10.7%
111 : SPI1 PRI=181 sp=0x3002DAB8 STACK= 856/1464 LOAD=17.5%
112 : SPI4 PRI=181 sp=0x3002D4A0 STACK= 888/1464 LOAD=18.3%
113 : rate PRI=182 sp=0x3002B1D0 STACK=1272/1976 LOAD=22.4%
114 :
115 : There is a direct correlation between the rate rate and CPU load, so if the rate rate is half the gyro
116 : rate (i.e. 2Khz) we observe the following:
117 :
118 : ArduCopter PRI=182 sp=0x30000600 STACK=4392/7168 LOAD=16.7%
119 : idle PRI= 1 sp=0x300217B0 STACK= 296/ 504 LOAD=21.3%
120 : rcout PRI=181 sp=0x3001DAF0 STACK= 504/ 952 LOAD= 6.2%
121 : SPI1 PRI=181 sp=0x3002DAB8 STACK= 856/1464 LOAD=16.7%
122 : SPI4 PRI=181 sp=0x3002D4A0 STACK= 888/1464 LOAD=17.8%
123 : rate PRI=182 sp=0x3002B1D0 STACK=1272/1976 LOAD=11.5%
124 :
125 : So we get almost a halving of CPU load in the rate and rcout threads. This is the main way that CPU
126 : load can be handled on lower-performance boards, with the other mechanism being lowering the gyro rate.
127 : So at a very respectable gyro rate and rate rate both of 2Khz (still 5x standard main loop rate) we see:
128 :
129 : ArduCopter PRI=182 sp=0x30000600 STACK=4440/7168 LOAD=15.6%
130 : idle PRI= 1 sp=0x300217B0 STACK= 296/ 504 LOAD=39.4%
131 : rcout PRI=181 sp=0x3001DAF0 STACK= 504/ 952 LOAD= 5.9%
132 : SPI1 PRI=181 sp=0x3002DAB8 STACK= 856/1464 LOAD= 8.9%
133 : SPI4 PRI=181 sp=0x3002D4A0 STACK= 896/1464 LOAD= 9.1%
134 : rate PRI=182 sp=0x30029FB0 STACK=1296/1976 LOAD=11.8%
135 :
136 : This essentially means that its possible to run this scheme successfully on all MCUs by careful setting of
137 : the maximum rates.
138 :
139 : Enabling rate thread timing debug for 4Khz reads with fast logging and armed we get the following data:
140 :
141 : Rate loop timing: gyro=178us, rate=13us, motors=45us, log=7us, ctrl=1us
142 : Rate loop timing: gyro=178us, rate=13us, motors=45us, log=7us, ctrl=1us
143 : Rate loop timing: gyro=177us, rate=13us, motors=46us, log=7us, ctrl=1us
144 :
145 : The log output is an average since it only runs at 1Khz, so roughly 28us elapsed. So the majority of the time
146 : is spent waiting for a gyro sample (higher is better here since it represents the idle time) updating the PIDs
147 : and outputting to the motors. Everything else is relatively cheap. Since the total cycle time is 250us the duty
148 : cycle is thus 29%
149 : */
150 :
151 : #define DIV_ROUND_INT(x, d) ((x + d/2) / d)
152 :
153 29 : uint8_t Copter::calc_gyro_decimation(uint8_t gyro_decimation, uint16_t rate_hz)
154 : {
155 29 : return MAX(uint8_t(DIV_ROUND_INT(ins.get_raw_gyro_rate_hz() / gyro_decimation, rate_hz)), 1U);
156 : }
157 :
158 832177 : static inline bool run_decimated_callback(uint8_t decimation_rate, uint8_t& decimation_count)
159 : {
160 832177 : return decimation_rate > 0 && ++decimation_count >= decimation_rate;
161 : }
162 :
163 : //#define RATE_LOOP_TIMING_DEBUG
164 : /*
165 : thread for rate control
166 : */
167 3 : void Copter::rate_controller_thread()
168 : {
169 3 : uint8_t target_rate_decimation = constrain_int16(g2.att_decimation.get(), 1,
170 3 : DIV_ROUND_INT(ins.get_raw_gyro_rate_hz(), AP::scheduler().get_loop_rate_hz()));
171 : uint8_t rate_decimation = target_rate_decimation;
172 :
173 : // set up the decimation rates
174 : RateControllerRates rates;
175 3 : rate_controller_set_rates(rate_decimation, rates, false);
176 :
177 : uint32_t rate_loop_count = 0;
178 : uint32_t prev_loop_count = 0;
179 :
180 3 : uint32_t last_run_us = AP_HAL::micros();
181 3 : float max_dt = 0.0;
182 3 : float min_dt = 1.0;
183 3 : uint32_t now_ms = AP_HAL::millis();
184 : uint32_t last_rate_check_ms = 0;
185 : uint32_t last_rate_increase_ms = 0;
186 : #if HAL_LOGGING_ENABLED
187 : uint32_t last_rtdt_log_ms = now_ms;
188 : #endif
189 : uint32_t last_notch_sample_ms = now_ms;
190 : bool was_using_rate_thread = false;
191 : bool notify_fixed_rate_active = true;
192 : bool was_armed = false;
193 : uint32_t running_slow = 0;
194 : #ifdef RATE_LOOP_TIMING_DEBUG
195 : uint32_t gyro_sample_time_us = 0;
196 : uint32_t rate_controller_time_us = 0;
197 : uint32_t motor_output_us = 0;
198 : uint32_t log_output_us = 0;
199 : uint32_t ctrl_output_us = 0;
200 : uint32_t timing_count = 0;
201 : uint32_t last_timing_msg_us = 0;
202 : #endif
203 :
204 : // run the filters at half the gyro rate
205 : #if HAL_LOGGING_ENABLED
206 3 : uint8_t log_loop_count = 0;
207 : #endif
208 3 : uint8_t main_loop_count = 0;
209 3 : uint8_t filter_loop_count = 0;
210 :
211 : while (true) {
212 :
213 : #ifdef RATE_LOOP_TIMING_DEBUG
214 : uint32_t rate_now_us = AP_HAL::micros();
215 : #endif
216 :
217 : // allow changing option at runtime
218 292677 : if (get_fast_rate_type() == FastRateType::FAST_RATE_DISABLED) {
219 837 : if (was_using_rate_thread) {
220 1 : disable_fast_rate_loop(rates);
221 : was_using_rate_thread = false;
222 : }
223 837 : hal.scheduler->delay_microseconds(500);
224 836 : last_run_us = AP_HAL::micros();
225 836 : continue;
226 : }
227 :
228 : // set up rate thread requirements
229 291840 : if (!using_rate_thread) {
230 3 : enable_fast_rate_loop(rate_decimation, rates);
231 : }
232 291840 : ins.set_rate_decimation(rate_decimation);
233 :
234 : // wait for an IMU sample
235 291840 : Vector3f gyro;
236 291840 : if (!ins.get_next_gyro_sample(gyro)) {
237 3618 : continue; // go around again
238 : }
239 :
240 : #ifdef RATE_LOOP_TIMING_DEBUG
241 : gyro_sample_time_us += AP_HAL::micros() - rate_now_us;
242 : rate_now_us = AP_HAL::micros();
243 : #endif
244 :
245 : // we must use multiples of the actual sensor rate
246 288220 : const float sensor_dt = 1.0f * rate_decimation / ins.get_raw_gyro_rate_hz();
247 288220 : const uint32_t now_us = AP_HAL::micros();
248 288220 : const uint32_t dt_us = now_us - last_run_us;
249 288220 : const float dt = dt_us * 1.0e-6;
250 : last_run_us = now_us;
251 :
252 : // check if we are falling behind
253 288220 : if (ins.get_num_gyro_samples() > 2) {
254 251 : running_slow++;
255 287969 : } else if (running_slow > 0) {
256 251 : running_slow--;
257 : }
258 288220 : if (AP::scheduler().get_extra_loop_us() == 0) {
259 288220 : rate_loop_count++;
260 : }
261 :
262 : // run the rate controller on all available samples
263 : // it is important not to drop samples otherwise the filtering will be fubar
264 : // there is no need to output to the motors more than once for every batch of samples
265 288220 : attitude_control->rate_controller_run_dt(gyro + ahrs.get_gyro_drift(), sensor_dt);
266 :
267 : #ifdef RATE_LOOP_TIMING_DEBUG
268 : rate_controller_time_us += AP_HAL::micros() - rate_now_us;
269 : rate_now_us = AP_HAL::micros();
270 : #endif
271 :
272 : // immediately output the new motor values
273 288220 : if (run_decimated_callback(rates.main_loop_rate, main_loop_count)) {
274 144109 : main_loop_count = 0;
275 : }
276 288220 : motors_output(main_loop_count == 0);
277 :
278 : // process filter updates
279 288220 : if (run_decimated_callback(rates.filter_rate, filter_loop_count)) {
280 144109 : filter_loop_count = 0;
281 :
282 144109 : rate_controller_filter_update();
283 : }
284 :
285 288220 : max_dt = MAX(dt, max_dt);
286 288220 : min_dt = MIN(dt, min_dt);
287 :
288 : #if HAL_LOGGING_ENABLED
289 288220 : if (now_ms - last_rtdt_log_ms >= 100) { // 10 Hz
290 2877 : Log_Write_Rate_Thread_Dt(dt, sensor_dt, max_dt, min_dt);
291 2877 : max_dt = sensor_dt;
292 2877 : min_dt = sensor_dt;
293 : last_rtdt_log_ms = now_ms;
294 : }
295 : #endif
296 :
297 : #ifdef RATE_LOOP_TIMING_DEBUG
298 : motor_output_us += AP_HAL::micros() - rate_now_us;
299 : rate_now_us = AP_HAL::micros();
300 : #endif
301 :
302 : #if HAL_LOGGING_ENABLED
303 : // fast logging output
304 288220 : if (should_log(MASK_LOG_ATTITUDE_FAST)) {
305 255733 : if (run_decimated_callback(rates.fast_logging_rate, log_loop_count)) {
306 127866 : log_loop_count = 0;
307 127866 : rate_controller_log_update();
308 :
309 : }
310 32487 : } else if (should_log(MASK_LOG_ATTITUDE_MED)) {
311 4 : if (run_decimated_callback(rates.medium_logging_rate, log_loop_count)) {
312 0 : log_loop_count = 0;
313 0 : rate_controller_log_update();
314 : }
315 : }
316 : #endif
317 :
318 : #ifdef RATE_LOOP_TIMING_DEBUG
319 : log_output_us += AP_HAL::micros() - rate_now_us;
320 : rate_now_us = AP_HAL::micros();
321 : #endif
322 :
323 288220 : now_ms = AP_HAL::millis();
324 :
325 : // make sure we have the latest target rate
326 288220 : target_rate_decimation = constrain_int16(g2.att_decimation.get(), 1,
327 288220 : DIV_ROUND_INT(ins.get_raw_gyro_rate_hz(), AP::scheduler().get_loop_rate_hz()));
328 288220 : if (now_ms - last_notch_sample_ms >= 1000 || !was_using_rate_thread) {
329 : // update the PID notch sample rate at 1Hz if we are
330 : // enabled at runtime
331 : last_notch_sample_ms = now_ms;
332 291 : attitude_control->set_notch_sample_rate(1.0 / sensor_dt);
333 : #ifdef RATE_LOOP_TIMING_DEBUG
334 : hal.console->printf("Sample rate %.1f, main loop %u, fast rate %u, med rate %u\n", 1.0 / sensor_dt,
335 : rates.main_loop_rate, rates.fast_logging_rate, rates.medium_logging_rate);
336 : #endif
337 : }
338 :
339 : // interlock for printing fixed rate active
340 288220 : if (was_armed != motors->armed()) {
341 6 : notify_fixed_rate_active = !was_armed;
342 6 : was_armed = motors->armed();
343 : }
344 :
345 : // Once armed, switch to the fast rate if configured to do so
346 288220 : if ((rate_decimation != target_rate_decimation || notify_fixed_rate_active)
347 288220 : && ((get_fast_rate_type() == FastRateType::FAST_RATE_FIXED_ARMED && motors->armed())
348 260260 : || get_fast_rate_type() == FastRateType::FAST_RATE_FIXED)) {
349 0 : rate_decimation = target_rate_decimation;
350 0 : rate_controller_set_rates(rate_decimation, rates, false);
351 : notify_fixed_rate_active = false;
352 : }
353 :
354 : // check that the CPU is not pegged, if it is drop the attitude rate
355 288220 : if (now_ms - last_rate_check_ms >= 100
356 288220 : && (get_fast_rate_type() == FastRateType::FAST_RATE_DYNAMIC
357 0 : || (get_fast_rate_type() == FastRateType::FAST_RATE_FIXED_ARMED && !motors->armed())
358 0 : || target_rate_decimation > rate_decimation)) {
359 : last_rate_check_ms = now_ms;
360 2880 : const uint32_t att_rate = ins.get_raw_gyro_rate_hz()/rate_decimation;
361 2880 : if (running_slow > 5 || AP::scheduler().get_extra_loop_us() > 0
362 : #if HAL_LOGGING_ENABLED
363 2880 : || AP::logger().in_log_download()
364 : #endif
365 5760 : || target_rate_decimation > rate_decimation) {
366 0 : const uint8_t new_rate_decimation = MAX(rate_decimation + 1, target_rate_decimation);
367 0 : const uint32_t new_attitude_rate = ins.get_raw_gyro_rate_hz() / new_rate_decimation;
368 0 : if (new_attitude_rate > AP::scheduler().get_filtered_loop_rate_hz()) {
369 : rate_decimation = new_rate_decimation;
370 0 : rate_controller_set_rates(rate_decimation, rates, true);
371 : prev_loop_count = rate_loop_count;
372 : rate_loop_count = 0;
373 : running_slow = 0;
374 : }
375 2880 : } else if (rate_decimation > target_rate_decimation && rate_loop_count > att_rate/10 // ensure 100ms worth of good readings
376 0 : && (prev_loop_count > att_rate/10 // ensure there was 100ms worth of good readings at the higher rate
377 0 : || prev_loop_count == 0 // last rate was actually a lower rate so keep going quickly
378 0 : || now_ms - last_rate_increase_ms >= 10000)) { // every 10s retry
379 0 : rate_decimation = rate_decimation - 1;
380 :
381 0 : rate_controller_set_rates(rate_decimation, rates, false);
382 : prev_loop_count = 0;
383 : rate_loop_count = 0;
384 : last_rate_increase_ms = now_ms;
385 : }
386 : }
387 :
388 : #ifdef RATE_LOOP_TIMING_DEBUG
389 : timing_count++;
390 : ctrl_output_us += AP_HAL::micros() - rate_now_us;
391 : rate_now_us = AP_HAL::micros();
392 :
393 : if (rate_now_us - last_timing_msg_us > 1e6) {
394 : hal.console->printf("Rate loop timing: gyro=%uus, rate=%uus, motors=%uus, log=%uus, ctrl=%uus\n",
395 : unsigned(gyro_sample_time_us/timing_count), unsigned(rate_controller_time_us/timing_count),
396 : unsigned(motor_output_us/timing_count), unsigned(log_output_us/timing_count), unsigned(ctrl_output_us/timing_count));
397 : last_timing_msg_us = rate_now_us;
398 : timing_count = 0;
399 : gyro_sample_time_us = rate_controller_time_us = motor_output_us = log_output_us = ctrl_output_us = 0;
400 : }
401 : #endif
402 :
403 : was_using_rate_thread = true;
404 : }
405 : }
406 :
407 : /*
408 : update rate controller filters. on an H7 this is about 30us
409 : */
410 144109 : void Copter::rate_controller_filter_update()
411 : {
412 : // update the frontend center frequencies of notch filters
413 576436 : for (auto ¬ch : ins.harmonic_notches) {
414 432327 : update_dynamic_notch(notch);
415 : }
416 :
417 : // this copies backend data to the frontend and updates the notches
418 144109 : ins.update_backend_filters();
419 144109 : }
420 :
421 : /*
422 : update rate controller rates and return the logging rate
423 : */
424 7 : void Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high)
425 : {
426 7 : const uint32_t attitude_rate = ins.get_raw_gyro_rate_hz() / rate_decimation;
427 7 : attitude_control->set_notch_sample_rate(attitude_rate);
428 7 : hal.rcout->set_dshot_rate(SRV_Channels::get_dshot_rate(), attitude_rate);
429 7 : motors->set_dt_s(1.0f / attitude_rate);
430 21 : gcs().send_text(warn_cpu_high ? MAV_SEVERITY_WARNING : MAV_SEVERITY_INFO,
431 : "Rate CPU %s, rate set to %uHz",
432 : warn_cpu_high ? "high" : "normal", (unsigned) attitude_rate);
433 : #if HAL_LOGGING_ENABLED
434 7 : if (attitude_rate > 1000) {
435 0 : rates.fast_logging_rate = calc_gyro_decimation(rate_decimation, 1000); // 1Khz
436 : } else {
437 7 : rates.fast_logging_rate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz());
438 : }
439 7 : rates.medium_logging_rate = calc_gyro_decimation(rate_decimation, 10); // 10Hz
440 : #endif
441 7 : rates.main_loop_rate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz());
442 7 : rates.filter_rate = calc_gyro_decimation(rate_decimation, ins.get_raw_gyro_rate_hz() / 2);
443 7 : }
444 :
445 : // enable the fast rate thread using the provided decimation rate and record the new output rates
446 3 : void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates)
447 : {
448 3 : ins.enable_fast_rate_buffer();
449 3 : rate_controller_set_rates(rate_decimation, rates, false);
450 3 : hal.rcout->force_trigger_groups(true);
451 3 : using_rate_thread = true;
452 3 : }
453 :
454 : // disable the fast rate thread and record the new output rates
455 1 : void Copter::disable_fast_rate_loop(RateControllerRates& rates)
456 : {
457 1 : using_rate_thread = false;
458 1 : uint8_t rate_decimation = calc_gyro_decimation(1, AP::scheduler().get_filtered_loop_rate_hz());
459 1 : rate_controller_set_rates(rate_decimation, rates, false);
460 1 : hal.rcout->force_trigger_groups(false);
461 1 : ins.disable_fast_rate_buffer();
462 1 : }
463 :
464 : /*
465 : log only those items that are updated at the rate loop rate
466 : */
467 127866 : void Copter::rate_controller_log_update()
468 : {
469 : #if HAL_LOGGING_ENABLED
470 127866 : if (!copter.flightmode->logs_attitude()) {
471 127866 : Log_Write_Rate();
472 127866 : Log_Write_PIDS(); // only logs if PIDS bitmask is set
473 : }
474 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
475 127866 : if (should_log(MASK_LOG_FTN_FAST)) {
476 0 : AP::ins().write_notch_log_messages();
477 : }
478 : #endif
479 : #endif
480 127866 : }
481 :
482 : // run notch update at either loop rate or 200Hz
483 30411539 : void Copter::update_dynamic_notch_at_specified_rate_main()
484 : {
485 30411539 : if (using_rate_thread) {
486 : return;
487 : }
488 :
489 30296417 : update_dynamic_notch_at_specified_rate();
490 : }
491 :
492 : #endif // AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
|
