From dcac12410578c4ffafc3c802457f3f603088b2c4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 28 Dec 2013 23:04:45 +0900 Subject: [PATCH] AC_PosControl: add throttle controller --- .../AC_AttitudeControl/AC_PosControl.cpp | 784 +++++------------- libraries/AC_AttitudeControl/AC_PosControl.h | 125 ++- 2 files changed, 295 insertions(+), 614 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 3b9773488d..2ec9e7d81c 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -5,34 +5,13 @@ extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AC_PosControl::var_info[] PROGMEM = { - // index 0 was used for the old orientation matrix - - // @Param: SPEED - // @DisplayName: Position Controller Maximum Horizontal Speed - // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally during a WP mission - // @Units: cm/s - // @Range: 0 2000 - // @Increment: 50 - // @User: Standard - AP_GROUPINFO("SPEED", 0, AC_PosControl, _wp_speed_cms, WPNAV_WP_SPEED), - - // @Param: SPEED_UP - // @DisplayName: Position Controller Maximum Speed Up - // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain while climbing during a WP mission - // @Units: cm/s + // @Param: THR_HOVER + // @DisplayName: Throttle Hover + // @Description: The autopilot's estimate of the throttle required to maintain a level hover. Calculated automatically from the pilot's throttle input while in stabilize mode // @Range: 0 1000 - // @Increment: 50 - // @User: Standard - AP_GROUPINFO("SPEED_UP", 1, AC_PosControl, _wp_speed_up_cms, WPNAV_WP_SPEED_UP), - - // @Param: SPEED_DN - // @DisplayName: Position Controller Maximum Speed Down - // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain while descending during a WP mission - // @Units: cm/s - // @Range: 0 1000 - // @Increment: 50 - // @User: Standard - AP_GROUPINFO("SPEED_DN", 2, AC_PosControl, _wp_speed_down_cms, WPNAV_WP_SPEED_DOWN), + // @Units: Percent*10 + // @User: Advanced + AP_GROUPINFO("THR_HOVER", 0, AC_PosControl, _throttle_hover, POSCONTROL_THROTTLE_HOVER), AP_GROUPEND }; @@ -41,101 +20,243 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] PROGMEM = { // Note that the Vector/Matrix constructors already implicitly zero // their values. // -AC_PosControl::AC_PosControl(const AP_InertialNav& inav, const AP_AHRS& ahrs, const AC_AttitudeControl& attitude_control, - APM_PI& pi_alt_pos, AC_PID& pid_alt_rate, AC_PID& pid_alt_accel, - APM_PI& pi_pos_lat, APM_PI& pi_pos_lon, AC_PID& pid_rate_lat, AC_PID& pid_rate_lon) - _inav(inav), +AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, + const AP_Motors& motors, AC_AttitudeControl& attitude_control, + APM_PI& pi_alt_pos, AC_PID& pid_alt_rate, AC_PID& pid_alt_accel, + APM_PI& pi_pos_lat, APM_PI& pi_pos_lon, AC_PID& pid_rate_lat, AC_PID& pid_rate_lon) : _ahrs(ahrs), + _inav(inav), + _motors(motors), _attitude_control(attitude_control), - _pi_alt_hold(pi_alt_pos), + _pi_alt_pos(pi_alt_pos), _pid_alt_rate(pid_alt_rate), _pid_alt_accel(pid_alt_accel), - _pid_pos_lat(pid_pos_lat), - _pid_pos_lon(pid_pos_lon), + _pi_pos_lat(pi_pos_lat), + _pi_pos_lon(pi_pos_lon), _pid_rate_lat(pid_rate_lat), _pid_rate_lon(pid_rate_lon), - _last_update(0), + _dt(POSCONTROL_DT_10HZ), + _last_update_ms(0), + _last_update_rate_ms(0), + _last_update_accel_ms(0), + _step(0), + _speed_cms(POSCONTROL_SPEED), + _vel_z_min(POSCONTROL_VEL_Z_MIN), + _vel_z_max(POSCONTROL_VEL_Z_MAX), + _accel_cms(POSCONTROL_ACCEL_XY_MAX), // To-Do: check this default _cos_yaw(1.0), _sin_yaw(0.0), _cos_pitch(1.0), - _desired_roll(0), - _desired_pitch(0), - _target(0,0,0), - _target_vel(0,0,0), - _vel_last(0,0,0), - _loiter_leash(WPNAV_MIN_LEASH_LENGTH), - _loiter_accel_cms(WPNAV_LOITER_ACCEL_MAX), - _lean_angle_max_cd(MAX_LEAN_ANGLE), - desired_vel(0,0), - desired_accel(0,0) + _desired_roll(0.0), + _desired_pitch(0.0), + _leash(POSCONTROL_LEASH_LENGTH_MIN) { AP_Param::setup_object_defaults(this, var_info); - // calculate loiter leash - calculate_leash_length(); + // calculate leash length + //calculate_leash_length(); } -// get_initial_alt_hold - get new target altitude based on current altitude and climb rate -static int32_t -get_initial_alt_hold( int32_t alt_cm, int16_t climb_rate_cms) +/// +/// z-axis position controller +/// + +/// get_stopping_point_z - returns reasonable stopping altitude in cm above home +float AC_PosControl::get_stopping_point_z() { - int32_t target_alt; - int32_t linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt. - int32_t linear_velocity; // the velocity we swap between linear and sqrt. + const Vector3f& curr_pos = _inav.get_position(); + const Vector3f& curr_vel = _inav.get_velocity(); + float target_alt; + float linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt + float linear_velocity; // the velocity we swap between linear and sqrt - linear_velocity = ALT_HOLD_ACCEL_MAX/g.pi_alt_hold.kP(); + // calculate the velocity at which we switch from calculating the stopping point using a linear funcction to a sqrt function + linear_velocity = POSCONTROL_ALT_HOLD_ACCEL_MAX/_pi_alt_pos.kP(); - if (abs(climb_rate_cms) < linear_velocity) { - target_alt = alt_cm + climb_rate_cms/g.pi_alt_hold.kP(); + if (fabs(curr_vel.z) < linear_velocity) { + // if our current velocity is below the cross-over point we use a linear function + target_alt = curr_pos.z + curr_vel.z/_pi_alt_pos.kP(); } else { - linear_distance = ALT_HOLD_ACCEL_MAX/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP()); - if (climb_rate_cms > 0){ - target_alt = alt_cm + linear_distance + (int32_t)climb_rate_cms*(int32_t)climb_rate_cms/(2*ALT_HOLD_ACCEL_MAX); + linear_distance = POSCONTROL_ALT_HOLD_ACCEL_MAX/(2.0f*_pi_alt_pos.kP()*_pi_alt_pos.kP()); + if (curr_vel.z > 0){ + target_alt = curr_pos.z + (linear_distance + curr_vel.z*curr_vel.z/(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX)); } else { - target_alt = alt_cm - ( linear_distance + (int32_t)climb_rate_cms*(int32_t)climb_rate_cms/(2*ALT_HOLD_ACCEL_MAX) ); + target_alt = curr_pos.z - (linear_distance + curr_vel.z*curr_vel.z/(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX)); } } - return constrain_int32(target_alt, alt_cm - ALT_HOLD_INIT_MAX_OVERSHOOT, alt_cm + ALT_HOLD_INIT_MAX_OVERSHOOT); + return constrain_float(target_alt, curr_pos.z - POSCONTROL_STOPPING_DIST_Z_MAX, curr_pos.z + POSCONTROL_STOPPING_DIST_Z_MAX); } -// get_throttle_althold - hold at the desired altitude in cm -// updates accel based throttle controller targets -// Note: max_climb_rate is an optional parameter to allow reuse of this function by landing controller -static void -get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate) +/// fly_to_z - fly to altitude in cm above home +void AC_PosControl::fly_to_z(const float alt_cm) { - int32_t alt_error; - float desired_rate; - int32_t linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt. + // call position controller + pos_to_rate_z(alt_cm); +} + +/// climb_at_rate - climb at rate provided in cm/s +void AC_PosControl::climb_at_rate(const float rate_target_cms) +{ + const Vector3f& curr_pos = _inav.get_position(); + + // 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; + } + + // 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; + _limit.pos_up = true; + } + + // do not let target alt get above limit + if (_alt_max > 0 && _pos_target.z > _alt_max) { + _pos_target.z = _alt_max; + _limit.pos_up = true; + } + + // call position controller + pos_to_rate_z(_pos_target.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 alt_cm) +{ + const Vector3f& curr_pos = _inav.get_position(); + float linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt. // calculate altitude error - alt_error = target_alt - current_loc.alt; + _pos_error.z = alt_cm - curr_pos.z; // check kP to avoid division by zero - if( g.pi_alt_hold.kP() != 0 ) { - linear_distance = ALT_HOLD_ACCEL_MAX/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP()); - if( alt_error > 2*linear_distance ) { - desired_rate = safe_sqrt(2*ALT_HOLD_ACCEL_MAX*(alt_error-linear_distance)); - }else if( alt_error < -2*linear_distance ) { - desired_rate = -safe_sqrt(2*ALT_HOLD_ACCEL_MAX*(-alt_error-linear_distance)); + if (_pi_alt_pos.kP() != 0) { + linear_distance = POSCONTROL_ALT_HOLD_ACCEL_MAX/(2.0f*_pi_alt_pos.kP()*_pi_alt_pos.kP()); + if (_pos_error.z > 2*linear_distance ) { + _vel_target.z = safe_sqrt(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX*(_pos_error.z-linear_distance)); + }else if (_pos_error.z < -2.0f*linear_distance) { + _vel_target.z = -safe_sqrt(2.0f*POSCONTROL_ALT_HOLD_ACCEL_MAX*(-_pos_error.z-linear_distance)); }else{ - desired_rate = g.pi_alt_hold.get_p(alt_error); + _vel_target.z = _pi_alt_pos.get_p(_pos_error.z); } }else{ - desired_rate = 0; + _vel_target.z = 0; } - desired_rate = constrain_float(desired_rate, min_climb_rate, max_climb_rate); - // call rate based throttle controller which will update accel based throttle controller targets - get_throttle_rate(desired_rate); - - // update altitude error reported to GCS - altitude_error = alt_error; - - // TO-DO: enabled PID logging for this controller + rate_to_accel_z(_vel_target.z); } +// rate_to_accel_z - calculates desired accel required to achieve the velocity target +// 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 + float desired_accel; // the target acceleration if the accel based throttle is enabled, otherwise the output to be sent to the motors + + // check speed limits + // To-Do: check these speed limits here or in the pos->rate controller + _limit.vel_up = false; + _limit.vel_down = false; + if (_vel_target.z < _vel_z_min) { + _vel_target.z = _vel_z_min; + _limit.vel_down = true; + } + if (_vel_target.z > _vel_z_max) { + _vel_target.z = _vel_z_max; + _limit.vel_up = true; + } + + // reset velocity error and filter if this controller has just been engaged + if (now - _last_update_rate_ms > 100 ) { + // Reset Filter + _vel_error.z = 0; + _vel_target_filt_z = vel_target_z; + desired_accel = 0; + } else { + // calculate rate error and filter with cut off frequency of 2 Hz + //To-Do: adjust constant below based on update rate + _vel_error.z = _vel_error.z + 0.20085f * ((vel_target_z - curr_vel.z) - _vel_error.z); + // feed forward acceleration based on change in the filtered desired speed. + z_target_speed_delta = 0.20085f * (vel_target_z - _vel_target_filt_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 = _pid_alt_rate.kP() * _vel_error.z; + + // consolidate and constrain target acceleration + desired_accel += p; + desired_accel = constrain_int32(desired_accel, -32000, 32000); + + // To-Do: re-enable PID logging? + // TO-DO: ensure throttle cruise is updated some other way in the main code or attitude control + + // set target for accel based throttle controller + accel_to_throttle(desired_accel); +} + +// accel_to_throttle - alt hold's acceleration controller +// 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 + + // Calculate Earth Frame Z acceleration + 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) { + // Reset Filter + _accel_error.z = 0; + } 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); + + // get i term + i = _pid_alt_accel.get_integrator(); + + // update i term as long as we haven't breached the limits or the I term will certainly reduce + // To-Do: should this be replaced with limits check from attitude_controller? + if ((!_motors.limit.throttle_lower && !_motors.limit.throttle_upper) || (i>0&&_accel_error.z<0) || (i<0&&_accel_error.z>0)) { + i = _pid_alt_accel.get_i(_accel_error.z, _dt); + } + + // get d term + d = _pid_alt_accel.get_d(_accel_error.z, _dt); + + // To-Do: pull min/max throttle from motors + // To-Do: where to get hover throttle? + // To-Do: we had a contraint here but it's now removed, is this ok? with the motors library handle it ok? + _attitude_control.set_throttle_out((int16_t)p+i+d+_throttle_hover, true); + + // 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 @@ -183,119 +304,12 @@ get_throttle_rate_stabilized(int16_t target_rate) 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 } - -// get_throttle_rate - calculates desired accel required to achieve desired z_target_speed -// sets accel based throttle controller target -static void -get_throttle_rate(float z_target_speed) -{ - static uint32_t last_call_ms = 0; - static float z_rate_error = 0; // The velocity error in cm. - static float z_target_speed_filt = 0; // The filtered requested speed - float z_target_speed_delta; // The change in requested speed - int32_t p; // used to capture pid values for logging - int32_t output; // the target acceleration if the accel based throttle is enabled, otherwise the output to be sent to the motors - uint32_t now = millis(); - - // reset target altitude if this controller has just been engaged - if( now - last_call_ms > 100 ) { - // Reset Filter - z_rate_error = 0; - z_target_speed_filt = z_target_speed; - output = 0; - } else { - // calculate rate error and filter with cut off frequency of 2 Hz - z_rate_error = z_rate_error + 0.20085f * ((z_target_speed - climb_rate) - z_rate_error); - // feed forward acceleration based on change in the filtered desired speed. - z_target_speed_delta = 0.20085f * (z_target_speed - z_target_speed_filt); - z_target_speed_filt = z_target_speed_filt + z_target_speed_delta; - output = z_target_speed_delta * 50.0f; // To-Do: replace 50 with dt - } - last_call_ms = now; - - // calculate p - p = g.pid_throttle_rate.kP() * z_rate_error; - - // consolidate and constrain target acceleration - output += p; - output = constrain_int32(output, -32000, 32000); - -#if LOGGING_ENABLED == ENABLED - // log output if PID loggins is on and we are tuning the yaw - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_THROTTLE_RATE_KP || g.radio_tuning == CH6_THROTTLE_RATE_KD) ) { - pid_log_counter++; - if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz - pid_log_counter = 0; - Log_Write_PID(CH6_THROTTLE_RATE_KP, z_rate_error, p, 0, 0, output, tuning_value); - } - } -#endif - - // set target for accel based throttle controller - set_throttle_accel_target(output); - - // update throttle cruise - // TO-DO: this may not be correct because g.rc_3.servo_out has not been updated for this iteration - if( z_target_speed == 0 ) { - update_throttle_cruise(g.rc_3.servo_out); - } -} - -/// -/// throttle controller -/// -// get_throttle_accel - accelerometer based throttle controller -// returns an actual throttle output (0 ~ 1000) to be sent to the motors -static int16_t -get_throttle_accel(int16_t z_target_accel) -{ - static float z_accel_error = 0; // The acceleration error in cm. - static uint32_t last_call_ms = 0; // the last time this controller was called - int32_t p,i,d; // used to capture pid values for logging - int16_t output; - float z_accel_meas; - uint32_t now = millis(); - - // Calculate Earth Frame Z acceleration - z_accel_meas = -(ahrs.get_accel_ef().z + GRAVITY_MSS) * 100; - - // reset target altitude if this controller has just been engaged - if( now - last_call_ms > 100 ) { - // Reset Filter - z_accel_error = 0; - } else { - // calculate accel error and Filter with fc = 2 Hz - z_accel_error = z_accel_error + 0.11164f * (constrain_float(z_target_accel - z_accel_meas, -32000, 32000) - z_accel_error); - } - last_call_ms = now; - - // separately calculate p, i, d values for logging - p = g.pid_throttle_accel.get_p(z_accel_error); - - // get i term - i = g.pid_throttle_accel.get_integrator(); - - // replace below with check of throttle limit from attitude_control - // update i term as long as we haven't breached the limits or the I term will certainly reduce - if ((!motors.limit.throttle_lower && !motors.limit.throttle_upper) || (i>0&&z_accel_error<0) || (i<0&&z_accel_error>0)) { - i = g.pid_throttle_accel.get_i(z_accel_error, .01f); - } - - d = g.pid_throttle_accel.get_d(z_accel_error, .01f); - - output = constrain_float(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max); - - // to-do add back in PID logging? - - return output; -} - - +*/ /// /// position controller /// - +/* /// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity void AC_PosControl::get_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target) const { @@ -331,16 +345,16 @@ void AC_PosControl::get_stopping_point(const Vector3f& position, const Vector3f& target.z = position.z; } -/// set_loiter_target in cm from home -void AC_PosControl::set_loiter_target(const Vector3f& position) +/// set_pos_target in cm from home +void AC_PosControl::set_pos_target(const Vector3f& position) { _target = position; _target_vel.x = 0; _target_vel.y = 0; } -/// init_loiter_target - set initial loiter target based on current position and velocity -void AC_PosControl::init_loiter_target(const Vector3f& position, const Vector3f& velocity) +/// init_pos_target - set initial loiter target based on current position and velocity +void AC_PosControl::init_pos_target(const Vector3f& position, const Vector3f& velocity) { // set target position and velocity based on current pos and velocity _target.x = position.x; @@ -361,73 +375,6 @@ void AC_PosControl::init_loiter_target(const Vector3f& position, const Vector3f& _vel_last = _inav->get_velocity(); } -/// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s -void AC_PosControl::move_loiter_target(float control_roll, float control_pitch, float dt) -{ - // convert pilot input to desired velocity in cm/s - _pilot_vel_forward_cms = -control_pitch * _loiter_accel_cms / 4500.0f; - _pilot_vel_right_cms = control_roll * _loiter_accel_cms / 4500.0f; -} - -/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target -void AC_PosControl::translate_loiter_target_movements(float nav_dt) -{ - Vector2f target_vel_adj; - float vel_total; - - // range check nav_dt - if( nav_dt < 0 ) { - return; - } - - // check loiter speed and avoid divide by zero - if( _loiter_speed_cms < 100.0f) { - _loiter_speed_cms = 100.0f; - } - - // rotate pilot input to lat/lon frame - target_vel_adj.x = (_pilot_vel_forward_cms*_cos_yaw - _pilot_vel_right_cms*_sin_yaw); - target_vel_adj.y = (_pilot_vel_forward_cms*_sin_yaw + _pilot_vel_right_cms*_cos_yaw); - - // add desired change in velocity to current target velocit - _target_vel.x += target_vel_adj.x*nav_dt; - _target_vel.y += target_vel_adj.y*nav_dt; - if(_target_vel.x > 0 ) { - _target_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.x/_loiter_speed_cms; - _target_vel.x = max(_target_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0); - }else if(_target_vel.x < 0) { - _target_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.x/_loiter_speed_cms; - _target_vel.x = min(_target_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0); - } - if(_target_vel.y > 0 ) { - _target_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.y/_loiter_speed_cms; - _target_vel.y = max(_target_vel.y - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0); - }else if(_target_vel.y < 0) { - _target_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.y/_loiter_speed_cms; - _target_vel.y = min(_target_vel.y + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0); - } - - // constrain the velocity vector and scale if necessary - vel_total = safe_sqrt(_target_vel.x*_target_vel.x + _target_vel.y*_target_vel.y); - if (vel_total > _loiter_speed_cms && vel_total > 0.0f) { - _target_vel.x = _loiter_speed_cms * _target_vel.x/vel_total; - _target_vel.y = _loiter_speed_cms * _target_vel.y/vel_total; - } - - // update target position - _target.x += _target_vel.x * nav_dt; - _target.y += _target_vel.y * nav_dt; - - // constrain target position to within reasonable distance of current location - Vector3f curr_pos = _inav->get_position(); - Vector3f distance_err = _target - curr_pos; - float distance = safe_sqrt(distance_err.x*distance_err.x + distance_err.y*distance_err.y); - if (distance > _loiter_leash && distance > 0.0f) { - _target.x = curr_pos.x + _loiter_leash * distance_err.x/distance; - _target.y = curr_pos.y + _loiter_leash * distance_err.y/distance; - } -} - /// get_distance_to_target - get horizontal distance to loiter target in cm float AC_PosControl::get_distance_to_target() const { @@ -488,8 +435,8 @@ void AC_PosControl::update_loiter() } } -/// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location -void AC_PosControl::calculate_loiter_leash_length() +/// calculate_leash_length - calculates the maximum distance in cm that the target position may be from the current location +void AC_PosControl::calculate_leash_length() { // get loiter position P float kP = _pid_pos_lat->kP(); @@ -523,231 +470,6 @@ void AC_PosControl::calculate_loiter_leash_length() } } -/// -/// waypoint navigation -/// - -/// set_destination - set destination using cm from home -void AC_PosControl::set_destination(const Vector3f& destination) -{ - // if waypoint controlls is active and copter has reached the previous waypoint use it for the origin - if( _flags.reached_destination && ((hal.scheduler->millis() - _wpnav_last_update) < 1000) ) { - _origin = _destination; - }else{ - // otherwise calculate origin from the current position and velocity - get_stopping_point(_inav->get_position(), _inav->get_velocity(), _origin); - } - - // set origin and destination - set_origin_and_destination(_origin, destination); -} - -/// set_origin_and_destination - set origin and destination using lat/lon coordinates -void AC_PosControl::set_origin_and_destination(const Vector3f& origin, const Vector3f& destination) -{ - // store origin and destination locations - _origin = origin; - _destination = destination; - Vector3f pos_delta = _destination - _origin; - - // calculate leash lengths - bool climb = pos_delta.z >= 0; // climbing vs descending leads to different leash lengths because speed_up_cms and speed_down_cms can be different - - _track_length = pos_delta.length(); // get track length - - // calculate each axis' percentage of the total distance to the destination - if (_track_length == 0.0f) { - // avoid possible divide by zero - _pos_delta_unit.x = 0; - _pos_delta_unit.y = 0; - _pos_delta_unit.z = 0; - }else{ - _pos_delta_unit = pos_delta/_track_length; - } - calculate_wp_leash_length(climb); // update leash lengths - - // initialise intermediate point to the origin - _track_desired = 0; - _target = origin; - _flags.reached_destination = false; - - // initialise the limited speed to current speed along the track - const Vector3f &curr_vel = _inav->get_velocity(); - // get speed along track (note: we convert vertical speed into horizontal speed equivalent) - float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z; - _limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms); - - // default waypoint back to slow - _flags.fast_waypoint = false; - - // initialise desired roll and pitch to current roll and pitch. This avoids a random twitch between now and when the wpnav controller is first run - _desired_roll = constrain_int32(_ahrs->roll_sensor,-_lean_angle_max_cd,_lean_angle_max_cd); - _desired_pitch = constrain_int32(_ahrs->pitch_sensor,-_lean_angle_max_cd,_lean_angle_max_cd); - - // reset target velocity - only used by loiter controller's interpretation of pilot input - _target_vel.x = 0; - _target_vel.y = 0; -} - -/// advance_target_along_track - move target location along track from origin to destination -void AC_PosControl::advance_target_along_track(float dt) -{ - float track_covered; - Vector3f track_error; - float track_desired_max; - float track_desired_temp = _track_desired; - float track_extra_max; - - // get current location - Vector3f curr_pos = _inav->get_position(); - Vector3f curr_delta = curr_pos - _origin; - - // calculate how far along the track we are - track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z; - - Vector3f track_covered_pos = _pos_delta_unit * track_covered; - track_error = curr_delta - track_covered_pos; - - // calculate the horizontal error - float track_error_xy = safe_sqrt(track_error.x*track_error.x + track_error.y*track_error.y); - - // calculate the vertical error - float track_error_z = fabsf(track_error.z); - - // calculate how far along the track we could move the intermediate target before reaching the end of the leash - track_extra_max = min(_track_leash_length*(_wp_leash_z-track_error_z)/_wp_leash_z, _track_leash_length*(_wp_leash_xy-track_error_xy)/_wp_leash_xy); - if(track_extra_max <0) { - track_desired_max = track_covered; - }else{ - track_desired_max = track_covered + track_extra_max; - } - - // get current velocity - const Vector3f &curr_vel = _inav->get_velocity(); - // get speed along track - float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z; - - // calculate point at which velocity switches from linear to sqrt - float linear_velocity = _wp_speed_cms; - float kP = _pid_pos_lat->kP(); - if (kP >= 0.0f) { // avoid divide by zero - linear_velocity = _track_accel/kP; - } - - // let the limited_speed_xy_cms be some range above or below current velocity along track - if (speed_along_track < -linear_velocity) { - // we are travelling fast in the opposite direction of travel to the waypoint so do not move the intermediate point - _limited_speed_xy_cms = 0; - }else{ - // increase intermediate target point's velocity if not yet at target speed (we will limit it below) - if(dt > 0) { - if(track_desired_max > _track_desired) { - _limited_speed_xy_cms += 2.0 * _track_accel * dt; - }else{ - // do nothing, velocity stays constant - _track_desired = track_desired_max; - } - } - // do not go over top speed - if(_limited_speed_xy_cms > _track_speed) { - _limited_speed_xy_cms = _track_speed; - } - // if our current velocity is within the linear velocity range limit the intermediate point's velocity to be no more than the linear_velocity above or below our current velocity - if (fabsf(speed_along_track) < linear_velocity) { - _limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms,speed_along_track-linear_velocity,speed_along_track+linear_velocity); - } - } - // advance the current target - track_desired_temp += _limited_speed_xy_cms * dt; - - // do not let desired point go past the end of the segment - track_desired_temp = constrain_float(track_desired_temp, 0, _track_length); - _track_desired = max(_track_desired, track_desired_temp); - - // recalculate the desired position - _target = _origin + _pos_delta_unit * _track_desired; - - // check if we've reached the waypoint - if( !_flags.reached_destination ) { - if( _track_desired >= _track_length ) { - // "fast" waypoints are complete once the intermediate point reaches the destination - if (_flags.fast_waypoint) { - _flags.reached_destination = true; - }else{ - // regular waypoints also require the copter to be within the waypoint radius - Vector3f dist_to_dest = curr_pos - _destination; - if( dist_to_dest.length() <= _wp_radius_cm ) { - _flags.reached_destination = true; - } - } - } - } -} - -/// get_distance_to_destination - get horizontal distance to destination in cm -float AC_PosControl::get_distance_to_destination() -{ - // get current location - Vector3f curr = _inav->get_position(); - return pythagorous2(_destination.x-curr.x,_destination.y-curr.y); -} - -/// get_bearing_to_destination - get bearing to next waypoint in centi-degrees -int32_t AC_PosControl::get_bearing_to_destination() -{ - return get_bearing_cd(_inav->get_position(), _destination); -} - -/// update_wpnav - run the wp controller - should be called at 10hz -void AC_PosControl::update_wpnav() -{ - // calculate dt - uint32_t now = hal.scheduler->millis(); - float dt = (now - _wpnav_last_update) / 1000.0f; - - // catch if we've just been started - if( dt >= 1.0 ) { - dt = 0.0; - reset_I(); - _wpnav_step = 0; - } - - // reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle - if (dt > 0.095f && _wpnav_step > 3) { - _wpnav_step = 0; - } - - // run loiter steps - switch (_wpnav_step) { - case 0: - // capture time since last iteration - _wpnav_dt = dt; - _wpnav_last_update = now; - - // advance the target if necessary - if (dt > 0.0f) { - advance_target_along_track(dt); - } - _wpnav_step++; - break; - case 1: - // run loiter's position to velocity step - get_loiter_position_to_velocity(_wpnav_dt, _wp_speed_cms); - _wpnav_step++; - break; - case 2: - // run loiter's velocity to acceleration step - get_loiter_velocity_to_acceleration(desired_vel.x, desired_vel.y, _wpnav_dt); - _wpnav_step++; - break; - case 3: - // run loiter's acceleration to lean angle step - get_loiter_acceleration_to_lean_angles(desired_accel.x, desired_accel.y); - _wpnav_step++; - break; - } -} - /// /// shared methods /// @@ -880,78 +602,4 @@ void AC_PosControl::reset_I() // set last velocity to current velocity _vel_last = _inav->get_velocity(); } - -/// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller -void AC_PosControl::calculate_wp_leash_length(bool climb) -{ - - // get loiter position P - float kP = _pid_pos_lat->kP(); - - // sanity check acceleration and avoid divide by zero - if (_wp_accel_cms <= 0.0f) { - _wp_accel_cms = WPNAV_ACCELERATION_MIN; - } - - // avoid divide by zero - if (kP <= 0.0f) { - _wp_leash_xy = WPNAV_MIN_LEASH_LENGTH; - return; - } - // calculate horiztonal leash length - if(_wp_speed_cms <= _wp_accel_cms / kP) { - // linear leash length based on speed close in - _wp_leash_xy = _wp_speed_cms / kP; - }else{ - // leash length grows at sqrt of speed further out - _wp_leash_xy = (_wp_accel_cms / (2.0f*kP*kP)) + (_wp_speed_cms*_wp_speed_cms / (2.0f*_wp_accel_cms)); - } - - // ensure leash is at least 1m long - if( _wp_leash_xy < WPNAV_MIN_LEASH_LENGTH ) { - _wp_leash_xy = WPNAV_MIN_LEASH_LENGTH; - } - - // calculate vertical leash length - float speed_vert; - if( climb ) { - speed_vert = _wp_speed_up_cms; - }else{ - speed_vert = _wp_speed_down_cms; - } - if(speed_vert <= WPNAV_ALT_HOLD_ACCEL_MAX / _althold_kP) { - // linear leash length based on speed close in - _wp_leash_z = speed_vert / _althold_kP; - }else{ - // leash length grows at sqrt of speed further out - _wp_leash_z = (WPNAV_ALT_HOLD_ACCEL_MAX / (2.0*_althold_kP*_althold_kP)) + (speed_vert*speed_vert / (2*WPNAV_ALT_HOLD_ACCEL_MAX)); - } - - // ensure leash is at least 1m long - if( _wp_leash_z < WPNAV_MIN_LEASH_LENGTH ) { - _wp_leash_z = WPNAV_MIN_LEASH_LENGTH; - } - - // length of the unit direction vector in the horizontal - float pos_delta_unit_xy = sqrt(_pos_delta_unit.x*_pos_delta_unit.x+_pos_delta_unit.y*_pos_delta_unit.y); - float pos_delta_unit_z = fabsf(_pos_delta_unit.z); - - // calculate the maximum acceleration, maximum velocity, and leash length in the direction of travel - if(pos_delta_unit_z == 0 && pos_delta_unit_xy == 0){ - _track_accel = 0; - _track_speed = 0; - _track_leash_length = WPNAV_MIN_LEASH_LENGTH; - }else if(_pos_delta_unit.z == 0){ - _track_accel = _wp_accel_cms/pos_delta_unit_xy; - _track_speed = _wp_speed_cms/pos_delta_unit_xy; - _track_leash_length = _wp_leash_xy/pos_delta_unit_xy; - }else if(pos_delta_unit_xy == 0){ - _track_accel = WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z; - _track_speed = speed_vert/pos_delta_unit_z; - _track_leash_length = _wp_leash_z/pos_delta_unit_z; - }else{ - _track_accel = min(WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy); - _track_speed = min(speed_vert/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy); - _track_leash_length = min(_wp_leash_z/pos_delta_unit_z, _wp_leash_xy/pos_delta_unit_xy); - } -} +*/ \ No newline at end of file diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index af10e93015..136daf18fc 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -9,27 +9,28 @@ #include // PID library #include // PID library #include // Inertial Navigation library +#include // Attitude control library +#include // motors library -// loiter maximum velocities and accelerations + +// position controller default definitions +#define POSCONTROL_THROTTLE_HOVER 450.0f // default throttle required to maintain hover +#define POSCONTROL_LEASH_Z 750.0f // leash length for z-axis altitude controller. To-Do: replace this with calculation based on alt_pos.kP()? #define POSCONTROL_ACCELERATION 100.0f // defines the default velocity vs distant curve. maximum acceleration in cm/s/s that position controller asks for from acceleration controller #define POSCONTROL_ACCELERATION_MIN 50.0f // minimum acceleration in cm/s/s - used for sanity checking _wp_accel parameter -#define POSCONTROL_ACCEL_MAX 980.0f // max acceleration in cm/s/s that the loiter 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 +#define POSCONTROL_STOPPING_DIST_Z_MAX 200.0f // max stopping distance vertically // should be 1.5 times larger than POSCONTROL_ACCELERATION. // max acceleration = max lean angle * 980 * pi / 180. i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s #define POSCONTROL_SPEED 500.0f // maximum default loiter speed in cm/s -#define POSCONTROL_SPEED_UP 250.0f // default maximum climb velocity -#define POSCONTROL_SPEED_DOWN 150.0f // default maximum descent velocity -#define POSCONTROL_ACCEL_MAX 250.0f // maximum acceleration in loiter mode -#define POSCONTROL_ACCEL_MIN 25.0f // minimum acceleration in loiter mode +#define POSCONTROL_VEL_Z_MIN -150.0f // default minimum climb velocity (i.e. max descent rate). To-Do: subtract 250 from this? +#define POSCONTROL_VEL_Z_MAX 250.0f // default maximum climb velocity. To-Do: add 250 to this? #define POSCONTROL_SPEED_MAX_TO_CORRECT_ERROR 200.0f // maximum speed used to correct position error (i.e. not including feed forward) -#define POSCONTROL_LEAN_ANGLE_MAX 4500 // default maximum lean angle - -#define POSCONTROL_ALT_HOLD_P 1.0f // default throttle controller's altitude hold's P gain. #define POSCONTROL_ALT_HOLD_ACCEL_MAX 250.0f // hard coded copy of throttle controller's maximum acceleration in cm/s. To-Do: remove duplication with throttle controller definition -#define POSCONTROL_MIN_LEASH_LENGTH 100.0f // minimum leash lengths in cm +#define POSCONTROL_LEASH_LENGTH_MIN 100.0f // minimum leash lengths in cm #define POSCONTROL_DT_10HZ 0.10f // time difference in seconds for 10hz update rate @@ -38,7 +39,8 @@ class AC_PosControl public: /// Constructor - AC_PosControl(const AP_InertialNav& inav, const AP_AHRS& ahrs, const AC_AttitudeControl& attitude_control, + AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, + const AP_Motors& motors, AC_AttitudeControl& attitude_control, APM_PI& pi_alt_pos, AC_PID& pid_alt_rate, AC_PID& pid_alt_accel, APM_PI& pi_pos_lat, APM_PI& pi_pos_lon, AC_PID& pid_rate_lat, AC_PID& pid_rate_lon); @@ -50,6 +52,15 @@ public: void set_dt(float delta_sec) { _dt = delta_sec; } float get_dt() { return _dt; } + /// set_alt_max - sets maximum altitude above home in cm + /// set to zero to disable limit + /// To-Do: update this intermittantly from main code after checking if fence is enabled/disabled + void set_alt_max(float alt) { _alt_max = alt; } + + /// set_vel_z_limits - sets maximum climb and descent rates + /// To-Do: call this in the main code as part of flight mode initialisation + void set_vel_z_limits(float vel_min, float vel_max) { _vel_z_min = vel_min; _vel_z_max = vel_max;} + /// /// z position controller /// @@ -60,9 +71,10 @@ public: /// fly_to_z - fly to altitude in cm above home void fly_to_z(const float alt_cm); - /// climb - climb at rate provided in cm/s - void climb(const float rate_cms); + /// climb_at_rate - climb at rate provided in cm/s + void climb_at_rate(const float rate_target_cms); +/* /// /// xy position controller /// @@ -95,12 +107,9 @@ public: /// get desired roll, pitch which should be fed into stabilize controllers int32_t get_desired_roll() const { return _desired_roll; }; int32_t get_desired_pitch() const { return _desired_pitch; }; - +*/ /// get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller - float get_desired_alt() const { return _target.z; } - - /// set_desired_alt - set desired altitude (in cm above home) - void set_desired_alt(float desired_alt) { _target.z = desired_alt; } + float get_desired_alt() const { return _pos_target.z; } /// set_cos_sin_yaw - short-cut to save on calculations to convert from roll-pitch frame to lat-lon frame void set_cos_sin_yaw(float cos_yaw, float sin_yaw, float cos_pitch) { @@ -109,9 +118,10 @@ public: _cos_pitch = cos_pitch; } - /// set_althold_kP - pass in alt hold controller's P gain - void set_althold_kP(float kP) { if(kP>0.0) _althold_kP = kP; } + // set_throttle_hover - update estimated throttle required to maintain hover + void set_throttle_hover(float throttle) { _throttle_hover = throttle; } +/* /// set_horizontal_velocity - allows main code to pass target horizontal velocity for wp navigation void set_horizontal_velocity(float velocity_cms) { _wp_speed_cms = velocity_cms; }; @@ -129,18 +139,29 @@ public: /// get_waypoint_acceleration - returns acceleration in cm/s/s during missions float get_waypoint_acceleration() const { return _wp_accel_cms.get(); } - - /// set_lean_angle_max - limits maximum lean angle - void set_lean_angle_max(int16_t angle_cd) { if (angle_cd >= 1000 && angle_cd <= 8000) {_lean_angle_max_cd = angle_cd;} } +*/ static const struct AP_Param::GroupInfo var_info[]; -protected: +private: // flags structure struct poscontroller_flags { - uint8_t dummy : 1; // dummy flag - } _flags; + uint8_t pos_up : 1; // 1 if we have hit the vertical position leash limit while going up + uint8_t pos_down : 1; // 1 if we have hit the vertical position leash limit while going down + uint8_t vel_up : 1; // 1 if we have hit the vertical velocity limit going up + uint8_t vel_down : 1; // 1 if we have hit the vertical velocity limit going down + } _limit; + // pos_to_rate_z - position to rate controller for Z axis + void pos_to_rate_z(float alt_cm); + + // rate_to_accel_z - calculates desired accel required to achieve the velocity target + void rate_to_accel_z(float vel_target_z); + + // accel_to_throttle - alt hold's acceleration controller + void accel_to_throttle(float accel_target_z); + + /* /// get_loiter_position_to_velocity - loiter position controller /// converts desired position held in _target vector to desired velocity void get_position_to_velocity(float dt, float max_speed_cms); @@ -161,43 +182,55 @@ protected: /// calculate_leash_length - calculates the maximum distance in cm that the target position may be from the current location void calculate_leash_length(); + */ // references to inertial nav and ahrs libraries - const AP_InertialNav& _inav; const AP_AHRS& _ahrs; - const AC_AttitudeControl& _attitude_control; + const AP_InertialNav& _inav; + const AP_Motors& _motors; + AC_AttitudeControl& _attitude_control; - // references to pid controllers + // references to pid controllers and motors APM_PI& _pi_alt_pos; AC_PID& _pid_alt_rate; AC_PID& _pid_alt_accel; - APM_PI& _pid_pos_lat; - APM_PI& _pid_pos_lon; + APM_PI& _pi_pos_lat; + APM_PI& _pi_pos_lon; AC_PID& _pid_rate_lat; AC_PID& _pid_rate_lon; // parameters - AP_Float _speed_cms; // maximum horizontal speed in cm/s while in loiter - AP_Float _speed_up_cms; // climb speed target in cm/s - AP_Float _speed_down_cms; // descent speed target in cm/s - uint8_t _step; // used to decide which portion of loiter controller to run during this iteration - uint32_t _last_update; // system time of last update_position_controller call - float _dt; // time difference since last update_position_controller call - float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame + AP_Float _throttle_hover; // estimated throttle required to maintain a level hover + + // 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) + uint8_t _step; // used to decide which portion of position controller to run during this iteration + float _speed_cms; // max horizontal speed in cm/s + float _vel_z_min; // min climb rate (i.e. max descent rate) in cm/s + float _vel_z_max; // max climb rate in cm/s + float _accel_cms; // max horizontal acceleration in cm/s/s + 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; // output from controller - int32_t _desired_roll; // fed to stabilize controllers at 50hz - int32_t _desired_pitch; // fed to stabilize controllers at 50hz + float _desired_roll; // desired roll angle in centi-degrees calculated by position controller + float _desired_pitch; // desired roll pitch in centi-degrees calculated by position controller // position controller internal variables - Vector3f _target; // loiter's target location in cm from home - Vector3f _target_vel; // pilot's latest desired velocity in earth-frame - Vector3f _vel_last; // previous iterations velocity in cm/s - float _leash; // horizontal leash length in cm. used to stop the pilot from pushing the target location too far from the current location - float _accel_cms; // maximum acceleration in cm/s/s - int16_t _lean_angle_max_cd; // maximum lean angle in centi-degrees + Vector3f _pos_target; // target location in cm from home + Vector3f _pos_error; // error between desired and actual position in cm + Vector3f _vel_target; // desired velocity in cm/s + Vector3f _vel_error; // error between desired and actual acceleration in cm/s. To-Do: x & y actually used? + Vector3f _vel_last; // previous iterations velocity in cm/s + float _vel_target_filt_z; // filtered target vertical velocity + Vector3f _accel_target; // desired acceleration in cm/s/s // To-Do: are xy actually required? + Vector3f _accel_error; // desired acceleration in cm/s/s // To-Do: are xy actually required? + float _leash; // horizontal leash length in cm. used to stop the pilot from pushing the target location too far from the current location + float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence public: // for logging purposes