Browse Source

AC_AttitudeControl: use floating-point multiplication instead of a more expensive floating-point division

mission-4.1.18
Dr.-Ing. Amilcar Do Carmo Lucas 7 years ago committed by Tom Pittenger
parent
commit
c7c2c4ff2c
  1. 4
      libraries/AC_AttitudeControl/AC_PosControl.cpp

4
libraries/AC_AttitudeControl/AC_PosControl.cpp

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

Loading…
Cancel
Save