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

AC_AttitudeControl: Support changing update period

This commit is contained in:
Leonard Hall 2022-12-05 22:51:43 +10:30 committed by Peter Barker
parent 6eb57d618a
commit 3c69d28237
11 changed files with 80 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -1,6 +1,7 @@
#include "AC_AttitudeControl.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_Scheduler/AP_Scheduler.h>
extern const AP_HAL::HAL& hal;
@ -196,9 +197,9 @@ void AC_AttitudeControl::reset_rate_controller_I_terms()
// reset rate controller I terms smoothly to zero in 0.5 seconds
void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly()
{
get_rate_roll_pid().relax_integrator(0.0, AC_ATTITUDE_RATE_RELAX_TC);
get_rate_pitch_pid().relax_integrator(0.0, AC_ATTITUDE_RATE_RELAX_TC);
get_rate_yaw_pid().relax_integrator(0.0, AC_ATTITUDE_RATE_RELAX_TC);
get_rate_roll_pid().relax_integrator(0.0, _dt, AC_ATTITUDE_RATE_RELAX_TC);
get_rate_pitch_pid().relax_integrator(0.0, _dt, AC_ATTITUDE_RATE_RELAX_TC);
get_rate_yaw_pid().relax_integrator(0.0, _dt, AC_ATTITUDE_RATE_RELAX_TC);
}
// The attitude controller works around the concept of the desired attitude, target attitude
@ -224,7 +225,7 @@ void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly()
// trust vector drops below 2*AC_ATTITUDE_THRUST_ERROR_ANGLE. At this point the heading is also corrected.
// Command a Quaternion attitude with feedforward and smoothing
// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity
// attitude_desired_quat: is updated on each time_step by the integral of the angular velocity
void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target)
{
Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat;
@ -1067,7 +1068,8 @@ float AC_AttitudeControl::get_althold_lean_angle_max_cd() const
// Return roll rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_roll()
{
float alpha = MIN(get_rate_roll_pid().get_filt_E_alpha(), get_rate_roll_pid().get_filt_D_alpha());
float dt_average = AP::scheduler().get_filtered_loop_time();
float alpha = MIN(get_rate_roll_pid().get_filt_E_alpha(dt_average), get_rate_roll_pid().get_filt_D_alpha(dt_average));
float alpha_remaining = 1 - alpha;
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
@ -1081,7 +1083,8 @@ float AC_AttitudeControl::max_rate_step_bf_roll()
// Return pitch rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_pitch()
{
float alpha = MIN(get_rate_pitch_pid().get_filt_E_alpha(), get_rate_pitch_pid().get_filt_D_alpha());
float dt_average = AP::scheduler().get_filtered_loop_time();
float alpha = MIN(get_rate_pitch_pid().get_filt_E_alpha(dt_average), get_rate_pitch_pid().get_filt_D_alpha(dt_average));
float alpha_remaining = 1 - alpha;
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
@ -1095,7 +1098,8 @@ float AC_AttitudeControl::max_rate_step_bf_pitch()
// Return yaw rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_yaw()
{
float alpha = MIN(get_rate_yaw_pid().get_filt_E_alpha(), get_rate_yaw_pid().get_filt_D_alpha());
float dt_average = AP::scheduler().get_filtered_loop_time();
float alpha = MIN(get_rate_yaw_pid().get_filt_E_alpha(dt_average), get_rate_yaw_pid().get_filt_D_alpha(dt_average));
float alpha_remaining = 1 - alpha;
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);

10
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -49,12 +49,10 @@ class AC_AttitudeControl {
public:
AC_AttitudeControl( AP_AHRS_View &ahrs,
const AP_MultiCopter &aparm,
AP_Motors& motors,
float dt) :
AP_Motors& motors) :
_p_angle_roll(AC_ATTITUDE_CONTROL_ANGLE_P),
_p_angle_pitch(AC_ATTITUDE_CONTROL_ANGLE_P),
_p_angle_yaw(AC_ATTITUDE_CONTROL_ANGLE_P),
_dt(dt),
_angle_boost(0),
_use_sqrt_controller(true),
_throttle_rpy_mix_desired(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
@ -74,6 +72,12 @@ public:
// Empty destructor to suppress compiler warning
virtual ~AC_AttitudeControl() {}
// set_dt / get_dt - dt is the time since the last time the attitude controllers were updated
// _dt should be set based on the time of the last IMU read used by these controllers
// the attitude controller should run updates for active controllers on each loop to ensure normal operation
void set_dt(float dt) { _dt = dt; }
float get_dt() const { return _dt; }
// pid accessors
AC_P& get_angle_roll_p() { return _p_angle_roll; }
AC_P& get_angle_pitch_p() { return _p_angle_pitch; }

6
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -387,13 +387,13 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_r
if (_flags_heli.leaky_i) {
_pid_rate_roll.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
}
float roll_pid = _pid_rate_roll.update_all(rate_roll_target_rads, rate_rads.x, _motors.limit.roll) + _actuator_sysid.x;
float roll_pid = _pid_rate_roll.update_all(rate_roll_target_rads, rate_rads.x, _dt, _motors.limit.roll) + _actuator_sysid.x;
if (_flags_heli.leaky_i) {
_pid_rate_pitch.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
}
float pitch_pid = _pid_rate_pitch.update_all(rate_pitch_target_rads, rate_rads.y, _motors.limit.pitch) + _actuator_sysid.y;
float pitch_pid = _pid_rate_pitch.update_all(rate_pitch_target_rads, rate_rads.y, _dt, _motors.limit.pitch) + _actuator_sysid.y;
// use pid library to calculate ff
float roll_ff = _pid_rate_roll.get_ff();
@ -449,7 +449,7 @@ float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_yaw_actual_ra
_pid_rate_yaw.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
}
float pid = _pid_rate_yaw.update_all(rate_target_rads, rate_yaw_actual_rads, _motors.limit.yaw) + _actuator_sysid.z;
float pid = _pid_rate_yaw.update_all(rate_target_rads, rate_yaw_actual_rads, _dt, _motors.limit.yaw) + _actuator_sysid.z;
// use pid library to calculate ff
float vff = _pid_rate_yaw.get_ff()*_feedforward_scalar;

11
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -33,12 +33,11 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl {
public:
AC_AttitudeControl_Heli( AP_AHRS_View &ahrs,
const AP_MultiCopter &aparm,
AP_MotorsHeli& motors,
float dt) :
AC_AttitudeControl(ahrs, aparm, motors, dt),
_pid_rate_roll(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_FF, AC_ATC_HELI_RATE_RP_IMAX, AC_ATTITUDE_HELI_RATE_RP_FF_FILTER, AC_ATC_HELI_RATE_RP_FILT_HZ, 0.0f, dt),
_pid_rate_pitch(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_FF, AC_ATC_HELI_RATE_RP_IMAX, AC_ATTITUDE_HELI_RATE_RP_FF_FILTER, AC_ATC_HELI_RATE_RP_FILT_HZ, 0.0f, dt),
_pid_rate_yaw(AC_ATC_HELI_RATE_YAW_P, AC_ATC_HELI_RATE_YAW_I, AC_ATC_HELI_RATE_YAW_D, AC_ATC_HELI_RATE_YAW_FF, AC_ATC_HELI_RATE_YAW_IMAX, AC_ATTITUDE_HELI_RATE_Y_FF_FILTER, AC_ATC_HELI_RATE_YAW_FILT_HZ, 0.0f, dt)
AP_MotorsHeli& motors) :
AC_AttitudeControl(ahrs, aparm, motors),
_pid_rate_roll(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_FF, AC_ATC_HELI_RATE_RP_IMAX, AC_ATTITUDE_HELI_RATE_RP_FF_FILTER, AC_ATC_HELI_RATE_RP_FILT_HZ, 0.0f),
_pid_rate_pitch(AC_ATC_HELI_RATE_RP_P, AC_ATC_HELI_RATE_RP_I, AC_ATC_HELI_RATE_RP_D, AC_ATC_HELI_RATE_RP_FF, AC_ATC_HELI_RATE_RP_IMAX, AC_ATTITUDE_HELI_RATE_RP_FF_FILTER, AC_ATC_HELI_RATE_RP_FILT_HZ, 0.0f),
_pid_rate_yaw(AC_ATC_HELI_RATE_YAW_P, AC_ATC_HELI_RATE_YAW_I, AC_ATC_HELI_RATE_YAW_D, AC_ATC_HELI_RATE_YAW_FF, AC_ATC_HELI_RATE_YAW_IMAX, AC_ATTITUDE_HELI_RATE_Y_FF_FILTER, AC_ATC_HELI_RATE_YAW_FILT_HZ, 0.0f)
{
AP_Param::setup_object_defaults(this, var_info);

16
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -235,12 +235,12 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
AP_GROUPEND
};
AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt) :
AC_AttitudeControl(ahrs, aparm, motors, dt),
AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors) :
AC_AttitudeControl(ahrs, aparm, motors),
_motors_multi(motors),
_pid_rate_roll(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt),
_pid_rate_pitch(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt),
_pid_rate_yaw(AC_ATC_MULTI_RATE_YAW_P, AC_ATC_MULTI_RATE_YAW_I, AC_ATC_MULTI_RATE_YAW_D, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, AC_ATC_MULTI_RATE_YAW_FILT_HZ, 0.0f, dt)
_pid_rate_roll(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ),
_pid_rate_pitch(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ),
_pid_rate_yaw(AC_ATC_MULTI_RATE_YAW_P, AC_ATC_MULTI_RATE_YAW_I, AC_ATC_MULTI_RATE_YAW_D, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, AC_ATC_MULTI_RATE_YAW_FILT_HZ, 0.0f)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -349,13 +349,13 @@ void AC_AttitudeControl_Multi::rate_controller_run()
Vector3f gyro_latest = _ahrs.get_gyro_latest();
_motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x);
_motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll) + _actuator_sysid.x);
_motors.set_roll_ff(get_rate_roll_pid().get_ff());
_motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y);
_motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _dt, _motors.limit.pitch) + _actuator_sysid.y);
_motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
_motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
_motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _dt, _motors.limit.yaw) + _actuator_sysid.z);
_motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
_sysid_ang_vel_body.zero();

