Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: run velocity controller z-axis at 400hz

Browse Source
This commit is contained in:
Randy Mackay 2015-11-17 15:48:40 +09:00
parent dbc8ce1d69
commit c9340dbeb6
1 changed files with 26 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -707,10 +707,12 @@ void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler)
{ {
// capture time since last iteration // capture time since last iteration
uint32_t now = hal.scheduler->millis(); uint32_t now = hal.scheduler->millis();
float dt = (now - _last_update_xy_ms) / 1000.0f; float dt = (now - _last_update_xy_ms)*0.001f;
// sanity check dt - expect to be called faster than ~5hz // call xy controller
if (dt >= POSCONTROL_ACTIVE_TIMEOUT_MS*1.0e-3f) { if (dt >= get_dt_xy()) {
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f; dt = 0.0f;
} }
@ -729,14 +731,15 @@ void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler)
// run acceleration to lean angle step // run acceleration to lean angle step
accel_to_lean_angles(dt, ekfNavVelGainScaler, false); accel_to_lean_angles(dt, ekfNavVelGainScaler, false);
// update xy update time
_last_update_xy_ms = now;
}
// update altitude target // update altitude target
set_alt_target_from_climb_rate_ff(_vel_desired.z, dt, false); set_alt_target_from_climb_rate_ff(_vel_desired.z, _dt, false);
// run z-axis position controller // run z-axis position controller
update_z_controller(); update_z_controller();
// record update time
_last_update_xy_ms = now;
} }
/// ///