|
|
|
@ -494,7 +494,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
@@ -494,7 +494,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
|
|
|
|
|
// get d term
|
|
|
|
|
d = _pid_accel_z.get_d(); |
|
|
|
|
|
|
|
|
|
float thr_out = (p+i+d)/1000.0f +_motors.get_throttle_hover(); |
|
|
|
|
float thr_out = (p+i+d)*0.001f +_motors.get_throttle_hover(); |
|
|
|
|
|
|
|
|
|
// send throttle to attitude controller with angle boost
|
|
|
|
|
_attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ); |
|
|
|
@ -660,7 +660,7 @@ void AC_PosControl::update_xy_controller(xy_mode mode, float ekfNavVelGainScaler
@@ -660,7 +660,7 @@ void AC_PosControl::update_xy_controller(xy_mode mode, float ekfNavVelGainScaler
|
|
|
|
|
{ |
|
|
|
|
// compute dt
|
|
|
|
|
uint32_t now = AP_HAL::millis(); |
|
|
|
|
float dt = (now - _last_update_xy_ms) / 1000.0f; |
|
|
|
|
float dt = (now - _last_update_xy_ms)*0.001f; |
|
|
|
|
_last_update_xy_ms = now; |
|
|
|
|
|
|
|
|
|
// sanity check dt - expect to be called faster than ~5hz
|
|
|
|
|