|
|
|
@ -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
|
|
|
|
|