diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index faf3be9ad0..732eac6b08 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -465,19 +465,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(); @@ -761,7 +761,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 @@ -802,11 +802,11 @@ void AC_PosControl::init_xy_controller() void AC_PosControl::update_xy_controller(float ekfNavVelGainScaler) { // 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; } @@ -823,13 +823,13 @@ void AC_PosControl::update_xy_controller(float ekfNavVelGainScaler) run_xy_controller(dt, ekfNavVelGainScaler); // 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 @@ -899,8 +899,8 @@ void AC_PosControl::init_vel_controller_xyz() void AC_PosControl::update_vel_controller_xy(float ekfNavVelGainScaler) { // 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) { @@ -921,7 +921,7 @@ void AC_PosControl::update_vel_controller_xy(float ekfNavVelGainScaler) run_xy_controller(dt, ekfNavVelGainScaler); // update xy update time - _last_update_xy_ms = now; + _last_update_xy_us = now_us; } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 4cba15852a..bd01c71e22 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/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) @@ -381,8 +381,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