2
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
View File

@ -41,7 +41,7 @@
class AC_AttitudeControl_Multi : public AC_AttitudeControl {
public:
AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt);
AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors);
// empty destructor to suppress compiler warning
virtual ~AC_AttitudeControl_Multi() {}

4
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h
View File

@ -5,8 +5,8 @@
class AC_AttitudeControl_Multi_6DoF : public AC_AttitudeControl_Multi {
public:
AC_AttitudeControl_Multi_6DoF(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt):
AC_AttitudeControl_Multi(ahrs,aparm,motors,dt) {
AC_AttitudeControl_Multi_6DoF(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors):
AC_AttitudeControl_Multi(ahrs,aparm,motors) {
if (_singleton != nullptr) {
AP_HAL::panic("Can only be one AC_AttitudeControl_Multi_6DoF");

16
libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp
View File

@ -259,12 +259,12 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = {
AP_GROUPEND
};
AC_AttitudeControl_Sub::AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt) :
AC_AttitudeControl(ahrs, aparm, motors, dt),
AC_AttitudeControl_Sub::AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors) :
AC_AttitudeControl(ahrs, aparm, motors),
_motors_multi(motors),
_pid_rate_roll(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ, dt),
_pid_rate_pitch(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ, dt),
_pid_rate_yaw(AC_ATC_SUB_RATE_YAW_P, AC_ATC_SUB_RATE_YAW_I, AC_ATC_SUB_RATE_YAW_D, 0.0f, AC_ATC_SUB_RATE_YAW_IMAX, AC_ATC_SUB_RATE_YAW_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_YAW_FILT_HZ, dt)
_pid_rate_roll(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ),
_pid_rate_pitch(AC_ATC_SUB_RATE_RP_P, AC_ATC_SUB_RATE_RP_I, AC_ATC_SUB_RATE_RP_D, 0.0f, AC_ATC_SUB_RATE_RP_IMAX, AC_ATC_SUB_RATE_RP_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_RP_FILT_HZ),
_pid_rate_yaw(AC_ATC_SUB_RATE_YAW_P, AC_ATC_SUB_RATE_YAW_I, AC_ATC_SUB_RATE_YAW_D, 0.0f, AC_ATC_SUB_RATE_YAW_IMAX, AC_ATC_SUB_RATE_YAW_FILT_HZ, 0.0f, AC_ATC_SUB_RATE_YAW_FILT_HZ)
{
AP_Param::setup_object_defaults(this, var_info);
@ -349,9 +349,9 @@ void AC_AttitudeControl_Sub::rate_controller_run()
update_throttle_rpy_mix();
Vector3f gyro_latest = _ahrs.get_gyro_latest();
_motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _motors.limit.roll));
_motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _motors.limit.pitch));
_motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _motors.limit.yaw));
_motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll));
_motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _dt, _motors.limit.pitch));
_motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _dt, _motors.limit.yaw));
control_monitor_update();
}

