Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: change int32/int16 to float in accel_to_throttle

Browse Source
This commit is contained in:
Jonathan Challinger 2015-03-26 14:42:42 -07:00 committed by Randy Mackay
parent d14893fcd5
commit 7abd02baf2
1 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -335,7 +335,6 @@ void AC_PosControl::rate_to_accel_z()
// consolidate and constrain target acceleration // consolidate and constrain target acceleration
desired_accel = _accel_feedforward.z + p; desired_accel = _accel_feedforward.z + p;
desired_accel = constrain_int32(desired_accel, -32000, 32000);
// set target for accel based throttle controller // set target for accel based throttle controller
accel_to_throttle(desired_accel); accel_to_throttle(desired_accel);
@ -346,7 +345,7 @@ void AC_PosControl::rate_to_accel_z()
void AC_PosControl::accel_to_throttle(float accel_target_z) void AC_PosControl::accel_to_throttle(float accel_target_z)
{ {
float z_accel_meas; // actual acceleration float z_accel_meas; // actual acceleration
int32_t p,i,d; // used to capture pid values for logging float p,i,d; // used to capture pid values for logging
// Calculate Earth Frame Z acceleration // Calculate Earth Frame Z acceleration
z_accel_meas = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f; z_accel_meas = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
@ -359,7 +358,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
_flags.reset_accel_to_throttle = false; _flags.reset_accel_to_throttle = false;
} else { } else {
// calculate accel error and Filter with fc = 2 Hz // calculate accel error and Filter with fc = 2 Hz
_accel_error.z = _accel_error_filter.apply(constrain_float(accel_target_z - z_accel_meas, -32000, 32000)); _accel_error.z = _accel_error_filter.apply(accel_target_z - z_accel_meas);
} }
// set input to PID // set input to PID
@ -380,7 +379,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
// get d term // get d term
d = _pid_accel_z.get_d(); d = _pid_accel_z.get_d();
int16_t thr_out = (int16_t)p+i+d+_throttle_hover; float thr_out = p+i+d+_throttle_hover;
// send throttle to attitude controller with angle boost // send throttle to attitude controller with angle boost
_attitude_control.set_throttle_out(thr_out, true); _attitude_control.set_throttle_out(thr_out, true);