Copter: run rate loop at full filter rate in its own thread

[andyp1per]

This is a redo of #26189

I have squashed the commits, rebased and started fixing the underlying problems. There were some fundamental problems with how the original PR was handling attitude control changes so I thought it was better to just open a new PR.

Support is enabled by setting:

FSTRATE_ENABLE = 1

which gives a variable attitude rate depending on load, and:

FSTRATE_ENABLE = 2

which gives an attitude rate locked to the gyro rate, but dynamic while disarmed. For testing there is also:

FSTRATE_ENABLE = 3

which is a locked rate at all times.

The output rate can be adjusted using FSTRATE_DIV which is a gyro divisor (default 1).
Two different rates - and therefore tunes - can be compared by using the lua applet switch_rates.lua which will persistently switch the appropriate tune/rate parameters with ones saved in names beginning with X_

[andyp1per]

Flow today on top of 4.5 - working well.

[andyp1per]

@rmackay9 this is close to being ready and I could use your input on what the configuration should look like. The basic premise is that we run the rate controller at the same rate as the gyros which is controlled by INS_GYRO_RATE (0=1Khz,1=2Khz,2=4Khz,3=8Khz). However, this is quite expensive and will not be possible on some hardware setups. In addition we need to be able to log the rate outputs at a faster rate than the main loop rate, but its unlikely that we could successfully log at the gyro rate. Finally we need to update the filters (notch filter in particular) at a faster rate than the main loop, but it does not make sense to do this any faster than at half the gyro rate.

Currently we have the following:
FLIGHT_OPTIONS=8 - enables the rate thread and dynamically schedules the relationship to the gyro rate based on load
FLIGHT_OPTIONS=16 - fixes the rate thread at the same rate as the gyro rate
ATT_FILTER_RATE - update rate of the filters in Hz or fixed at half the gyro rate if set to 0
ATT_LOG_RATE - update rate of the rate logs in Hz or set at the main loop rate if set to 0
The last two are not exact numbers - they are just the closest we can get to the required frequency by counting gyro samples so need to be an integer divisor or the gyro rate.

I'm not currently convinced about the naming or the configuration. I think this is an important enough feature that it needs its own enable flag. I also think it would be worth having a rate parameter that allows you to fix the rate. I also think that ATT is a little bit misleading. Maybe FAST_RATE_ or RATET_ or something. So an alternative might be:

FRATE_ENABLE - set to 1 to enable the rate thread
FRATE_RATE - set to 0 to dynamically throttle, fixed Hz otherwise
FRATE_LOG_RATE - as above
FRATE_FILT_RATE - as above

or something. It would also be possible to not use Hz at all but simply pick the divisor. So 2 would be gyro rate / 2 etc.
Your thoughts appreciated on this important topic!

[andyp1per]

This is the rough cost for the refactoring required:

