Copter: set dshot rate when changing attitude rate

ArduPilot · Jul 11, 2024 · 4d9a3d8 · 4d9a3d8
1 parent 2977209
commit 4d9a3d8
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp
@@ -29,6 +29,7 @@ void Copter::rate_controller_thread()
 
     HAL_CondMutex cmutex;
     ins.set_rate_loop_mutex(&cmutex);
+    hal.rcout->set_dshot_rate(1, ins.get_raw_gyro_rate_hz() / rate_decimation);
 
     uint32_t last_run_us = AP_HAL::micros();
     float max_dt = 0.0;
@@ -79,6 +80,7 @@ void Copter::rate_controller_thread()
                 // setup the notch filter sample rate
                 const float loop_rate_hz = AP::scheduler().get_filtered_loop_rate_hz();
                 attitude_control->set_notch_sample_rate(loop_rate_hz);
+                hal.rcout->set_dshot_rate(SRV_Channels::get_dshot_rate(), loop_rate_hz);
                 motors->set_dt(1.0/loop_rate_hz);
                 ins.set_rate_decimation(0);
                 hal.rcout->force_trigger_groups(false);
@@ -212,7 +214,9 @@ void Copter::rate_controller_thread()
             && ((get_fast_rate_type() == FastRateType::FAST_RATE_FIXED_ARMED && motors->armed())
                 || get_fast_rate_type() == FastRateType::FAST_RATE_FIXED)) {
             rate_decimation = target_rate_decimation;
-            attitude_control->set_notch_sample_rate(ins.get_raw_gyro_rate_hz() / rate_decimation);
+            const uint32_t att_rate = ins.get_raw_gyro_rate_hz() / rate_decimation;
+            attitude_control->set_notch_sample_rate(att_rate);
+            hal.rcout->set_dshot_rate(1, att_rate);
             gcs().send_text(MAV_SEVERITY_INFO, "Attitude rate active at %uHz", (unsigned)ins.get_raw_gyro_rate_hz() / rate_decimation);
             notify_fixed_rate_active = false;
         }
@@ -234,6 +238,7 @@ void Copter::rate_controller_thread()
                 if (new_attitude_rate > AP::scheduler().get_filtered_loop_rate_hz()) {
                     rate_decimation = new_rate_decimation;
                     attitude_control->set_notch_sample_rate(new_attitude_rate);
+                    hal.rcout->set_dshot_rate(1, new_attitude_rate);
                     gcs().send_text(MAV_SEVERITY_WARNING, "Attitude CPU high, dropping rate to %uHz",
                                     (unsigned)new_attitude_rate);
                     prev_loop_count = rate_loop_count;
@@ -254,6 +259,7 @@ void Copter::rate_controller_thread()
                 const uint32_t new_attitude_rate = ins.get_raw_gyro_rate_hz()/(rate_decimation-1);
                 rate_decimation = rate_decimation - 1;
                 attitude_control->set_notch_sample_rate(new_attitude_rate);
+                hal.rcout->set_dshot_rate(1, new_attitude_rate);
                 gcs().send_text(MAV_SEVERITY_INFO, "Attitude CPU normal, increasing rate to %uHz",
                                 (unsigned) new_attitude_rate);
                 prev_loop_count = 0;