Plane: check G_Dt to catch startup errors

this catches cases where constructors lead to G_Dt being incorrect
tridge · Jul 21, 2022 · 25e317d · 25e317d
1 parent 3e89e32
commit 25e317d
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
@@ -334,6 +334,13 @@ void Plane::one_second_loop()
             // reset the landing altitude correction
             landing.alt_offset = 0;
     }
+
+    // this ensures G_Dt is correct, catching startup issues with constructors
+    // calling the scheduler methods
+    if (!is_equal(1.0f/scheduler.get_loop_rate_hz(), scheduler.get_loop_period_s()) ||
+        !is_equal(G_Dt, scheduler.get_loop_period_s())) {
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+    }
 }
 
 void Plane::three_hz_loop()