diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp new file mode 100644 index 0000000000..3b9773488d --- /dev/null +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -0,0 +1,957 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include +#include + +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 + // @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), + + AP_GROUPEND +}; + +// Default constructor. +// 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), + _ahrs(ahrs), + _attitude_control(attitude_control), + _pi_alt_hold(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), + _pid_rate_lat(pid_rate_lat), + _pid_rate_lon(pid_rate_lon), + _last_update(0), + _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) +{ + AP_Param::setup_object_defaults(this, var_info); + + // calculate loiter leash + 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) +{ + 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. + + linear_velocity = ALT_HOLD_ACCEL_MAX/g.pi_alt_hold.kP(); + + if (abs(climb_rate_cms) < linear_velocity) { + target_alt = alt_cm + climb_rate_cms/g.pi_alt_hold.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); + } else { + target_alt = alt_cm - ( linear_distance + (int32_t)climb_rate_cms*(int32_t)climb_rate_cms/(2*ALT_HOLD_ACCEL_MAX) ); + } + } + return constrain_int32(target_alt, alt_cm - ALT_HOLD_INIT_MAX_OVERSHOOT, alt_cm + ALT_HOLD_INIT_MAX_OVERSHOOT); +} + +// 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) +{ + 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. + + // calculate altitude error + alt_error = target_alt - current_loc.alt; + + // 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)); + }else{ + desired_rate = g.pi_alt_hold.get_p(alt_error); + } + }else{ + desired_rate = 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 +} + +// 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 +} + +// 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) +{ + // 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; + } + + // 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; + } + } +#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 +} + +// 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 +{ + 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. + float vel_total; + float target_dist; + float kP = _pid_pos_lat->kP(); + + // calculate current velocity + vel_total = safe_sqrt(velocity.x*velocity.x + velocity.y*velocity.y); + + // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero + if (vel_total < 10.0f || kP <= 0.0f || _wp_accel_cms <= 0.0f) { + target = position; + return; + } + + // calculate point at which velocity switches from linear to sqrt + linear_velocity = _wp_accel_cms/kP; + + // calculate distance within which we can stop + if (vel_total < linear_velocity) { + target_dist = vel_total/kP; + } else { + linear_distance = _wp_accel_cms/(2.0f*kP*kP); + target_dist = linear_distance + (vel_total*vel_total)/(2.0f*_wp_accel_cms); + } + target_dist = constrain_float(target_dist, 0, _wp_leash_xy*2.0f); + + target.x = position.x + (target_dist * velocity.x / vel_total); + target.y = position.y + (target_dist * velocity.y / vel_total); + target.z = position.z; +} + +/// set_loiter_target in cm from home +void AC_PosControl::set_loiter_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) +{ + // set target position and velocity based on current pos and velocity + _target.x = position.x; + _target.y = position.y; + _target_vel.x = velocity.x; + _target_vel.y = velocity.y; + + // initialise desired roll and pitch to current roll and pitch. This avoids a random twitch between now and when the loiter 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); + + // initialise pilot input + _pilot_vel_forward_cms = 0; + _pilot_vel_right_cms = 0; + + // set last velocity to current velocity + // To-Do: remove the line below by instead forcing reset_I to be called on the first loiter_update call + _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 +{ + return _distance_to_target; +} + +/// get_bearing_to_target - get bearing to loiter target in centi-degrees +int32_t AC_PosControl::get_bearing_to_target() const +{ + return get_bearing_cd(_inav->get_position(), _target); +} + +/// update_loiter - run the loiter controller - should be called at 10hz +void AC_PosControl::update_loiter() +{ + // calculate dt + uint32_t now = hal.scheduler->millis(); + float dt = (now - _loiter_last_update) / 1000.0f; + + // catch if we've just been started + if( dt >= 1.0 ) { + dt = 0.0; + reset_I(); + _loiter_step = 0; + } + + // reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle + if (dt > 0.095f && _loiter_step > 3) { + _loiter_step = 0; + } + + // run loiter steps + switch (_loiter_step) { + case 0: + // capture time since last iteration + _loiter_dt = dt; + _loiter_last_update = now; + + // translate any adjustments from pilot to loiter target + translate_loiter_target_movements(_loiter_dt); + _loiter_step++; + break; + case 1: + // run loiter's position to velocity step + get_loiter_position_to_velocity(_loiter_dt, WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR); + _loiter_step++; + break; + case 2: + // run loiter's velocity to acceleration step + get_loiter_velocity_to_acceleration(desired_vel.x, desired_vel.y, _loiter_dt); + _loiter_step++; + break; + case 3: + // run loiter's acceleration to lean angle step + get_loiter_acceleration_to_lean_angles(desired_accel.x, desired_accel.y); + _loiter_step++; + break; + } +} + +/// 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() +{ + // get loiter position P + float kP = _pid_pos_lat->kP(); + + // check loiter speed + if( _loiter_speed_cms < 100.0f) { + _loiter_speed_cms = 100.0f; + } + + // set loiter acceleration to 1/2 loiter speed + _loiter_accel_cms = _loiter_speed_cms / 2.0f; + + // avoid divide by zero + if (kP <= 0.0f || _wp_accel_cms <= 0.0f) { + _loiter_leash = WPNAV_MIN_LEASH_LENGTH; + return; + } + + // calculate horizontal leash length + if(WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR <= _wp_accel_cms / kP) { + // linear leash length based on speed close in + _loiter_leash = WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR / kP; + }else{ + // leash length grows at sqrt of speed further out + _loiter_leash = (_wp_accel_cms / (2.0f*kP*kP)) + (WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR*WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR / (2.0f*_wp_accel_cms)); + } + + // ensure leash is at least 1m long + if( _loiter_leash < WPNAV_MIN_LEASH_LENGTH ) { + _loiter_leash = WPNAV_MIN_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 +/// + +/// get_loiter_position_to_velocity - loiter position controller +/// converts desired position held in _target vector to desired velocity +void AC_PosControl::get_loiter_position_to_velocity(float dt, float max_speed_cms) +{ + Vector3f curr = _inav->get_position(); + float dist_error_total; + + float vel_sqrt; + float vel_total; + + float linear_distance; // the distace we swap between linear and sqrt. + float kP = _pid_pos_lat->kP(); + + // avoid divide by zero + if (kP <= 0.0f) { + desired_vel.x = 0.0; + desired_vel.y = 0.0; + }else{ + // calculate distance error + dist_error.x = _target.x - curr.x; + dist_error.y = _target.y - curr.y; + + linear_distance = _wp_accel_cms/(2.0f*kP*kP); + + dist_error_total = safe_sqrt(dist_error.x*dist_error.x + dist_error.y*dist_error.y); + _distance_to_target = dist_error_total; // for reporting purposes + + if( dist_error_total > 2.0f*linear_distance ) { + vel_sqrt = safe_sqrt(2.0f*_wp_accel_cms*(dist_error_total-linear_distance)); + desired_vel.x = vel_sqrt * dist_error.x/dist_error_total; + desired_vel.y = vel_sqrt * dist_error.y/dist_error_total; + }else{ + desired_vel.x = _pid_pos_lat->kP() * dist_error.x; + desired_vel.y = _pid_pos_lon->kP() * dist_error.y; + } + + // ensure velocity stays within limits + vel_total = safe_sqrt(desired_vel.x*desired_vel.x + desired_vel.y*desired_vel.y); + if( vel_total > max_speed_cms ) { + desired_vel.x = max_speed_cms * desired_vel.x/vel_total; + desired_vel.y = max_speed_cms * desired_vel.y/vel_total; + } + + // feed forward velocity request + desired_vel.x += _target_vel.x; + desired_vel.y += _target_vel.y; + } +} + +/// get_loiter_velocity_to_acceleration - loiter velocity controller +/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame +void AC_PosControl::get_loiter_velocity_to_acceleration(float vel_lat, float vel_lon, float dt) +{ + const Vector3f &vel_curr = _inav->get_velocity(); // current velocity in cm/s + Vector3f vel_error; // The velocity error in cm/s. + float accel_total; // total acceleration in cm/s/s + + // reset last velocity if this controller has just been engaged or dt is zero + if( dt == 0.0 ) { + desired_accel.x = 0; + desired_accel.y = 0; + } else { + // feed forward desired acceleration calculation + desired_accel.x = (vel_lat - _vel_last.x)/dt; + desired_accel.y = (vel_lon - _vel_last.y)/dt; + } + + // store this iteration's velocities for the next iteration + _vel_last.x = vel_lat; + _vel_last.y = vel_lon; + + // calculate velocity error + vel_error.x = vel_lat - vel_curr.x; + vel_error.y = vel_lon - vel_curr.y; + + // combine feed foward accel with PID outpu from velocity error + desired_accel.x += _pid_rate_lat->get_pid(vel_error.x, dt); + desired_accel.y += _pid_rate_lon->get_pid(vel_error.y, dt); + + // scale desired acceleration if it's beyond acceptable limit + accel_total = safe_sqrt(desired_accel.x*desired_accel.x + desired_accel.y*desired_accel.y); + if( accel_total > WPNAV_ACCEL_MAX ) { + desired_accel.x = WPNAV_ACCEL_MAX * desired_accel.x/accel_total; + desired_accel.y = WPNAV_ACCEL_MAX * desired_accel.y/accel_total; + } +} + +/// get_loiter_acceleration_to_lean_angles - loiter acceleration controller +/// converts desired accelerations provided in lat/lon frame to roll/pitch angles +void AC_PosControl::get_loiter_acceleration_to_lean_angles(float accel_lat, float accel_lon) +{ + float z_accel_meas = -GRAVITY_MSS * 100; // gravity in cm/s/s + float accel_forward; + float accel_right; + + // To-Do: add 1hz filter to accel_lat, accel_lon + + // rotate accelerations into body forward-right frame + accel_forward = accel_lat*_cos_yaw + accel_lon*_sin_yaw; + accel_right = -accel_lat*_sin_yaw + accel_lon*_cos_yaw; + + // update angle targets that will be passed to stabilize controller + _desired_roll = constrain_float(fast_atan(accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max_cd, _lean_angle_max_cd); + _desired_pitch = constrain_float(fast_atan(-accel_forward/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max_cd, _lean_angle_max_cd); +} + +// get_bearing_cd - return bearing in centi-degrees between two positions +// To-Do: move this to math library +float AC_PosControl::get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const +{ + float bearing = 9000 + atan2f(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f; + if (bearing < 0) { + bearing += 36000; + } + return bearing; +} + +/// reset_I - clears I terms from loiter PID controller +void AC_PosControl::reset_I() +{ + _pid_pos_lon->reset_I(); + _pid_pos_lat->reset_I(); + _pid_rate_lon->reset_I(); + _pid_rate_lat->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); + } +} diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h new file mode 100644 index 0000000000..af10e93015 --- /dev/null +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -0,0 +1,208 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#ifndef AC_POSCONTROL_H +#define AC_POSCONTROL_H + +#include +#include +#include +#include +#include // PID library +#include // PID library +#include // Inertial Navigation library + +// loiter maximum velocities and accelerations +#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. + // 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_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_DT_10HZ 0.10f // time difference in seconds for 10hz update rate + +class AC_PosControl +{ +public: + + /// Constructor + 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); + + /// + /// initialisation functions + /// + + /// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025) + void set_dt(float delta_sec) { _dt = delta_sec; } + float get_dt() { return _dt; } + + /// + /// z position controller + /// + + /// get_stopping_point_z - returns reasonable stopping altitude in cm above home + float get_stopping_point_z(); + + /// 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); + + /// + /// xy position controller + /// + + /// get_pos_target - get target as position vector (from home in cm) + const Vector3f &get_pos_target() const { return _target; } + + /// set_pos_target in cm from home + void set_pos_target(const Vector3f& position); + + /// init_pos_target - set initial loiter target based on current position and velocity + void init_pos_target(const Vector3f& position, const Vector3f& velocity); + + /// get_distance_to_target - get horizontal distance to loiter target in cm + float get_distance_to_target() const; + + /// get_bearing_to_target - get bearing to loiter target in centi-degrees + int32_t get_bearing_to_target() const; + + /// update_loiter - run the loiter controller - should be called at 10hz + void update_pos_controller(); + + /// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity + void get_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target) const; + + /// + /// shared methods + /// + + /// 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; } + + /// 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) { + _cos_yaw = cos_yaw; + _sin_yaw = sin_yaw; + _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_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; }; + + /// get_horizontal_velocity - allows main code to retrieve target horizontal velocity for wp navigation + float get_horizontal_velocity() { return _wp_speed_cms; }; + + /// get_climb_velocity - returns target climb speed in cm/s during missions + float get_climb_velocity() const { return _wp_speed_up_cms; }; + + /// get_descent_velocity - returns target descent speed in cm/s during missions. Note: always positive + float get_descent_velocity() const { return _wp_speed_down_cms; }; + + /// get_waypoint_radius - access for waypoint radius in cm + float get_waypoint_radius() const { return _wp_radius_cm; } + + /// 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: + // flags structure + struct poscontroller_flags { + uint8_t dummy : 1; // dummy flag + } _flags; + + /// 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); + + /// get_loiter_velocity_to_acceleration - loiter velocity controller + /// converts desired velocities in lat/lon directions to accelerations in lat/lon frame + void get_velocity_to_acceleration(float vel_lat_cms, float vel_lon_cms, float dt); + + /// get_loiter_acceleration_to_lean_angles - loiter acceleration controller + /// converts desired accelerations provided in lat/lon frame to roll/pitch angles + void get_acceleration_to_lean_angles(float accel_lat_cmss, float accel_lon_cmss); + + /// get_bearing_cd - return bearing in centi-degrees between two positions + float get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const; + + /// reset_I - clears I terms from position PID controller + void reset_I(); + + /// 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; + + // references to pid controllers + 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; + 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 + 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 + + // 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 + +public: + // for logging purposes + Vector2f dist_error; // distance error calculated by loiter controller + Vector2f desired_vel; // loiter controller desired velocity + Vector2f desired_accel; // the resulting desired acceleration +}; +#endif // AC_POSCONTROL_H