/// advance_spline_target_along_track - move target location along track from origin to destination void AC_WPNav::advance_spline_target_along_track(float dt) { if (!_flags.reached_destination) { Vector3f target_pos, target_vel; // update target position and velocity from spline calculator calc_spline_pos_vel(_spline_time, target_pos, target_vel); // update velocity float spline_dist_to_wp = (_destination - target_pos).length(); // if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) { _spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cms); } else if(_spline_vel_scaler < _wp_speed_cms) { // increase velocity using acceleration // To-Do: replace 0.1f below with update frequency passed in from main program _spline_vel_scaler += _wp_accel_cms* 0.1f; } // constrain target velocity if (_spline_vel_scaler > _wp_speed_cms) { _spline_vel_scaler = _wp_speed_cms; } // scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator float target_vel_length = target_vel.length(); if (target_vel_length != 0.0f) { _spline_time_scale = _spline_vel_scaler/target_vel_length; } // update target position _pos_control.set_pos_target(target_pos); // update the yaw _yaw = RadiansToCentiDegrees(fast_atan2(target_vel.y,target_vel.x)); // advance spline time to next step _spline_time += _spline_time_scale*dt; // we will reach the next waypoint in the next step so set reached_destination flag // To-Do: is this one step too early? if (_spline_time >= 1.0f) { _flags.reached_destination = true; } } }
/// advance_spline_target_along_track - move target location along track from origin to destination bool AC_WPNav::advance_spline_target_along_track(float dt) { if (!_flags.reached_destination) { Vector3f target_pos, target_vel; // update target position and velocity from spline calculator calc_spline_pos_vel(_spline_time, target_pos, target_vel); _pos_delta_unit = target_vel/target_vel.length(); calculate_wp_leash_length(); // get current location Vector3f curr_pos = _inav.get_position(); // get terrain altitude offset for origin and current position (i.e. change in terrain altitude from a position vs ekf origin) float terr_offset = 0.0f; if (_terrain_alt && !get_terrain_offset(terr_offset)) { return false; } // calculate position error Vector3f track_error = curr_pos - target_pos; track_error.z -= terr_offset; // calculate the horizontal error float track_error_xy = pythagorous2(track_error.x, track_error.y); // calculate the vertical error float track_error_z = fabsf(track_error.z); // get position control leash lengths float leash_xy = _pos_control.get_leash_xy(); float leash_z; if (track_error.z >= 0) { leash_z = _pos_control.get_leash_up_z(); }else{ leash_z = _pos_control.get_leash_down_z(); } // calculate how far along the track we could move the intermediate target before reaching the end of the leash float track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy); if (track_leash_slack < 0.0f) { track_leash_slack = 0.0f; } // update velocity float spline_dist_to_wp = (_destination - target_pos).length(); float vel_limit = _wp_speed_cms; if (!is_zero(dt)) { vel_limit = MIN(vel_limit, track_leash_slack/dt); } // if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) { _spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cms); }else if(_spline_vel_scaler < vel_limit) { // increase velocity using acceleration _spline_vel_scaler += _wp_accel_cms * dt; } // constrain target velocity _spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit); // scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator float target_vel_length = target_vel.length(); if (!is_zero(target_vel_length)) { _spline_time_scale = _spline_vel_scaler/target_vel_length; } // update target position target_pos.z += terr_offset; _pos_control.set_pos_target(target_pos); // update the yaw _yaw = RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x)); // advance spline time to next step _spline_time += _spline_time_scale*dt; // we will reach the next waypoint in the next step so set reached_destination flag // To-Do: is this one step too early? if (_spline_time >= 1.0f) { _flags.reached_destination = true; } } return true; }
/// advance_spline_target_along_track - move target location along track from origin to destination bool AC_WPNav::advance_spline_target_along_track(float dt) { if (!_flags.reached_destination) { Vector3f target_pos, target_vel; // update target position and velocity from spline calculator calc_spline_pos_vel(_spline_time, target_pos, target_vel); // if target velocity is zero the origin and destination must be the same // so flag reached destination (and protect against divide by zero) float target_vel_length = target_vel.length(); if (is_zero(target_vel_length)) { _flags.reached_destination = true; return true; } _pos_delta_unit = target_vel / target_vel_length; calculate_wp_leash_length(); // get current location Vector3f curr_pos = _inav.get_position(); // get terrain altitude offset for origin and current position (i.e. change in terrain altitude from a position vs ekf origin) float terr_offset = 0.0f; if (_terrain_alt && !get_terrain_offset(terr_offset)) { return false; } // calculate position error Vector3f track_error = curr_pos - target_pos; track_error.z -= terr_offset; // calculate the horizontal error _track_error_xy = norm(track_error.x, track_error.y); // calculate the vertical error float track_error_z = fabsf(track_error.z); // get position control leash lengths float leash_xy = _pos_control.get_leash_xy(); float leash_z; if (track_error.z >= 0) { leash_z = _pos_control.get_leash_up_z(); }else{ leash_z = _pos_control.get_leash_down_z(); } // calculate how far along the track we could move the intermediate target before reaching the end of the leash float track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-_track_error_xy)/leash_xy); if (track_leash_slack < 0.0f) { track_leash_slack = 0.0f; } // update velocity float spline_dist_to_wp = (_destination - target_pos).length(); float vel_limit = _wp_speed_cms; if (!is_zero(dt)) { vel_limit = MIN(vel_limit, track_leash_slack/dt); } // if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) { _spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cmss); }else if(_spline_vel_scaler < vel_limit) { // increase velocity using acceleration _spline_vel_scaler += _wp_accel_cmss * dt; } // constrain target velocity _spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit); // scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator _spline_time_scale = _spline_vel_scaler / target_vel_length; // update target position target_pos.z += terr_offset; _pos_control.set_pos_target(target_pos); // update the target yaw if origin and destination are at least 2m apart horizontally if (_track_length_xy >= WPNAV_YAW_DIST_MIN) { if (_pos_control.get_leash_xy() < WPNAV_YAW_DIST_MIN) { // if the leash is very short (i.e. flying at low speed) use the target point's velocity along the track if (!is_zero(target_vel.x) && !is_zero(target_vel.y)) { set_yaw_cd(RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x))); } } else { // point vehicle along the leash (i.e. point vehicle towards target point on the segment from origin to destination) float track_error_xy_length = safe_sqrt(sq(track_error.x)+sq(track_error.y)); if (track_error_xy_length > MIN(WPNAV_YAW_DIST_MIN, _pos_control.get_leash_xy()*WPNAV_YAW_LEASH_PCT_MIN)) { // To-Do: why is track_error sign reversed? set_yaw_cd(RadiansToCentiDegrees(atan2f(-track_error.y,-track_error.x))); } } } // advance spline time to next step _spline_time += _spline_time_scale*dt; // we will reach the next waypoint in the next step so set reached_destination flag // To-Do: is this one step too early? if (_spline_time >= 1.0f) { _flags.reached_destination = true; } } return true; }