AC_PosControl: increase accuracy of dt calcs

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -460,19 +460,19 @@ void AC_PosControl::init_takeoff()
 // is_active_z - returns true if the z-axis position controller has been run very recently
 bool AC_PosControl::is_active_z() const
 {
-    return ((AP_HAL::millis() - _last_update_z_ms) <= POSCONTROL_ACTIVE_TIMEOUT_MS);
+    return ((AP_HAL::micros64() - _last_update_z_us) <= POSCONTROL_ACTIVE_TIMEOUT_US);
 }
 
 /// update_z_controller - fly to altitude in cm above home
 void AC_PosControl::update_z_controller()
 {
     // check time since last cast
-    uint32_t now = AP_HAL::millis();
-    if (now - _last_update_z_ms > POSCONTROL_ACTIVE_TIMEOUT_MS) {
+    uint64_t now_us = AP_HAL::micros64();
+    if (now_us - _last_update_z_us > POSCONTROL_ACTIVE_TIMEOUT_US) {
         _flags.reset_rate_to_accel_z = true;
         _flags.reset_accel_to_throttle = true;
     }
-    _last_update_z_ms = now;
+    _last_update_z_us = now_us;
 
     // check for ekf altitude reset
     check_for_ekf_z_reset();
@@ -756,7 +756,7 @@ int32_t AC_PosControl::get_bearing_to_target() const
 // is_active_xy - returns true if the xy position controller has been run very recently
 bool AC_PosControl::is_active_xy() const
 {
-    return ((AP_HAL::millis() - _last_update_xy_ms) <= POSCONTROL_ACTIVE_TIMEOUT_MS);
+    return ((AP_HAL::micros64() - _last_update_xy_us) <= POSCONTROL_ACTIVE_TIMEOUT_US);
 }
 
 /// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
@@ -797,11 +797,11 @@ void AC_PosControl::init_xy_controller()
 void AC_PosControl::update_xy_controller()
 {
     // compute dt
-    uint32_t now = AP_HAL::millis();
-    float dt = (now - _last_update_xy_ms)*0.001f;
+    uint64_t now_us = AP_HAL::micros64();
+    float dt = (now_us - _last_update_xy_us) * 1.0e-6f;
 
     // sanity check dt
-    if (dt >= POSCONTROL_ACTIVE_TIMEOUT_MS*1.0e-3f) {
+    if (dt >= POSCONTROL_ACTIVE_TIMEOUT_US * 1.0e-6f) {
         dt = 0.0f;
     }
 
@@ -818,13 +818,13 @@ void AC_PosControl::update_xy_controller()
     run_xy_controller(dt);
 
     // update xy update time
-    _last_update_xy_ms = now;
+    _last_update_xy_us = now_us;
 }
 
 float AC_PosControl::time_since_last_xy_update() const
 {
-    uint32_t now = AP_HAL::millis();
-    return (now - _last_update_xy_ms)*0.001f;
+    uint64_t now_us = AP_HAL::micros64();
+    return (now_us - _last_update_xy_us) * 1.0e-6f;
 }
 
 // write log to dataflash
@@ -894,8 +894,8 @@ void AC_PosControl::init_vel_controller_xyz()
 void AC_PosControl::update_vel_controller_xy()
 {
     // capture time since last iteration
-    uint32_t now = AP_HAL::millis();
-    float dt = (now - _last_update_xy_ms)*0.001f;
+    uint64_t now_us = AP_HAL::micros64();
+    float dt = (now_us - _last_update_xy_us) * 1.0e-6f;
 
     // sanity check dt
     if (dt >= 0.2f) {
@@ -916,7 +916,7 @@ void AC_PosControl::update_vel_controller_xy()
     run_xy_controller(dt);
 
     // update xy update time
-    _last_update_xy_ms = now;
+    _last_update_xy_us = now_us;
 }
 
 

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -31,7 +31,7 @@
 #define POSCONTROL_DT_50HZ                      0.02f   // time difference in seconds for 50hz update rate
 #define POSCONTROL_DT_400HZ                     0.0025f // time difference in seconds for 400hz update rate
 
-#define POSCONTROL_ACTIVE_TIMEOUT_MS            200     // position controller is considered active if it has been called within the past 0.2 seconds
+#define POSCONTROL_ACTIVE_TIMEOUT_US            200000  // position controller is considered active if it has been called within the past 0.2 seconds
 
 #define POSCONTROL_VEL_ERROR_CUTOFF_FREQ        4.0f    // low-pass filter on velocity error (unit: hz)
 #define POSCONTROL_THROTTLE_CUTOFF_FREQ         2.0f    // low-pass filter on accel error (unit: hz)
@@ -380,8 +380,8 @@ protected:
 
     // internal variables
     float       _dt;                    // time difference (in seconds) between calls from the main program
-    uint32_t    _last_update_xy_ms;     // system time of last update_xy_controller call
-    uint32_t    _last_update_z_ms;      // system time of last update_z_controller call
+    uint64_t    _last_update_xy_us;     // system time (in microseconds) since last update_xy_controller call
+    uint64_t    _last_update_z_us;      // system time (in microseconds) of last update_z_controller call
     float       _speed_down_cms;        // max descent rate in cm/s
     float       _speed_up_cms;          // max climb rate in cm/s
     float       _speed_cms;             // max horizontal speed in cm/s