WIP do not merge

ArduPilot · Jun 3, 2024 · a6b149a · a6b149a
1 parent a9472b8
commit a6b149a
Show file tree

Hide file tree

Showing 5 changed files with 52 additions and 2 deletions.
diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp
@@ -12,6 +12,9 @@
 void Copter::rate_controller_thread()
 {
     uint8_t rate_decimation = 1;
+    // rate at which to decimate to the logging and notch updates
+    uint8_t slow_loop_decimation = 4;
+    uint32_t slow_loop_count = 0;
     uint32_t rate_loop_count = 0;
     uint32_t prev_loop_count = 0;
 
@@ -112,6 +115,14 @@ void Copter::rate_controller_thread()
          */
         update_dynamic_notch_at_specified_rate();
 
+        /*
+          process slow loop update
+         */
+        if (slow_loop_count++ >= slow_loop_decimation) {
+            slow_loop_count = 0;
+            rate_controller_slow_loop();
+        }
+
         now_ms = AP_HAL::millis();
 
         if (now_ms - last_notch_sample_ms >= 1000 || !was_using_rate_thread) {
@@ -124,6 +135,7 @@ void Copter::rate_controller_thread()
         // Once armed, switch to the fast rate if configured to do so
         if (rate_decimation > 1 && motors->armed() && flight_option_is_set(FlightOptions::USE_FIXED_RATE_LOOP_THREAD)) {
             rate_decimation = 1;
+            slow_loop_decimation = 4;
             ins.set_rate_decimation(rate_decimation);
             attitude_control->set_notch_sample_rate(ins.get_raw_gyro_rate_hz());
             gcs().send_text(MAV_SEVERITY_INFO, "Attitude rate active at %uHz", (unsigned)ins.get_raw_gyro_rate_hz());
@@ -143,6 +155,7 @@ void Copter::rate_controller_thread()
                 const uint32_t new_attitude_rate = ins.get_raw_gyro_rate_hz()/(rate_decimation+1);
                 if (new_attitude_rate > AP::scheduler().get_filtered_loop_rate_hz() * 2) {
                     rate_decimation = rate_decimation + 1;
+                    slow_loop_decimation = 4 / rate_decimation;
                     ins.set_rate_decimation(rate_decimation);
                     attitude_control->set_notch_sample_rate(new_attitude_rate);
                     gcs().send_text(MAV_SEVERITY_WARNING, "Attitude CPU high, dropping rate to %uHz",
@@ -156,6 +169,7 @@ void Copter::rate_controller_thread()
                     || now_ms - last_rate_increase_ms >= 10000)) { // every 10s retry
                 const uint32_t new_attitude_rate = ins.get_raw_gyro_rate_hz()/(rate_decimation-1);
                 rate_decimation = rate_decimation - 1;
+                slow_loop_decimation = 4 / rate_decimation;
                 ins.set_rate_decimation(rate_decimation);
                 attitude_control->set_notch_sample_rate(new_attitude_rate);
                 gcs().send_text(MAV_SEVERITY_INFO, "Attitude CPU normal, increasing rate to %uHz",
@@ -169,6 +183,16 @@ void Copter::rate_controller_thread()
         was_using_rate_thread = true;
     }
 }
+
+/*
+  update rate controller slow loop
+*/
+void Copter::rate_controller_slow_loop()
+{
+#if HAL_LOGGING_ENABLED
+    fast_logging();
+#endif
+}
 #endif
 
 /*

diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
@@ -521,6 +521,15 @@ void Copter::update_batt_compass(void)
 // Full rate logging of attitude, rate and pid loops
 // should be run at loop rate
 void Copter::loop_rate_logging()
+{
+    if (using_rate_thread) {
+        return;
+    }
+
+    fast_logging();
+}
+
+void Copter::fast_logging()
 {
     if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
         Log_Write_Attitude();

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
@@ -709,6 +709,7 @@ class Copter : public AP_Vehicle {
     void throttle_loop();
     void update_batt_compass(void);
     void loop_rate_logging();
+    void fast_logging();
     void ten_hz_logging_loop();
     void twentyfive_hz_logging();
     void three_hz_loop();
@@ -731,6 +732,7 @@ class Copter : public AP_Vehicle {
     void rotate_body_frame_to_NE(float &x, float &y);
     uint16_t get_pilot_speed_dn() const;
     void rate_controller_thread();
+    void rate_controller_slow_loop();
     void run_rate_controller_main();
 
 #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -1880,8 +1880,11 @@ void AP_InertialSensor::update(void)
             _delta_velocity_valid[i] = false;
             _delta_angle_valid[i] = false;
         }
-        for (uint8_t i=0; i<_backend_count; i++) {
-            _backends[i]->update();
+
+        if (!_cmutex) {
+            for (uint8_t i=0; i<_backend_count; i++) {
+                _backends[i]->update();
+            }
         }
 
         if (!_startup_error_counts_set) {
@@ -2803,6 +2806,17 @@ bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro)
 
     return ret;
 }
+
+void AP_InertialSensor::update_backends()
+{
+    if (!_cmutex) {
+        return;
+    }
+
+    for (uint8_t i=0; i<_backend_count; i++) {
+        _backends[i]->update();
+    }
+}
 #endif
 
 namespace AP {

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -818,6 +818,7 @@ class AP_InertialSensor : AP_AccelCal_Client
     bool get_next_gyro_sample(Vector3f& gyro);
     uint32_t get_num_gyro_samples() { return _rate_loop_gyro_window.available(); }
     void set_rate_decimation(uint8_t rdec) { rate_decimation = rdec; }
+    void update_backends();
 
     bool gyro_samples_available() { return  _rate_loop_gyro_window.available() > 0; }
 #endif