aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_PosControl: add is_active_z method

Browse Source 
Consolidated z-axis timeout checks to save 4bytes of RAM
Added POS_CONTROL_ACTIVE_TIMEOUT_MS to make timeout consistent
This commit is contained in:
Randy Mackay 2014-05-02 12:08:18 +09:00
parent 3909a9574b
commit a1f1dd8059
2 changed files with 35 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -35,9 +35,8 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
_pid_rate_lat(pid_rate_lat),
_pid_rate_lon(pid_rate_lon),
_dt(POSCONTROL_DT_10HZ),
_last_update_ms(0),
_last_update_rate_ms(0),
_last_update_accel_ms(0),
_last_update_xy_ms(0),
_last_update_z_ms(0),
_step(0),
_speed_down_cms(POSCONTROL_SPEED_DOWN),
_speed_up_cms(POSCONTROL_SPEED_UP),
@ -65,6 +64,8 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
_flags.recalc_leash_xy = true;
_flags.recalc_leash_z = true;
_flags.keep_xy_I_terms = false;
_flags.reset_rate_to_accel_z = true;
_flags.reset_accel_to_throttle = true;
}
///
@ -181,9 +182,23 @@ void AC_PosControl::init_takeoff()
}
}
// is_active_z - returns true if the z-axis position controller has been run very recently
bool AC_PosControl::is_active_z() const
{
return ((hal.scheduler->millis() - _last_update_z_ms) <= POSCONTROL_ACTIVE_TIMEOUT_MS);
}
/// update_z_controller - fly to altitude in cm above home
void AC_PosControl::update_z_controller()
{
// check time since last cast
uint32_t now = hal.scheduler->millis();
if (now - _last_update_z_ms > POSCONTROL_ACTIVE_TIMEOUT_MS) {
_flags.reset_rate_to_accel_z = true;
_flags.reset_accel_to_throttle = true;
}
_last_update_z_ms = now;
// check if leash lengths need to be recalculated
calc_leash_length_z();
@ -257,7 +272,6 @@ void AC_PosControl::pos_to_rate_z()
// calculates desired acceleration and calls accel throttle controller
void AC_PosControl::rate_to_accel_z(float vel_target_z)
{
uint32_t now = hal.scheduler->millis();
const Vector3f& curr_vel = _inav.get_velocity();
float z_target_speed_delta; // The change in requested speed
float p; // used to capture pid values for logging
@ -277,11 +291,12 @@ void AC_PosControl::rate_to_accel_z(float vel_target_z)
}
// reset velocity error and filter if this controller has just been engaged
if (now - _last_update_rate_ms > 100 ) {
if (_flags.reset_rate_to_accel_z) {
// Reset Filter
_vel_error.z = 0;
_vel_target_filt_z = vel_target_z;
desired_accel = 0;
_flags.reset_rate_to_accel_z = false;
} else {
// calculate rate error and filter with cut off frequency of 2 Hz
//To-Do: adjust constant below based on update rate
@ -291,7 +306,6 @@ void AC_PosControl::rate_to_accel_z(float vel_target_z)
_vel_target_filt_z = _vel_target_filt_z + z_target_speed_delta;
desired_accel = z_target_speed_delta / _dt;
}
_last_update_rate_ms = now;
// calculate p
p = _p_alt_rate.kP() * _vel_error.z;
@ -311,7 +325,6 @@ void AC_PosControl::rate_to_accel_z(float vel_target_z)
// calculates a desired throttle which is sent directly to the motors
void AC_PosControl::accel_to_throttle(float accel_target_z)
{
uint32_t now = hal.scheduler->millis();
float z_accel_meas; // actual acceleration
int32_t p,i,d; // used to capture pid values for logging
@ -319,15 +332,15 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
z_accel_meas = -(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f;
// reset target altitude if this controller has just been engaged
if (now - _last_update_accel_ms > 100) {
if (_flags.reset_accel_to_throttle) {
// Reset Filter
_accel_error.z = 0;
_flags.reset_accel_to_throttle = false;
} else {
// calculate accel error and Filter with fc = 2 Hz
// To-Do: replace constant below with one that is adjusted for update rate
_accel_error.z = _accel_error.z + 0.11164f * (constrain_float(accel_target_z - z_accel_meas, -32000, 32000) - _accel_error.z);
}
_last_update_accel_ms = now;
// separately calculate p, i, d values for logging
p = _pid_alt_accel.get_p(_accel_error.z);
@ -449,8 +462,8 @@ void AC_PosControl::update_xy_controller(bool use_desired_velocity)
{
// catch if we've just been started
uint32_t now = hal.scheduler->millis();
if ((now - _last_update_ms) >= 1000) {
_last_update_ms = now;
if ((now - _last_update_xy_ms) >= POSCONTROL_ACTIVE_TIMEOUT_MS) {
_last_update_xy_ms = now;
if (!_flags.keep_xy_I_terms) {
reset_I_xy();
}
@ -472,8 +485,8 @@ void AC_PosControl::update_xy_controller(bool use_desired_velocity)
switch (_xy_step) {
case 0:
// capture time since last iteration
_dt_xy = (now - _last_update_ms) / 1000.0f;
_last_update_ms = now;
_dt_xy = (now - _last_update_xy_ms) / 1000.0f;
_last_update_xy_ms = now;
// translate any adjustments from pilot to loiter target
desired_vel_to_pos(_dt_xy);

12
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -34,6 +34,8 @@
#define POSCONTROL_DT_10HZ 0.10f // time difference in seconds for 10hz update rate
#define POSCONTROL_ACTIVE_TIMEOUT_MS 200 // position controller is considered active if it has been called within the past 200ms (0.2 seconds)
class AC_PosControl
{
public:
@ -117,6 +119,9 @@ public:
/// init_takeoff - initialises target altitude if we are taking off
void init_takeoff();
// is_active - returns true if the z-axis position controller has been run very recently
bool is_active_z() const;
/// update_z_controller - fly to altitude in cm above home
void update_z_controller();
@ -207,6 +212,8 @@ private:
uint8_t force_recalc_xy : 1; // 1 if we want the xy position controller to run at it's next possible time. set by loiter and wp controllers after they have updated the target position.
uint8_t slow_cpu : 1; // 1 if we are running on a slow_cpu machine. xy position control is broken up into multiple steps
uint8_t keep_xy_I_terms : 1; // 1 if we are to keep I terms when the position controller is next run. Reset automatically back to zero in update_xy_controller.
uint8_t reset_rate_to_accel_z : 1; // 1 if we should reset the rate_to_accel_z step
uint8_t reset_accel_to_throttle : 1; // 1 if we should reset the accel_to_throttle step of the z-axis controller
} _flags;
// limit flags structure
@ -282,9 +289,8 @@ private:
// internal variables
float _dt; // time difference (in seconds) between calls from the main program
uint32_t _last_update_ms; // system time of last update_position_controller call
uint32_t _last_update_rate_ms; // system time of last call to rate_to_accel_z (alt hold accel controller)
uint32_t _last_update_accel_ms; // system time of last call to accel_to_throttle (alt hold accel controller)
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
uint8_t _step; // used to decide which portion of position controller to run during this iteration
float _speed_down_cms; // max descent rate in cm/s
float _speed_up_cms; // max climb rate in cm/s