Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: Move hover throttle calculation to AP_Motors

This commit is contained in:
Leonard Hall 2016-03-23 00:27:41 +10:30 committed by Randy Mackay
parent b27da7699e
commit dcbb071c07
2 changed files with 4 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -41,7 +41,6 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
_dt_xy(POSCONTROL_DT_50HZ),
_last_update_xy_ms(0),
_last_update_z_ms(0),
_throttle_hover(POSCONTROL_THROTTLE_HOVER),
_speed_down_cms(POSCONTROL_SPEED_DOWN),
_speed_up_cms(POSCONTROL_SPEED_UP),
_speed_cms(POSCONTROL_SPEED),
@ -307,7 +306,7 @@ void AC_PosControl::init_takeoff()
freeze_ff_z();
// shift difference between last motor out and hover throttle into accelerometer I
_pid_accel_z.set_integrator(_motors.get_throttle()-_throttle_hover);
_pid_accel_z.set_integrator(_motors.get_throttle()-_motors.get_throttle_hover());
}
// is_active_z - returns true if the z-axis position controller has been run very recently
@ -483,7 +482,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 +_throttle_hover;
float thr_out = (p+i+d)/1000.0f +_motors.get_throttle_hover();
// send throttle to attitude controller with angle boost
_attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);

9
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -14,7 +14,6 @@
// position controller default definitions
#define POSCONTROL_THROTTLE_HOVER 0.5f // default throttle required to maintain hover
#define POSCONTROL_ACCELERATION_MIN 50.0f // minimum horizontal acceleration in cm/s/s - used for sanity checking acceleration in leash length calculation
#define POSCONTROL_ACCEL_XY 100.0f // default horizontal acceleration in cm/s/s. This is overwritten by waypoint and loiter controllers
#define POSCONTROL_ACCEL_XY_MAX 980.0f // max horizontal acceleration in cm/s/s that the position velocity controller will ask from the lower accel controller
@ -108,9 +107,6 @@ public:
/// called by pos_to_rate_z if z-axis speed or accelerations are changed
void calc_leash_length_z();
/// set_throttle_hover - update estimated throttle required to maintain hover
void set_throttle_hover(float throttle) { _throttle_hover = throttle; }
/// set_alt_target - set altitude target in cm above home
void set_alt_target(float alt_cm) { _pos_target.z = alt_cm; }
@ -248,7 +244,7 @@ public:
/// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration
/// distance_max allows limiting distance to stopping point
/// results placed in stopping_position vector
/// results placed in stopping_position vector
/// set_accel_xy() should be called before this method to set vehicle acceleration
/// set_leash_length() should have been called before this method
void get_stopping_point_xy(Vector3f &stopping_point) const;
@ -366,7 +362,7 @@ private:
AC_P& _p_pos_z;
AC_P& _p_vel_z;
AC_PID& _pid_accel_z;
AC_P& _p_pos_xy;
AC_P& _p_pos_xy;
AC_PI_2D& _pi_vel_xy;
// parameters
@ -377,7 +373,6 @@ private:
float _dt_xy; // time difference (in seconds) between update_xy_controller and update_vel_controller_xyz calls
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
float _throttle_hover; // estimated throttle required to maintain a level hover
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