zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_PosControl: recalculate leash when speed or accel modified

master
Randy Mackay 11 years ago committed by Andrew Tridgell
parent
d70862e44b
commit
de34359808
2 changed files with 166 additions and 96 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 204
      libraries/AC_AttitudeControl/AC_PosControl.cpp
  2. 58
      libraries/AC_AttitudeControl/AC_PosControl.h

204
libraries/AC_AttitudeControl/AC_PosControl.cpp
Unescape View File

@ -66,16 +66,68 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, @@ -66,16 +66,68 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
#else
_flags.slow_cpu = true;
#endif
_flags.recalc_leash_xy = true;
_flags.recalc_leash_z = true;
}
///
/// z-axis position controller
///
/// set_speed_z - sets maximum climb and descent rates
void AC_PosControl::set_speed_z(float speed_down, float speed_up)
{
if ((fabs(_speed_down_cms-speed_down) > 1.0f) || (fabs(_speed_up_cms-speed_up) > 1.0f)) {
_speed_down_cms = speed_down;
_speed_up_cms = speed_up;
_flags.recalc_leash_z = true;
}
}
/// set_accel_z - set vertical acceleration in cm/s/s
void AC_PosControl::set_accel_z(float accel_cmss)
{
if (fabs(_accel_z_cms-accel_cmss) > 1.0f) {
_accel_z_cms = accel_cmss;
_flags.recalc_leash_z = true;
}
}
/// set_alt_target_with_slew - adjusts target towards a final altitude target
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
{
float alt_change = alt_cm-_pos_target.z;
// adjust desired alt if motors have not hit their limits
if ((alt_change<0 && !_motors.limit.throttle_lower) || (alt_change>0 && !_motors.limit.throttle_upper)) {
_pos_target.z += constrain_float(alt_change, _speed_down_cms*dt, _speed_up_cms*dt);
}
// do not let target get too far from current altitude
float curr_alt = _inav.get_altitude();
_pos_target.z = constrain_float(_pos_target.z,curr_alt-_leash_down_z,curr_alt+_leash_up_z);
}
/// set_alt_target_from_climb_rate - adjusts target up or down using a climb rate in cm/s
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float dt)
{
// adjust desired alt if motors have not hit their limits
// To-Do: add check of _limit.pos_up and _limit.pos_down?
if ((climb_rate_cms<0 && !_motors.limit.throttle_lower) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) {
_pos_target.z += climb_rate_cms * _dt;
}
}
// get_alt_error - returns altitude error in cm
float AC_PosControl::get_alt_error() const
{
return (_pos_target.z - _inav.get_position().z);
return (_pos_target.z - _inav.get_altitude());
}
/// set_target_to_stopping_point_z - returns reasonable stopping altitude in cm above home
@ -119,34 +171,50 @@ void AC_PosControl::init_takeoff() @@ -119,34 +171,50 @@ void AC_PosControl::init_takeoff()
/// update_z_controller - fly to altitude in cm above home
void AC_PosControl::update_z_controller()
{
// check if leash lengths need to be recalculated
calc_leash_length_z();
// call position controller
pos_to_rate_z();
}
/// climb_at_rate - climb at rate provided in cm/s
void AC_PosControl::climb_at_rate(const float rate_target_cms)
/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
/// called by pos_to_rate_z if z-axis speed or accelerations are changed
void AC_PosControl::calc_leash_length_z()
{
const Vector3f& curr_pos = _inav.get_position();
if (_flags.recalc_leash_z) {
_leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _pi_alt_pos.kP());
_leash_down_z = calc_leash_length(_speed_down_cms, _accel_z_cms, _pi_alt_pos.kP());
_flags.recalc_leash_z = false;
// debug -- remove me!
hal.console->printf_P(PSTR("\nLLZ:%4.2f %4.2f\n"),(float)_leash_up_z, (float)_leash_down_z);
}
}
// pos_to_rate_z - position to rate controller for Z axis
// calculates desired rate in earth-frame z axis and passes to rate controller
// vel_up_max, vel_down_max should have already been set before calling this method
void AC_PosControl::pos_to_rate_z()
{
float curr_alt = _inav.get_altitude();
float linear_distance; // half the distance we swap between linear and sqrt and the distance we offset sqrt.
// clear position limit flags
_limit.pos_up = false;
_limit.pos_down = false;
// adjust desired alt if motors have not hit their limits
// To-Do: should we use some other limits? this controller's vel limits?
if ((rate_target_cms<0 && !_motors.limit.throttle_lower) || (rate_target_cms>0 && !_motors.limit.throttle_upper)) {
_pos_target.z += rate_target_cms * _dt;
}
// calculate altitude error
_pos_error.z = _pos_target.z - curr_alt;
// do not let target altitude get too far from current altitude
if (_pos_target.z < curr_pos.z - POSCONTROL_LEASH_Z) {
_pos_target.z = curr_pos.z - POSCONTROL_LEASH_Z;
_limit.pos_down = true;
}
if (_pos_target.z > curr_pos.z + POSCONTROL_LEASH_Z) {
_pos_target.z = curr_pos.z + POSCONTROL_LEASH_Z;
if (_pos_error.z > _leash_up_z) {
_pos_target.z = curr_alt + _leash_up_z;
_limit.pos_up = true;
}
if (_pos_error.z < -_leash_down_z) {
_pos_target.z = curr_alt - _leash_down_z;
_limit.pos_down = true;
}
// do not let target alt get above limit
if (_alt_max > 0 && _pos_target.z > _alt_max) {
@ -154,21 +222,6 @@ void AC_PosControl::climb_at_rate(const float rate_target_cms) @@ -154,21 +222,6 @@ void AC_PosControl::climb_at_rate(const float rate_target_cms)
_limit.pos_up = true;
}
// call position controller
pos_to_rate_z();
}
// pos_to_rate_z - position to rate controller for Z axis
// calculates desired rate in earth-frame z axis and passes to rate controller
// vel_up_max, vel_down_max should have already been set before calling this method
void AC_PosControl::pos_to_rate_z()
{
const Vector3f& curr_pos = _inav.get_position();
float linear_distance; // half the distance we swap between linear and sqrt and the distance we offset sqrt.
// calculate altitude error
_pos_error.z = _pos_target.z - curr_pos.z;
// check kP to avoid division by zero
if (_pi_alt_pos.kP() != 0) {
linear_distance = POSCONTROL_ALT_HOLD_ACCEL_MAX/(2.0f*_pi_alt_pos.kP()*_pi_alt_pos.kP());
@ -285,68 +338,47 @@ void AC_PosControl::accel_to_throttle(float accel_target_z) @@ -285,68 +338,47 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
// to-do add back in PID logging?
}
/*
// get_throttle_althold_with_slew - altitude controller with slew to avoid step changes in altitude target
// calls normal althold controller which updates accel based throttle controller targets
static void
get_throttle_althold_with_slew(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate)
{
float alt_change = target_alt-controller_desired_alt;
// adjust desired alt if motors have not hit their limits
if ((alt_change<0 && !motors.limit.throttle_lower) || (alt_change>0 && !motors.limit.throttle_upper)) {
controller_desired_alt += constrain_float(alt_change, min_climb_rate*0.02f, max_climb_rate*0.02f);
}
// do not let target altitude get too far from current altitude
controller_desired_alt = constrain_float(controller_desired_alt,current_loc.alt-750,current_loc.alt+750);
get_throttle_althold(controller_desired_alt, min_climb_rate-250, max_climb_rate+250); // 250 is added to give head room to alt hold controller
}
///
/// position controller
///
// get_throttle_rate_stabilized - rate controller with additional 'stabilizer'
// 'stabilizer' ensure desired rate is being met
// calls normal throttle rate controller which updates accel based throttle controller targets
static void
get_throttle_rate_stabilized(int16_t target_rate)
/// set_accel_xy - set horizontal acceleration in cm/s/s
/// calc_leash_length_xy should be called afterwards
void AC_PosControl::set_accel_xy(float accel_cmss)
{
// adjust desired alt if motors have not hit their limits
if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) {
controller_desired_alt += target_rate * 0.02f;
if (fabs(_accel_cms-accel_cmss) > 1.0f) {
_accel_cms = accel_cmss;
_flags.recalc_leash_xy = true;
}
}
// do not let target altitude get too far from current altitude
controller_desired_alt = constrain_float(controller_desired_alt,current_loc.alt-750,current_loc.alt+750);
#if AC_FENCE == ENABLED
// do not let target altitude be too close to the fence
// To-Do: add this to other altitude controllers
if((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
float alt_limit = fence.get_safe_alt() * 100.0f;
if (controller_desired_alt > alt_limit) {
controller_desired_alt = alt_limit;
}
/// set_speed_xy - set horizontal speed maximum in cm/s
/// calc_leash_length_xy should be called afterwards
void AC_PosControl::set_speed_xy(float speed_cms)
{
if (fabs(_speed_cms-speed_cms) > 1.0f) {
_speed_cms = speed_cms;
_flags.recalc_leash_xy = true;
}
#endif
// update target altitude for reporting purposes
set_target_alt_for_reporting(controller_desired_alt);
get_throttle_althold(controller_desired_alt, -g.pilot_velocity_z_max-250, g.pilot_velocity_z_max+250); // 250 is added to give head room to alt hold controller
}
*/
///
/// position controller
///
/// set_pos_target in cm from home
void AC_PosControl::set_pos_target(const Vector3f& position)
{
_pos_target = position;
// debug -- remove me!
static uint8_t counter = 0;
counter++;
if (counter >= 10) {
counter = 0;
hal.console->printf_P(PSTR("\nPosX:%4.2f Y:%4.2f Z:%4.2f\n"), (float)_pos_target.x, (float)_pos_target.y, (float)_pos_target.z);
}
// initialise roll and pitch to current roll and pitch. This avoids a twitch between when the target is set and the pos controller is first run
_roll_target = constrain_int32(_ahrs.roll_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max());
_pitch_target = constrain_int32(_ahrs.pitch_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max());
// To-Do: this initialisation of roll and pitch targets needs to go somewhere between when pos-control is initialised and when it completes it's first cycle
//_roll_target = constrain_int32(_ahrs.roll_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max());
//_pitch_target = constrain_int32(_ahrs.pitch_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max());
}
/// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration
@ -408,6 +440,9 @@ void AC_PosControl::update_pos_controller(bool use_desired_velocity) @@ -408,6 +440,9 @@ void AC_PosControl::update_pos_controller(bool use_desired_velocity)
_xy_step = 0;
}
// check if xy leash needs to be recalculated
calc_leash_length_xy();
// reset step back to 0 if loiter or waypoint parents have triggered an update and we completed the last full cycle
if (_flags.force_recalc_xy && _xy_step > 3) {
_flags.force_recalc_xy = false;
@ -447,6 +482,19 @@ void AC_PosControl::update_pos_controller(bool use_desired_velocity) @@ -447,6 +482,19 @@ void AC_PosControl::update_pos_controller(bool use_desired_velocity)
/// private methods
///
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration
/// should be called whenever the speed, acceleration or position kP is modified
void AC_PosControl::calc_leash_length_xy()
{
if (_flags.recalc_leash_xy) {
_leash = calc_leash_length(_speed_cms, _accel_cms, _pi_pos_lon.kP());
_flags.recalc_leash_xy = false;
// debug -- remove me!
hal.console->printf_P(PSTR("\nXYA:%4.2f S:%4.2f kP:%4.2f\n"),(float)_accel_cms, (float)_speed_cms, (float)_pi_pos_lon.kP());
hal.console->printf_P(PSTR("\nLLXY:%4.2f\n"),(float)_leash);
}
}
/// desired_vel_to_pos - move position target using desired velocities
void AC_PosControl::desired_vel_to_pos(float nav_dt)
{

58
libraries/AC_AttitudeControl/AC_PosControl.h
Unescape View File

@ -65,23 +65,39 @@ public: @@ -65,23 +65,39 @@ public:
/// set_speed_z - sets maximum climb and descent rates
/// To-Do: call this in the main code as part of flight mode initialisation
/// calc_leash_length_z should be called afterwards
void set_speed_z(float speed_down, float speed_up) { _speed_down_cms = speed_down; _speed_up_cms = speed_up;}
void set_speed_z(float speed_down, float speed_up);
/// set_accel_z - set vertical acceleration in cm/s/s
/// calc_leash_length_z should be called afterwards
void set_accel_z(float accel_cmss) { _accel_z_cms = accel_cmss; }
void set_accel_z(float accel_cmss);
// set_throttle_hover - update estimated throttle required to maintain hover
/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
/// 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
/// set_alt_target - set altitude target in cm above home
void set_alt_target(float alt_cm) { _pos_target.z = alt_cm; }
/// set_alt_target_with_slew - adjusts target towards a final altitude target
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
void set_alt_target_with_slew(float alt_cm, float dt);
/// set_alt_target_from_climb_rate - adjusts target up or down using a climb rate in cm/s
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
void set_alt_target_from_climb_rate(float climb_rate_cms, float dt);
/// get_alt_target, get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller
/// To-Do: remove one of the two functions below
float get_alt_target() const { return _pos_target.z; }
// get_alt_error - returns altitude error in cm
/// get_alt_error - returns altitude error in cm
float get_alt_error() const;
/// set_target_to_stopping_point_z - sets altitude target to reasonable stopping altitude in cm above home
@ -93,35 +109,28 @@ public: @@ -93,35 +109,28 @@ public:
/// update_z_controller - fly to altitude in cm above home
void update_z_controller();
/// climb_at_rate - climb at rate provided in cm/s
void climb_at_rate(const float rate_target_cms);
// get_leash_down_z, get_leash_up_z - returns vertical leash lengths in cm
float get_leash_down_z() { return _leash_down_z; }
float get_leash_up_z() { return _leash_up_z; }
/// althold_kP - returns altitude hold position control PID's kP gain
float althold_kP() { return _pi_alt_pos.kP(); }
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration
/// should be called whenever the speed, acceleration or position kP is modified
float calc_leash_length_z(float speed_cms, float accel_cms) const { return calc_leash_length(speed_cms, accel_cms, _pi_alt_pos.kP()); }
///
/// xy position controller
///
/// set_accel_xy - set horizontal acceleration in cm/s/s
/// calc_leash_length_xy should be called afterwards
void set_accel_xy(float accel_cmss) { _accel_cms = accel_cmss; }
void set_accel_xy(float accel_cmss);
/// set_speed_xy - set horizontal speed maximum in cm/s
/// calc_leash_length_xy should be called afterwards
void set_speed_xy(float speed_cms) { _speed_cms = speed_cms; }
/// set_leash_xy - sets horizontal leash lenght
/// should be based on results from calc_leash_length_xy() method
void set_leash_xy(float leash_cm) { _leash = leash_cm; }
void set_speed_xy(float speed_cms);
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration
/// should be called whenever the speed, acceleration or position kP is modified
float calc_leash_length_xy(float speed_cms, float accel_cms) const { return calc_leash_length(speed_cms, accel_cms, _pi_pos_lon.kP()); }
void calc_leash_length_xy();
/// get_pos_target - get target as position vector (from home in cm)
const Vector3f& get_pos_target() const { return _pos_target; }
@ -158,6 +167,9 @@ public: @@ -158,6 +167,9 @@ public:
float get_roll() const { return _roll_target; }
float get_pitch() const { return _pitch_target; }
// get_leash_xy - returns horizontal leash length in cm
float get_leash_xy() { return _leash; }
/// accessors for reporting
const Vector3f get_vel_target() { return _vel_target; }
const Vector3f get_accel_target() { return _accel_target; }
@ -190,6 +202,10 @@ private: @@ -190,6 +202,10 @@ private:
uint8_t accel_xy : 1; // 1 if we have hit the horizontal accel limit
} _limit;
///
/// z controller private methods
///
// pos_to_rate_z - position to rate controller for Z axis
// target altitude should be placed into _pos_target.z using or set with one of these functions
// set_alt_target
@ -203,6 +219,10 @@ private: @@ -203,6 +219,10 @@ private:
// accel_to_throttle - alt hold's acceleration controller
void accel_to_throttle(float accel_target_z);
///
/// xy controller private methods
///
/// desired_vel_to_pos - move position target using desired velocities
void desired_vel_to_pos(float nav_dt);
@ -257,6 +277,8 @@ private: @@ -257,6 +277,8 @@ private:
float _accel_z_cms; // max vertical acceleration in cm/s/s
float _accel_cms; // max horizontal acceleration in cm/s/s
float _leash; // horizontal leash length in cm. target will never be further than this distance from the vehicle
float _leash_down_z; // vertical leash down in cm. target will never be further than this distance below the vehicle
float _leash_up_z; // vertical leash up in cm. target will never be further than this distance above the vehicle
float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
float _sin_yaw;
float _cos_pitch;

Write Preview
Loading…
Cancel
Save