Browse Source

AC_PosControl: constify dt calcs

master
Randy Mackay 6 years ago
parent
commit
8710526894
  1. 8
      libraries/AC_AttitudeControl/AC_PosControl.cpp

8
libraries/AC_AttitudeControl/AC_PosControl.cpp

@ -467,7 +467,7 @@ bool AC_PosControl::is_active_z() const @@ -467,7 +467,7 @@ bool AC_PosControl::is_active_z() const
void AC_PosControl::update_z_controller()
{
// check time since last cast
uint64_t now_us = AP_HAL::micros64();
const 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;
@ -797,7 +797,7 @@ void AC_PosControl::init_xy_controller() @@ -797,7 +797,7 @@ void AC_PosControl::init_xy_controller()
void AC_PosControl::update_xy_controller()
{
// compute dt
uint64_t now_us = AP_HAL::micros64();
const uint64_t now_us = AP_HAL::micros64();
float dt = (now_us - _last_update_xy_us) * 1.0e-6f;
// sanity check dt
@ -823,7 +823,7 @@ void AC_PosControl::update_xy_controller() @@ -823,7 +823,7 @@ void AC_PosControl::update_xy_controller()
float AC_PosControl::time_since_last_xy_update() const
{
uint64_t now_us = AP_HAL::micros64();
const uint64_t now_us = AP_HAL::micros64();
return (now_us - _last_update_xy_us) * 1.0e-6f;
}
@ -894,7 +894,7 @@ void AC_PosControl::init_vel_controller_xyz() @@ -894,7 +894,7 @@ void AC_PosControl::init_vel_controller_xyz()
void AC_PosControl::update_vel_controller_xy()
{
// capture time since last iteration
uint64_t now_us = AP_HAL::micros64();
const uint64_t now_us = AP_HAL::micros64();
float dt = (now_us - _last_update_xy_us) * 1.0e-6f;
// sanity check dt

Loading…
Cancel
Save