2
libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h
View File

@ -25,7 +25,7 @@
class AC_AttitudeControl_Sub : public AC_AttitudeControl {
public:
AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors, float dt);
AC_AttitudeControl_Sub(AP_AHRS_View &ahrs, const AP_MultiCopter &aparm, AP_MotorsMulticopter& motors);
// empty destructor to suppress compiler warning
virtual ~AC_AttitudeControl_Sub() {}

47
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -4,6 +4,7 @@
#include <AP_Logger/AP_Logger.h>
#include <AP_Motors/AP_Motors.h> // motors library
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
extern const AP_HAL::HAL& hal;
@ -300,17 +301,16 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = {
// their values.
//
AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
const AP_Motors& motors, AC_AttitudeControl& attitude_control, float dt) :
const AP_Motors& motors, AC_AttitudeControl& attitude_control) :
_ahrs(ahrs),
_inav(inav),
_motors(motors),
_attitude_control(attitude_control),
_p_pos_z(POSCONTROL_POS_Z_P, dt),
_pid_vel_z(POSCONTROL_VEL_Z_P, 0.0f, 0.0f, 0.0f, POSCONTROL_VEL_Z_IMAX, POSCONTROL_VEL_Z_FILT_HZ, POSCONTROL_VEL_Z_FILT_D_HZ, dt),
_pid_accel_z(POSCONTROL_ACC_Z_P, POSCONTROL_ACC_Z_I, POSCONTROL_ACC_Z_D, 0.0f, POSCONTROL_ACC_Z_IMAX, 0.0f, POSCONTROL_ACC_Z_FILT_HZ, 0.0f, dt),
_p_pos_xy(POSCONTROL_POS_XY_P, dt),
_pid_vel_xy(POSCONTROL_VEL_XY_P, POSCONTROL_VEL_XY_I, POSCONTROL_VEL_XY_D, 0.0f, POSCONTROL_VEL_XY_IMAX, POSCONTROL_VEL_XY_FILT_HZ, POSCONTROL_VEL_XY_FILT_D_HZ, dt),
_dt(dt),
_p_pos_z(POSCONTROL_POS_Z_P),
_pid_vel_z(POSCONTROL_VEL_Z_P, 0.0f, 0.0f, 0.0f, POSCONTROL_VEL_Z_IMAX, POSCONTROL_VEL_Z_FILT_HZ, POSCONTROL_VEL_Z_FILT_D_HZ),
_pid_accel_z(POSCONTROL_ACC_Z_P, POSCONTROL_ACC_Z_I, POSCONTROL_ACC_Z_D, 0.0f, POSCONTROL_ACC_Z_IMAX, 0.0f, POSCONTROL_ACC_Z_FILT_HZ, 0.0f),
_p_pos_xy(POSCONTROL_POS_XY_P),
_pid_vel_xy(POSCONTROL_VEL_XY_P, POSCONTROL_VEL_XY_I, POSCONTROL_VEL_XY_D, 0.0f, POSCONTROL_VEL_XY_IMAX, POSCONTROL_VEL_XY_FILT_HZ, POSCONTROL_VEL_XY_FILT_D_HZ),
_vel_max_down_cms(POSCONTROL_SPEED_DOWN),
_vel_max_up_cms(POSCONTROL_SPEED_UP),
_vel_max_xy_cms(POSCONTROL_SPEED),
@ -458,8 +458,10 @@ void AC_PosControl::relax_velocity_controller_xy()
{
// decay acceleration and therefore current attitude target to zero
// this will be reset by init_xy_controller() if !is_active_xy()
float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC);
_accel_target.xy() *= decay;
if (is_positive(_dt)) {
float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC);
_accel_target.xy() *= decay;
}
init_xy_controller();
}
@ -468,7 +470,9 @@ void AC_PosControl::relax_velocity_controller_xy()
void AC_PosControl::soften_for_landing_xy()
{
// decay position error to zero
_pos_target.xy() += (_inav.get_position_xy_cm().topostype() - _pos_target.xy()) * (_dt / (_dt + POSCONTROL_RELAX_TC));
if (is_positive(_dt)) {
_pos_target.xy() += (_inav.get_position_xy_cm().topostype() - _pos_target.xy()) * (_dt / (_dt + POSCONTROL_RELAX_TC));
}
// Prevent I term build up in xy velocity controller.
// Note that this flag is reset on each loop in update_xy_controller()
@ -515,7 +519,7 @@ void AC_PosControl::init_xy_controller()
init_ekf_xy_reset();
// initialise z_controller time out
_last_update_xy_us = AP_HAL::micros64();
_last_update_xy_us = AP::ins().get_last_update_usec();
}
/// input_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
@ -583,10 +587,10 @@ void AC_PosControl::stop_vel_xy_stabilisation()
_pid_vel_xy.reset_I();
}
// is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times
// is_active_xy - returns true if the xy position controller has bee n run in the previous 5 loop times
bool AC_PosControl::is_active_xy() const
{
return ((AP_HAL::micros64() - _last_update_xy_us) <= _dt * 5000000.0);
return ((AP::ins().get_last_update_usec() - _last_update_xy_us) <= _dt * 1500000.0);
}
/// update_xy_controller - runs the horizontal position controller correcting position, velocity and acceleration errors.
@ -606,7 +610,7 @@ void AC_PosControl::update_xy_controller()
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}
_last_update_xy_us = AP_HAL::micros64();
_last_update_xy_us = AP::ins().get_last_update_usec();
float ahrsGndSpdLimit, ahrsControlScaleXY;
AP::ahrs().getControlLimits(ahrsGndSpdLimit, ahrsControlScaleXY);
@ -624,7 +628,7 @@ void AC_PosControl::update_xy_controller()
// Velocity Controller
const Vector2f &curr_vel = _inav.get_velocity_xy_cms();
Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), curr_vel, _limit_vector.xy());
Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), curr_vel, _dt, _limit_vector.xy());
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
accel_target *= ahrsControlScaleXY;
@ -734,7 +738,7 @@ void AC_PosControl::relax_z_controller(float throttle_setting)
// init_z_controller has set the accel PID I term to generate the current throttle set point
// Use relax_integrator to decay the throttle set point to throttle_setting
_pid_accel_z.relax_integrator((throttle_setting - _motors.get_throttle_hover()) * 1000.0f, POSCONTROL_RELAX_TC);
_pid_accel_z.relax_integrator((throttle_setting - _motors.get_throttle_hover()) * 1000.0f, _dt, POSCONTROL_RELAX_TC);
}
/// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
@ -775,7 +779,7 @@ void AC_PosControl::init_z_controller()
init_ekf_z_reset();
// initialise z_controller time out
_last_update_z_us = AP_HAL::micros64();
_last_update_z_us = AP::ins().get_last_update_usec();
}
/// input_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
@ -885,7 +889,6 @@ void AC_PosControl::set_alt_target_with_slew(float pos)
/// update_pos_offset_z - updates the vertical offsets used by terrain following
void AC_PosControl::update_pos_offset_z(float pos_offset_z)
{
postype_t p_offset_z = _pos_offset_z;
update_pos_vel_accel(p_offset_z, _vel_offset_z, _accel_offset_z, _dt, MIN(_limit_vector.z, 0.0f), _p_pos_z.get_error(), _pid_vel_z.get_error());
_pos_offset_z = p_offset_z;
@ -901,7 +904,7 @@ void AC_PosControl::update_pos_offset_z(float pos_offset_z)
// is_active_z - returns true if the z position controller has been run in the previous 5 loop times
bool AC_PosControl::is_active_z() const
{
return ((AP_HAL::micros64() - _last_update_z_us) <= _dt * 5000000.0);
return ((AP::ins().get_last_update_usec() - _last_update_z_us) <= _dt * 1500000.0);
}
/// update_z_controller - runs the vertical position controller correcting position, velocity and acceleration errors.
@ -921,7 +924,7 @@ void AC_PosControl::update_z_controller()
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}
_last_update_z_us = AP_HAL::micros64();
_last_update_z_us = AP::ins().get_last_update_usec();
// calculate the target velocity correction
float pos_target_zf = _pos_target.z;
@ -937,7 +940,7 @@ void AC_PosControl::update_z_controller()
// Velocity Controller
const float curr_vel_z = _inav.get_velocity_z_up_cms();
_accel_target.z = _pid_vel_z.update_all(_vel_target.z, curr_vel_z, _motors.limit.throttle_lower, _motors.limit.throttle_upper);
_accel_target.z = _pid_vel_z.update_all(_vel_target.z, curr_vel_z, _dt, _motors.limit.throttle_lower, _motors.limit.throttle_upper);
_accel_target.z *= AP::ahrs().getControlScaleZ();
// add feed forward component
@ -956,7 +959,7 @@ void AC_PosControl::update_z_controller()
if (_vibe_comp_enabled) {
thr_out = get_throttle_with_vibration_override();
} else {
thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f;
thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, _dt, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f;
thr_out += _pid_accel_z.get_ff() * 0.001f;
}
thr_out += _motors.get_throttle_hover();

