mirror of https://github.com/ArduPilot/ardupilot
AC_PosControl: Move hover throttle calculation to AP_Motors
This commit is contained in:
parent
b27da7699e
commit
dcbb071c07
|
@ -41,7 +41,6 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
|
||||||
_dt_xy(POSCONTROL_DT_50HZ),
|
_dt_xy(POSCONTROL_DT_50HZ),
|
||||||
_last_update_xy_ms(0),
|
_last_update_xy_ms(0),
|
||||||
_last_update_z_ms(0),
|
_last_update_z_ms(0),
|
||||||
_throttle_hover(POSCONTROL_THROTTLE_HOVER),
|
|
||||||
_speed_down_cms(POSCONTROL_SPEED_DOWN),
|
_speed_down_cms(POSCONTROL_SPEED_DOWN),
|
||||||
_speed_up_cms(POSCONTROL_SPEED_UP),
|
_speed_up_cms(POSCONTROL_SPEED_UP),
|
||||||
_speed_cms(POSCONTROL_SPEED),
|
_speed_cms(POSCONTROL_SPEED),
|
||||||
|
@ -307,7 +306,7 @@ void AC_PosControl::init_takeoff()
|
||||||
freeze_ff_z();
|
freeze_ff_z();
|
||||||
|
|
||||||
// shift difference between last motor out and hover throttle into accelerometer I
|
// 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
|
// 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
|
// get d term
|
||||||
d = _pid_accel_z.get_d();
|
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
|
// send throttle to attitude controller with angle boost
|
||||||
_attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
|
_attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
|
||||||
|
|
|
@ -14,7 +14,6 @@
|
||||||
|
|
||||||
|
|
||||||
// position controller default definitions
|
// 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_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 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
|
#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
|
/// called by pos_to_rate_z if z-axis speed or accelerations are changed
|
||||||
void calc_leash_length_z();
|
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
|
/// set_alt_target - set altitude target in cm above home
|
||||||
void set_alt_target(float alt_cm) { _pos_target.z = alt_cm; }
|
void set_alt_target(float alt_cm) { _pos_target.z = alt_cm; }
|
||||||
|
|
||||||
|
@ -377,7 +373,6 @@ private:
|
||||||
float _dt_xy; // time difference (in seconds) between update_xy_controller and update_vel_controller_xyz calls
|
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_xy_ms; // system time of last update_xy_controller call
|
||||||
uint32_t _last_update_z_ms; // system time of last update_z_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_down_cms; // max descent rate in cm/s
|
||||||
float _speed_up_cms; // max climb rate in cm/s
|
float _speed_up_cms; // max climb rate in cm/s
|
||||||
float _speed_cms; // max horizontal speed in cm/s
|
float _speed_cms; // max horizontal speed in cm/s
|
||||||
|
|
Loading…
Reference in New Issue