Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: increase accuracy of dt calcs

Browse Source
This commit is contained in:
Randy Mackay 2019-01-24 20:20:12 +09:00
parent 0be7a7cf2d
commit 4f74075e53
2 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

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

6
libraries/AC_AttitudeControl/AC_PosControl.h
View File

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