13
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -39,9 +39,14 @@ public:
/// Constructor
AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
const class AP_Motors& motors, AC_AttitudeControl& attitude_control, float dt);
const class AP_Motors& motors, AC_AttitudeControl& attitude_control);
/// get_dt - gets time delta in seconds for all position controllers
/// set_dt / get_dt - dt is the time since the last time the position controllers were updated
/// _dt should be set based on the time of the last IMU read used by these controllers
/// the position controller should run updates for active controllers on each loop to ensure normal operation
void set_dt(float dt) { _dt = dt; }
float get_dt() const { return _dt; }
/// get_shaping_jerk_xy_cmsss - gets the jerk limit of the xy kinematic path generation in cm/s/s/s
@ -417,7 +422,7 @@ protected:
// references to inertial nav and ahrs libraries
AP_AHRS_View& _ahrs;
const AP_InertialNav& _inav;
const class AP_Motors& _motors;
const class AP_Motors& _motors;
AC_AttitudeControl& _attitude_control;
// parameters
@ -431,7 +436,7 @@ protected:
AC_PID _pid_accel_z; // Z axis acceleration controller to convert desired acceleration to throttle output
// internal variables
float _dt; // time difference (in seconds) between calls from the main program
float _dt; // time difference (in seconds) since the last loop time
uint64_t _last_update_xy_us; // system time (in microseconds) since last update_xy_controller call
uint64_t _last_update_z_us; // system time (in microseconds) since last update_z_controller call
float _vel_max_xy_cms; // max horizontal speed in cm/s used for kinematic shaping