andy@Eagle:~/github/ardupilot$ ./Tools/scripts/pretty_diff_sym.py ../ardupilot-dev/build/MatekH743-bdshot/bin/arduplane build/MatekH743-bdshot/bin/arduplane
Symbol size differences between ../ardupilot-dev/build/MatekH743-bdshot/bin/arduplane and build/MatekH743-bdshot/bin/arduplane:
  18 AP_InertialSensor_Backend::_publish_gyro(unsigned 126 -> 144
   4 AC_AttitudeControl::input_thrust_vector_heading(Vector3<float> 376 -> 380
 180 AP_InertialSensor_Backend::update_gyro_filters(unsigned 0 -> 180
-190 AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float, 212 -> 22
   2 AP_HAL::RCOutput::force_trigger_groups(bool) 0 -> 2
   4 ChibiOS::DeviceBus::bus_thread(void*) 188 -> 192
   6 ChibiOS::SPIDevice::set_periodic_minimum(unsigned 0 -> 6
  -8 ChibiOS::RCOutput::push_local() 408 -> 400
  20 chMtxLockS 120 -> 140
   4 AP_AHRS_get_accel(lua_State*) 84 -> 88
   4 AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float, 540 -> 544
   2 AP_InertialSensor_Backend::set_primary_gyro(unsigned 0 -> 2
  -8 AC_AttitudeControl::attitude_controller_run_quat() 508 -> 500
   4 AC_AttitudeControl::relax_attitude_controllers() 176 -> 180
   4 SLT_Transition::update() 1148 -> 1152
   4 AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float, 296 -> 300
   4 AC_AttitudeControl_TS::input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool, 556 -> 560
  20 AP_AHRS_View::Write_Rate(AP_Motors 484 -> 504
   8 SRV_Channels::calc_pwm() 212 -> 220
   4 ChibiOS::DeviceBus::DeviceBus(unsigned 44 -> 48
   8 rcoutDriver 672 -> 680
  44 SRV_Channels::output_pwm_chan(unsigned 0 -> 44
  16 AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float, 472 -> 488
  24 AP_InertialSensor::update() 464 -> 488
   4 AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float, 136 -> 140
  -4 AP_InertialSensor::write_notch_log_messages() 368 -> 364
   4 AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float, 420 -> 424
   4 ChibiOS::RCOutput::set_dshot_rate(unsigned 136 -> 140
   6 ChibiOS::I2CDevice::set_periodic_minimum(unsigned 0 -> 6
  24 AP_Motors::add_motor_num(signed 26 -> 50
  12 AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS_View&, 480 -> 492
  62 AP_MotorsMulticopter::update_servo_map() 0 -> 62
   8 AP_Vehicle::update_dynamic_notch_at_specified_rate() 92 -> 100
 300 AC_AttitudeControl_Multi::rate_controller_run_dt(float, 0 -> 300
 104 AC_AttitudeControl::input_rate_step_bf_roll_pitch_yaw(float, 0 -> 104
   4 AP_AHRS::_get_primary_accel_index() 0 -> 4
   4 AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float, 384 -> 388
   4 GCS_MAVLINK_Plane::send_attitude_target() 104 -> 108
   2 AP_Motors::rc_write(unsigned 72 -> 74
 -32 AP_InertialSensor_Backend::update_accel(unsigned 134 -> 102
   6 AP_AHRS::write_video_stabilisation() 186 -> 192
  28 CSWTCH.378 0 -> 28
   2 AP_InertialSensor_Backend::set_primary_accel(unsigned 0 -> 2
   2 AC_AttitudeControl::rate_controller_run_dt(float, 0 -> 2
   4 AP_MotorsMulticopter::output() 80 -> 84
  60 AP_InertialSensor_Backend::update_accel_filters(unsigned 0 -> 60
 228 AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_4(float, 0 -> 228
 -64 AC_AutoTune_Multi::twitch_test_run(AC_AutoTune::AxisType, 832 -> 768
-290 AC_AttitudeControl_Multi::rate_controller_run() 332 -> 42
   8 ChibiOS::I2CDeviceManager::businfo 200 -> 208
   4 AP_InertialSensor_Invensense::start() 924 -> 928
   8 ChibiOS::RCOutput::trigger_groups() 204 -> 212
  44 AC_AttitudeControl_Multi::rate_controller_target_reset() 0 -> 44
 -28 CSWTCH.368 28 -> 0
   8 Plane::set_takeoff_expected() 140 -> 148
  48 AP_InertialSensor_Invensense::_read_fifo() 524 -> 572
   6 ChibiOS::RCOutput::force_trigger_groups(bool) 0 -> 6
   2 AP_InertialSensor_Invensensev3::set_primary_gyro(unsigned 0 -> 2
  -6 AP_InertialSensor_Backend::apply_gyro_filters(unsigned 276 -> 270
-172 AP_InertialSensor_Backend::update_gyro(unsigned 274 -> 102
   2 AC_AttitudeControl::rate_controller_target_reset() 0 -> 2
   2 AP_InertialSensor_Invensense::set_primary_gyro(unsigned 0 -> 2
Total: 586

[andyp1per]

The reason why BinarySemaphore does not work in this case is two-fold:

1. It does not use mutexes and so there is no priority inheritance between related threads, this was one reason why the original code had to increase the thread prioty of the attitude thread. The new code has no need of this - the priorities manage themselves. This makes for a safer change.
2. If you wait after signal has been called you will timeout. This is because binary semaphores contain no history - there is no matching of wait to signal. This is easy to verify by making the first thread in the test signal and delay before creating the second thread. We are managing a queue of gyro samples of which we need to process all. If a gyro sample is queued before the attitude thread is ready to receive then signal is lost and the attitude thread will not get woken up until the next gyro sample arrives. Obviously not good. To try and fix this you could then have an additional mutex to control access to the number of waiting gyros, but at that point you have essentially implemented condition variables which ChibiOS has a much more low-level and efficient implementation of - plus you still don't get the benefits of point (1).

[tridge]

I hope I'm not just being thick here, but I think I really do need to understand what is going on here. Potentially dumb questions below.

1. It does not use mutexes and so there is no priority inheritance between related threads, this was one reason why the original code had to increase the thread prioty of the attitude thread. The new code has no need of this - the priorities manage themselves. This makes for a safer change.

first off, by "attitude thread" I assume you mean the rate thread? The attitude control is still in the main thread as far as I can see, only rate update in the rate thread. I know we discussed moving the attitude update into that thread but unless I've missed it that isn't being done in this PR, and it would be a tricky thing to do as you would need an attitude update (so would need to either run EKF propagation at high rate or do your own propagation using gyro/accel data)
secondly, I can't see how priority inheritance plays any role at all. The only two threads involved with this CondMutex are the SPI threads doing the transfers and filtering and the rate thread. Both are at priority 181, so neither change priority at all. Is there some interaction I'm missing that causes priority inheritance to kick in? If so, between what threads and under what circumstances?

2. This is easy to verify by making the first thread in the test signal and delay before creating the second thread.

the signal can come from any thread, the state is in the binary semaphore object itself, so I don't see how the first thread delaying makes a difference. I'd like to have this as an actual test to understand it

[tridge]

2. This is easy to verify by making the first thread in the test signal and delay before creating the second thread.

I've updated the BinarySem test to do what you describe and it works and behaves exactly as I expected

https://github.com/tridge/ardupilot/commits/pr-copter-rate-thread/

I used a blocking wait, which matches what you are doing in this PR, no timeouts (you can't timeout with a blocking wait, it blocks forever), and gets exactly the performance I expect:

tick 21068 99.681 ops/s 0.000 timeouts/s
tick 22071 99.681 ops/s 0.000 timeouts/s
tick 23075 99.681 ops/s 0.000 timeouts/s
tick 24078 99.681 ops/s 0.000 timeouts/s
tick 25081 99.681 ops/s 0.000 timeouts/s
tick 26084 99.681 ops/s 0.000 timeouts/s
tick 27087 99.681 ops/s 0.000 timeouts/s
tick 28091 99.681 ops/s 0.000 timeouts/s
tick 29094 99.681 ops/s 0.000 timeouts/s
tick 30097 99.681 ops/s 0.000 timeouts/s
tick 31100 99.681 ops/s 0.000 timeouts/s
tick 32103 99.681 ops/s 0.000 timeouts/s
tick 33107 99.681 ops/s 0.000 timeouts/s

[tridge]

1. It does not use mutexes and so there is no priority inheritance between related threads, this was one reason why the original code had to increase the thread prioty of the attitude thread. The new code has no need of this - the priorities manage themselves. This makes for a safer change.

I just noticed that the rate thread is being created with AP_HAL::Scheduler::PRIORITY_RCOUT+1, so it is priority 182, and SPI threads are priority 181, so there are different priorities, but the rate thread is the one waiting, and priority inheritance only ever boosts priority of the waiting thread. As the thread that is waiting (the rate thread) is at a higher priority than the thread sending the signal there can be no priority change, so I'm still puzzled as to how priority inheritance comes into the picture at all.

[tridge]

why are you signalling before you push?? I think this may be why you are getting the odd binary sem behaviour. You have to signal after you have made the sample available or the rate thread will wake up pointlessly and you will always be one loop behind

[tridge]

@andyp1per we had a "climb away" event with this PR on Sunday with Michelle's quad, with FSTRATE_ENABLE=2. Looks like Z controller was not updating in ALT_HOLD mode.