/// update_wpnav - run the wp controller - should be called at 10hz void AC_WPNav::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.0f ) { dt = 0.0; reset_I(); _wpnav_step = 0; } else if (_wpnav_reset > 0) { _wpnav_step = 0; _wpnav_reset = 0; _accel_reset = 1; } // 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; } }
/// get_loiter_position_to_velocity - loiter position controller /// converts desired position held in _target vector to desired velocity void AC_WPNav::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; } // call velocity to acceleration controller get_loiter_velocity_to_acceleration(desired_vel.x, desired_vel.y, dt); }
/// update_loiter - run the loiter controller - should be called at 10hz void AC_WPNav::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.0f)||loiter_reset) { // ST-JD : add "or loiter_reset" dt = 0.0f; loiter_reset=false; // ST-JD 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; } }