static void test_one_offset(const struct Location &loc,
float ofs_north, float ofs_east,
float dist, float bearing)
{
struct Location loc2;
float dist2, bearing2;
loc2 = loc;
uint32_t t1 = AP_HAL::micros();
location_offset(loc2, ofs_north, ofs_east);
hal.console->printf("location_offset took %u usec\n",
(unsigned)(AP_HAL::micros() - t1));
dist2 = get_distance(loc, loc2);
bearing2 = get_bearing_cd(loc, loc2) * 0.01f;
float brg_error = bearing2-bearing;
if (brg_error > 180) {
brg_error -= 360;
} else if (brg_error < -180) {
brg_error += 360;
}
if (fabsf(dist - dist2) > 1.0f ||
brg_error > 1.0f) {
hal.console->printf("Failed offset test brg_error=%f dist_error=%f\n",
brg_error, dist-dist2);
}
}
/*
start a VTOL landing
*/
bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
{
if (!setup()) {
return false;
}
motors->slow_start(true);
pid_rate_roll.reset_I();
pid_rate_pitch.reset_I();
pid_rate_yaw.reset_I();
pid_accel_z.reset_I();
pi_vel_xy.reset_I();
plane.set_next_WP(cmd.content.location);
// initially aim for current altitude
plane.next_WP_loc.alt = plane.current_loc.alt;
land_state = QLAND_POSITION;
throttle_wait = false;
land_yaw_cd = get_bearing_cd(plane.prev_WP_loc, plane.next_WP_loc);
land_wp_proportion = 0;
motors_lower_limit_start_ms = 0;
Location origin = inertial_nav.get_origin();
Vector2f diff2d;
Vector3f target;
diff2d = location_diff(origin, plane.next_WP_loc);
target.x = diff2d.x * 100;
target.y = diff2d.y * 100;
target.z = plane.next_WP_loc.alt - origin.alt;
wp_nav->set_wp_origin_and_destination(inertial_nav.get_position(), target);
pos_control->set_alt_target(inertial_nav.get_altitude());
// also update nav_controller for status output
plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
return true;
}
// set desired location
void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{
// record targets
_origin = rover.current_loc;
_destination = destination;
// initialise distance
_distance_to_destination = get_distance(_origin, _destination);
_reached_destination = false;
// set final desired speed
_desired_speed_final = 0.0f;
if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {
// if not turning can continue at full speed
if (is_zero(next_leg_bearing_cd)) {
_desired_speed_final = _desired_speed;
} else {
// calculate maximum speed that keeps overshoot within bounds
const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination);
const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
_desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));
}
}
}
bool Plane::verify_continue_and_change_alt()
{
//climbing?
if (condition_value == 1 && adjusted_altitude_cm() >= next_WP_loc.alt) {
return true;
}
//descending?
else if (condition_value == 2 &&
adjusted_altitude_cm() <= next_WP_loc.alt) {
return true;
}
//don't care if we're climbing or descending
else if (abs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) {
return true;
}
// Is the next_WP less than 200 m away?
if (get_distance(current_loc, next_WP_loc) < 200.0f) {
//push another 300 m down the line
int32_t next_wp_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
location_update(next_WP_loc, next_wp_bearing_cd * 0.01f, 300.0f);
}
//keep flying the same course
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
return false;
}
/**
update_bearing_and_distance - updates bearing and distance to the vehicle
should be called at 50hz
*/
void Tracker::update_bearing_and_distance()
{
// exit immediately if we do not have a valid vehicle position
if (!vehicle.location_valid) {
return;
}
// calculate bearing to vehicle
// To-Do: remove need for check of control_mode
if (control_mode != SCAN && !nav_status.manual_control_yaw) {
nav_status.bearing = get_bearing_cd(current_loc, vehicle.location_estimate) * 0.01f;
}
// calculate distance to vehicle
nav_status.distance = get_distance(current_loc, vehicle.location_estimate);
// calculate altitude difference to vehicle using gps
if (g.alt_source == ALT_SOURCE_GPS) {
nav_status.alt_difference_gps = (vehicle.location_estimate.alt - current_loc.alt) / 100.0f;
} else {
// g.alt_source == ALT_SOURCE_GPS_VEH_ONLY
nav_status.alt_difference_gps = vehicle.relative_alt / 100.0f;
}
// calculate pitch to vehicle
// To-Do: remove need for check of control_mode
if (control_mode != SCAN && !nav_status.manual_control_pitch) {
if (g.alt_source == ALT_SOURCE_BARO) {
nav_status.pitch = degrees(atan2f(nav_status.alt_difference_baro, nav_status.distance));
} else {
nav_status.pitch = degrees(atan2f(nav_status.alt_difference_gps, nav_status.distance));
}
}
}
void ModeLoiter::update()
{
// get distance (in meters) to destination
_distance_to_destination = get_distance(rover.current_loc, _destination);
// if within waypoint radius slew desired speed towards zero and use existing desired heading
if (_distance_to_destination <= g.waypoint_radius) {
_desired_speed = attitude_control.get_desired_speed_accel_limited(0.0f, rover.G_Dt);
_yaw_error_cd = 0.0f;
} else {
// P controller with hard-coded gain to convert distance to desired speed
// To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
_desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);
// calculate bearing to destination
_desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
// if destination is behind vehicle, reverse towards it
if (fabsf(_yaw_error_cd) > 9000 && g2.loit_type == 0) {
_desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
_desired_speed = -_desired_speed;
}
// reduce desired speed if yaw_error is large
// 45deg of error reduces speed to 75%, 90deg of error reduces speed to 50%
float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * 0.5f;
_desired_speed *= yaw_error_ratio;
}
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd);
calc_throttle(_desired_speed, false, true);
}
// set desired location
void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{
// set origin to last destination if waypoint controller active
if ((AP_HAL::millis() - last_steer_to_wp_ms < 100) && _reached_destination) {
_origin = _destination;
} else {
// otherwise use reasonable stopping point
calc_stopping_location(_origin);
}
_destination = destination;
// initialise distance
_distance_to_destination = get_distance(_origin, _destination);
_reached_destination = false;
// set final desired speed
_desired_speed_final = 0.0f;
if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {
const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination);
const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
if (is_zero(turn_angle_cd)) {
// if not turning can continue at full speed
_desired_speed_final = _desired_speed;
} else if (rover.use_pivot_steering_at_next_WP(turn_angle_cd)) {
// pivoting so we will stop
_desired_speed_final = 0.0f;
} else {
// calculate maximum speed that keeps overshoot within bounds
const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
_desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));
}
}
}
// currently identical to the slope aborts
void AP_Landing_Deepstall::verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed)
{
// when aborting a landing, mimic the verify_takeoff with steering hold. Once
// the altitude has been reached, restart the landing sequence
throttle_suppressed = false;
landing.nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));
}
/// get the ground course of the next navigation leg in centidegrees
/// from 0 36000. Return default_angle if next navigation
/// leg cannot be determined
int32_t AP_Mission::get_next_ground_course_cd(int32_t default_angle)
{
Mission_Command cmd;
if (!get_next_nav_cmd(_nav_cmd.index+1, cmd)) {
return default_angle;
}
return get_bearing_cd(_nav_cmd.content.location, cmd.content.location);
}
/*
verify a LOITER_TO_ALT command. This involves checking we have
reached both the desired altitude and desired heading. The desired
altitude only needs to be reached once.
*/
bool Plane::verify_loiter_to_alt()
{
update_loiter();
//has target altitude been reached?
if (condition_value != 0) {
if (labs(condition_value - current_loc.alt) < 500) {
//Only have to reach the altitude once -- that's why I need
//this global condition variable.
//
//This is in case of altitude oscillation when still trying
//to reach the target heading.
condition_value = 0;
} else {
return false;
}
}
//has target heading been reached?
if (condition_value2 != 0) {
//Get the lat/lon of next Nav waypoint after this one:
AP_Mission::Mission_Command next_nav_cmd;
if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,
next_nav_cmd)) {
//no next waypoint to shoot for -- go ahead and break out of loiter
return true;
}
// Bearing in radians
int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location);
// get current heading. We should probably be using the ground
// course instead to improve the accuracy in wind
int32_t heading_cd = ahrs.yaw_sensor;
int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
/*
Check to see if the the plane is heading toward the land
waypoint
We use 10 degrees of slop so that we can handle 100
degrees/second of yaw
*/
if (abs(heading_err_cd) <= 1000) {
//Want to head in a straight line from _here_ to the next waypoint.
//DON'T want to head in a line from the center of the loiter to
//the next waypoint.
//Therefore: mark the "last" (next_wp_loc is about to be updated)
//wp lat/lon as the current location.
next_WP_loc = current_loc;
return true;
} else {
return false;
}
}
return true;
}
bool Plane::verify_loiter_heading(bool init)
{
if (quadplane.in_vtol_auto()) {
// skip heading verify if in VTOL auto
return true;
}
//Get the lat/lon of next Nav waypoint after this one:
AP_Mission::Mission_Command next_nav_cmd;
if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,
next_nav_cmd)) {
//no next waypoint to shoot for -- go ahead and break out of loiter
return true;
}
if (get_distance(next_WP_loc, next_nav_cmd.content.location) < abs(aparm.loiter_radius)) {
/* Whenever next waypoint is within the loiter radius,
maintaining loiter would prevent us from ever pointing toward the next waypoint.
Hence break out of loiter immediately
*/
return true;
}
// Bearing in degrees
int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location);
// get current heading.
int32_t heading_cd = gps.ground_course_cd();
int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
if (init) {
loiter.total_cd = wrap_360_cd(bearing_cd - heading_cd);
loiter.sum_cd = 0;
}
/*
Check to see if the the plane is heading toward the land
waypoint. We use 20 degrees (+/-10 deg) of margin so that
we can handle 200 degrees/second of yaw. Allow turn count
to stop it too to ensure we don't loop around forever in
case high winds are forcing us beyond 200 deg/sec at this
particular moment.
*/
if (labs(heading_err_cd) <= 1000 ||
loiter.sum_cd > loiter.total_cd) {
// Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp
// 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
if (next_WP_loc.flags.loiter_xtrack) {
next_WP_loc = current_loc;
}
return true;
}
return false;
}
/// set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
/// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if these are alt-above-ekf-origin)
/// returns false on failure (likely caused by missing terrain data)
bool AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt)
{
// store origin and destination locations
_origin = origin;
_destination = destination;
_terrain_alt = terrain_alt;
Vector3f pos_delta = _destination - _origin;
_track_length = pos_delta.length(); // get track length
// calculate each axis' percentage of the total distance to the destination
if (is_zero(_track_length)) {
// 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 leash lengths
calculate_wp_leash_length();
// initialise yaw heading
if (_track_length >= WPNAV_YAW_DIST_MIN) {
_yaw = get_bearing_cd(_origin, _destination);
} else {
// set target yaw to current heading target
_yaw = _attitude_control.get_att_target_euler_cd().z;
}
// get origin's alt-above-terrain
float origin_terr_offset = 0.0f;
if (terrain_alt) {
if (!get_terrain_offset(origin_terr_offset)) {
return false;
}
}
// initialise intermediate point to the origin
_pos_control.set_pos_target(origin + Vector3f(0,0,origin_terr_offset));
_track_desired = 0; // target is at beginning of track
_flags.reached_destination = false;
_flags.fast_waypoint = false; // default waypoint back to slow
_flags.slowing_down = false; // target is not slowing down yet
_flags.segment_type = SEGMENT_STRAIGHT;
_flags.new_wp_destination = true; // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition
// 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);
return true;
}
/// update - update circle controller
void AC_Circle::update()
{
// calculate dt
float dt = _pos_control.time_since_last_xy_update();
if (dt >= 0.2f) {
dt = 0.0f;
}
// ramp angular velocity to maximum
if (_angular_vel < _angular_vel_max) {
_angular_vel += fabsf(_angular_accel) * dt;
_angular_vel = MIN(_angular_vel, _angular_vel_max);
}
if (_angular_vel > _angular_vel_max) {
_angular_vel -= fabsf(_angular_accel) * dt;
_angular_vel = MAX(_angular_vel, _angular_vel_max);
}
// update the target angle and total angle traveled
float angle_change = _angular_vel * dt;
_angle += angle_change;
_angle = wrap_PI(_angle);
_angle_total += angle_change;
// if the circle_radius is zero we are doing panorama so no need to update loiter target
if (!is_zero(_radius)) {
// calculate target position
Vector3f target;
target.x = _center.x + _radius * cosf(-_angle);
target.y = _center.y - _radius * sinf(-_angle);
target.z = _pos_control.get_alt_target();
// update position controller target
_pos_control.set_xy_target(target.x, target.y);
// heading is from vehicle to center of circle
_yaw = get_bearing_cd(_inav.get_position(), _center);
} else {
// set target position to center
Vector3f target;
target.x = _center.x;
target.y = _center.y;
target.z = _pos_control.get_alt_target();
// update position controller target
_pos_control.set_xy_target(target.x, target.y);
// heading is same as _angle but converted to centi-degrees
_yaw = _angle * DEGX100;
}
// update position controller
_pos_control.update_xy_controller();
}
// get_roi_yaw - returns heading towards location held in roi_WP
// should be called at 100hz
float Copter::get_roi_yaw()
{
static uint8_t roi_yaw_counter = 0; // used to reduce update rate to 100hz
roi_yaw_counter++;
if (roi_yaw_counter >= 4) {
roi_yaw_counter = 0;
yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position(), roi_WP);
}
return yaw_look_at_WP_bearing;
}
/*
calculate the turn angle for the next leg of the mission
*/
void Plane::setup_turn_angle(void)
{
int32_t next_ground_course_cd = mission.get_next_ground_course_cd(-1);
if (next_ground_course_cd == -1) {
// the mission library can't determine a turn angle, assume 90 degrees
auto_state.next_turn_angle = 90.0f;
} else {
// get the heading of the current leg
int32_t ground_course_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
// work out the angle we need to turn through
auto_state.next_turn_angle = wrap_180_cd(next_ground_course_cd - ground_course_cd) * 0.01f;
}
}
/// set_origin_and_destination - set origin and destination using lat/lon coordinates
void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
{
// store origin and destination locations
_origin = origin;
_destination = destination;
Vector3f pos_delta = _destination - _origin;
_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;
}
// check _wp_accel_cms is reasonable
if (_wp_accel_cms <= 0) {
_wp_accel_cms.set_and_save(WPNAV_ACCELERATION);
}
// initialise position controller speed and acceleration
_pos_control.set_speed_xy(_wp_speed_cms);
_pos_control.set_accel_xy(_wp_accel_cms);
_pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
_pos_control.calc_leash_length_xy();
_pos_control.calc_leash_length_z();
// calculate leash lengths
calculate_wp_leash_length();
// initialise yaw heading
_yaw = get_bearing_cd(_origin, _destination);
// initialise intermediate point to the origin
_pos_control.set_pos_target(origin);
_track_desired = 0; // target is at beginning of track
_flags.reached_destination = false;
_flags.fast_waypoint = false; // default waypoint back to slow
_flags.segment_type = SEGMENT_STRAIGHT;
// 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);
}
void ModeLoiter::update()
{
// get distance (in meters) to destination
_distance_to_destination = get_distance(rover.current_loc, _destination);
// if within waypoint radius slew desired speed towards zero and use existing desired heading
if (_distance_to_destination <= g.waypoint_radius) {
if (is_negative(_desired_speed)) {
_desired_speed = MIN(_desired_speed + attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
} else {
_desired_speed = MAX(_desired_speed - attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
}
_yaw_error_cd = 0.0f;
} else {
// P controller with hard-coded gain to convert distance to desired speed
// To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
_desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);
// calculate bearing to destination
_desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
// if destination is behind vehicle, reverse towards it
if (fabsf(_yaw_error_cd) > 9000) {
_desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
_desired_speed = -_desired_speed;
}
// reduce desired speed if yaw_error is large
// note: copied from calc_reduced_speed_for_turn_or_distance
float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * ((100.0f - g.speed_turn_gain) * 0.01f);
_desired_speed *= yaw_error_ratio;
}
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
calc_throttle(_desired_speed, false, true);
// mark us as in_reverse when using a negative throttle
// To-Do: only in reverse if vehicle is actually travelling backwards?
rover.set_reverse(_desired_speed < 0);
}
void AP_OSD_Screen::draw_home(uint8_t x, uint8_t y)
{
AP_AHRS &ahrs = AP::ahrs();
Location loc;
if (ahrs.get_position(loc) && ahrs.home_is_set()) {
const Location &home_loc = ahrs.get_home();
float distance = get_distance(home_loc, loc);
int32_t angle = wrap_360_cd(get_bearing_cd(loc, home_loc) - ahrs.yaw_sensor);
int32_t interval = 36000 / SYM_ARROW_COUNT;
if (distance < 2.0f) {
//avoid fast rotating arrow at small distances
angle = 0;
}
char arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT;
backend->write(x, y, false, "%c%c", SYM_HOME, arrow);
draw_distance(x+2, y, distance);
} else {
backend->write(x, y, true, "%c", SYM_HOME);
}
}
// calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
// should be called after calc_lateral_acceleration and before calc_throttle
// relies on these internal members being updated: _yaw_error_cd, _distance_to_destination
float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed)
{
// reduce speed to zero during pivot turns
if (rover.use_pivot_steering(_yaw_error_cd)) {
return 0.0f;
}
// reduce speed to limit overshoot from line between origin and destination
// calculate number of degrees vehicle must turn to face waypoint
const float heading_cd = is_negative(desired_speed) ? wrap_180_cd(ahrs.yaw_sensor + 18000) : ahrs.yaw_sensor;
const float wp_yaw_diff = wrap_180_cd(rover.nav_controller->target_bearing_cd() - heading_cd);
const float turn_angle_rad = fabsf(radians(wp_yaw_diff * 0.01f));
// calculate distance from vehicle to line + wp_overshoot
const float line_yaw_diff = wrap_180_cd(get_bearing_cd(_origin, _destination) - heading_cd);
const float crosstrack_error = rover.nav_controller->crosstrack_error();
const float dist_from_line = fabsf(crosstrack_error);
const bool heading_away = is_positive(line_yaw_diff) == is_positive(crosstrack_error);
const float wp_overshoot_adj = heading_away ? -dist_from_line : dist_from_line;
// calculate radius of circle that touches vehicle's current position and heading and target position and heading
float radius_m = 999.0f;
float radius_calc_denom = fabsf(1.0f - cosf(turn_angle_rad));
if (!is_zero(radius_calc_denom)) {
radius_m = MAX(0.0f, rover.g.waypoint_overshoot + wp_overshoot_adj) / radius_calc_denom;
}
// calculate and limit speed to allow vehicle to stay on circle
float overshoot_speed_max = safe_sqrt(g.turn_max_g * GRAVITY_MSS * MAX(g2.turn_radius, radius_m));
float speed_max = constrain_float(desired_speed, -overshoot_speed_max, overshoot_speed_max);
// limit speed based on distance to waypoint and max acceleration/deceleration
if (is_positive(_distance_to_destination) && is_positive(attitude_control.get_decel_max())) {
const float dist_speed_max = safe_sqrt(2.0f * _distance_to_destination * attitude_control.get_decel_max() + sq(_desired_speed_final));
speed_max = constrain_float(speed_max, -dist_speed_max, dist_speed_max);
}
// return minimum speed
return speed_max;
}
/**
update_bearing_and_distance - updates bearing and distance to the vehicle
should be called at 50hz
*/
void Tracker::update_bearing_and_distance()
{
// exit immediately if we do not have a valid vehicle position
if (!vehicle.location_valid) {
return;
}
// calculate bearing to vehicle
// To-Do: remove need for check of control_mode
if (control_mode != SCAN && !nav_status.manual_control_yaw) {
nav_status.bearing = get_bearing_cd(current_loc, vehicle.location_estimate) * 0.01f;
}
// calculate distance to vehicle
nav_status.distance = get_distance(current_loc, vehicle.location_estimate);
// calculate pitch to vehicle
// To-Do: remove need for check of control_mode
if (control_mode != SCAN && !nav_status.manual_control_pitch) {
nav_status.pitch = degrees(atan2f(nav_status.altitude_difference, nav_status.distance));
}
}
bool Plane::verify_continue_and_change_alt()
{
// is waypoint info not available and heading hold is?
if (locations_are_same(prev_WP_loc, next_WP_loc) &&
steer_state.hold_course_cd != -1) {
//keep flying the same course with fixed steering heading computed at start if cmd
nav_controller->update_heading_hold(steer_state.hold_course_cd);
}
else {
// Is the next_WP less than 200 m away?
if (get_distance(current_loc, next_WP_loc) < 200.0f) {
//push another 300 m down the line
int32_t next_wp_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
location_update(next_WP_loc, next_wp_bearing_cd * 0.01f, 300.0f);
}
//keep flying the same course
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
}
//climbing?
if (condition_value == 1 && adjusted_altitude_cm() >= next_WP_loc.alt) {
return true;
}
//descending?
else if (condition_value == 2 &&
adjusted_altitude_cm() <= next_WP_loc.alt) {
return true;
}
//don't care if we're climbing or descending
else if (labs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) {
return true;
}
return false;
}
/// get_bearing_to_target - get bearing to target position in centi-degrees
int32_t AC_PosControl::get_bearing_to_target() const
{
return get_bearing_cd(_inav.get_position(), _pos_target);
}
/// get_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t AC_WPNav::get_bearing_to_destination()
{
return get_bearing_cd(_inav->get_position(), _destination);
}
void pf_field::update(local_member* p_ac, AP_AHRS_DCM* ahrs, AP_Airspeed* airspeed)
{
#endif
#if HIL_MODE == HIL_MODE_ATTITUDE
void pf_field::update(local_member* p_ac, AP_AHRS_HIL* ahrs, AP_Airspeed* airspeed)
{
#endif
const Relative* tmp_p_rel = p_ac->get_rel(); //Gets the current relative information from the ac object
Vector3i tmp_gV_L = *p_ac->get_gV_L();
uint16_t tmp_psi_ac = *p_ac->get_hdg(); //Get our heading (deg)
//Calculate total repulsive function from i flock members
Vector3f tmp_r_phi; //initialize vector (NED components)
//Sum repulsive functions from i flock members
for(int i=0;i<tmp_p_rel->Num_members;i++)
{
//indexing convention to organize member pointers
int j = tmp_p_rel->Member_ids[i]-1;
//Relative distance to flock members (NED components)
Vector3f tmp_dX; //initialize vector each iteration
tmp_dX.x=tmp_p_rel->dX[j]/100.0; //[m]
tmp_dX.y=tmp_p_rel->dY[j]/100.0; //[m]
tmp_dX.z=tmp_p_rel->dZ[j]/100.0; //[m]
//Calculate repulsive parameters based on relative states
//Gaussian repulsive function accumulation
tmp_r_phi.x += (-2*(tmp_dX.x)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0));
tmp_r_phi.y += (-2*(tmp_dX.y)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0));
tmp_r_phi.z += (-2*(tmp_dX.z)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0));
}
_phi_r = tmp_r_phi; //Repulsive potential gradient vector (NED component)
//Relative distance vector to leader (NED components)
Vector3i tmp_dL; //initialize vector (NED components)
tmp_dL.x=tmp_p_rel->dXL; //[m*100]
tmp_dL.y=tmp_p_rel->dYL; //[m*100]
tmp_dL.z=tmp_p_rel->dZL; //[m*100]
//Relative distance magnitude to leader
int32_t tmp_d2L=tmp_p_rel->d2L; //[m*100]
//Relative velocity to leader (NED components)
//Vector3i tmp_dV; //initialize vector (NED components)
Vector3i tmp_gV_LNAV; //initialize dVaspd (Local Navigation frame)
//tmp_dV.x=tmp_p_rel->dVXL; //[m/s*100]
//tmp_dV.y=tmp_p_rel->dVYL; //[m/s*100]
//tmp_dV.z=tmp_p_rel->dVZL; //[m/s*100]
float Cpsi = cos(ToRad(tmp_psi_ac/100.0));
float Spsi = sin(ToRad(tmp_psi_ac/100.0));
tmp_gV_LNAV.x=int16_t((tmp_gV_L.x)*Cpsi+(tmp_gV_L.y)*Spsi); //[m/s*100]
tmp_gV_LNAV.y=int16_t(-(tmp_gV_L.x)*Spsi+(tmp_gV_L.y)*Cpsi); //[m/s*100]
tmp_gV_LNAV.z=tmp_p_rel->dVZL; //[m/s*100]
//Calculate the attractive potential gradient (NED components)
/*Note:
Attractive potential is comprised of two types of potential functions
-In the far-field regime we use a linear potential field
-In the near-field regime we use a quadratic potential field
These fields must be matched at _chi, the near-field, far-field regime cut-off such that
-Gradient of quadratic at _chi = gradient of linear at _chi
In far-field, we are guided towards the leader's position (no offset) - this should improve convergence
In near-field, we are guided towards the offset position, the side of which is dictated by swarm algorithm
*/
Vector3i tmp_pf_offset_NED; //Initialize vector
uint16_t tmp_psi_L = tmp_p_rel->hdgL; //Get the heading of the Leader
float CpsiL = cos(ToRad(tmp_p_rel->hdgL/100.0));
float SpsiL = sin(ToRad(tmp_p_rel->hdgL/100.0));
//set offset (Leader's Local frame)
Vector3i tmp_pf_offset = _pf_offset_l;
//update offset with side correction
tmp_pf_offset.y = tmp_pf_offset.y*_side;
//Rotate the offset distances (Leader's Local Navigation Frame) to NED frame
tmp_pf_offset_NED.x = tmp_pf_offset.x*CpsiL - tmp_pf_offset.y*SpsiL;
tmp_pf_offset_NED.y = tmp_pf_offset.x*SpsiL + tmp_pf_offset.y*CpsiL;
tmp_pf_offset_NED.z = tmp_pf_offset.z;
//Extrapolate Goal for NF case
Location tmp_goal_loc = *p_ac->get_loc();
Location tmp_current_loc = *p_ac->get_loc();
//A bunch of new stuff for this extrapolation technique
location_offset(&tmp_goal_loc,tmp_dL.x/100.0, tmp_dL.y/100.0);
location_offset(&tmp_goal_loc,-tmp_pf_offset_NED.x/100.0, -tmp_pf_offset_NED.y/100.0);
float tmp_N_extrap = 10*CpsiL;
float tmp_E_extrap = 10*SpsiL;
location_offset(&tmp_goal_loc,tmp_N_extrap, tmp_E_extrap);
int32_t tmp_B_extrap = get_bearing_cd(&tmp_current_loc,&tmp_goal_loc);
Vector3f extrap_uvec_NED;
extrap_uvec_NED.x = cos(ToRad(tmp_B_extrap/100.0));
extrap_uvec_NED.y = sin(ToRad(tmp_B_extrap/100.0));
extrap_uvec_NED.z = 0;
tmp_dL -=tmp_pf_offset_NED;
tmp_d2L = int32_t(100*safe_sqrt(square(tmp_dL.x/100.0)+square(tmp_dL.y/100.0)+square(tmp_dL.z/100.0)));
//Subtract offset distance vector from leader distance vector to get modified relative distance vector to leader
if(tmp_d2L<(_chi*100)) //Near-field regime condition
{
_phi_a.x = 2*(_lambda.x/100.0)*(tmp_dL.x/100.0); //Calculate Quad PF X gradient with weighting parameter (NED frame)
_phi_a.y = 2*(_lambda.y/100.0)*(tmp_dL.y/100.0); //Calculate Quad PF Y gradient with weighting parameter (NED frame)
_phi_a.z = 2*(_lambda.z/100.0)*(tmp_dL.z/100.0); //Calculate Quad PF Z gradient with weighting parameter (NED frame)
float tmp_phi_a_mag = _phi_a.length();
_phi_a_c.x = tmp_phi_a_mag*extrap_uvec_NED.x;
_phi_a_c.y = tmp_phi_a_mag*extrap_uvec_NED.y;
_phi_a_c.z = 0;
}
else
{
//float tmp_M =2*safe_sqrt(square(_lambda.x/100.0)+square(_lambda.y/100.0)+square(_lambda.z/100.0))*(_chi); //Slope of linear potential field (Magnitude of gradient) = nominal magnitude of quadratic gradient at chi
float tmp_M = 2*_chi;
float tmp_R = safe_sqrt(square((_lambda.x/100.0)*(tmp_dL.x/100.0))+square((_lambda.y/100.0)*(tmp_dL.y/100.0))+square((_lambda.z/100.0)*(tmp_dL.z/100)));
_phi_a.x = tmp_M*square(_lambda.x/100.0)*(tmp_dL.x/100.0)/tmp_R; //Calculate Lin PF X gradient with weighting parameter (NED frame)
_phi_a.y = tmp_M*square(_lambda.y/100.0)*(tmp_dL.y/100.0)/tmp_R; //Calculate Lin PF Y gradient with weighting parameter (NED frame)
_phi_a.z = tmp_M*square(_lambda.z/100.0)*(tmp_dL.z/100.0)/tmp_R; //Calculate Lin PF Z gradient with weighting parameter (NED frame)
}
//Calculate the total potential gradient components
_phi_NED = _phi_a + _phi_r;
_phi_c_NED = _phi_a_c + _phi_r;
//Calculate the magnitude of the total potential function gradient
if(_phi_NED.length() > 0) //divide-by-zero check (safety first)
{
//Calculate the normalized potential gradient components
_Nphi_NED = _phi_NED.normalized();
_Nphi_c_NED = _phi_c_NED.normalized();
//Find X component in Local Navigation frame by rotating NED vector about heading
_Nphi_l.x = _Nphi_NED.x*Cpsi + _Nphi_NED.y*Spsi;
if(tmp_d2L<_chi*100) //near-field consideration- We don't want the VWP behind the body
{
_regime_mask = 0x01; //Make first bit in regime mask 1 to reflect that we're in near-field regime
/* //Correct normalized vector for VWP in near-field
if(_Nphi_l.x<0) //Vector is pointing behind ac in Local Navigation frame
{
//Rotate NED pf gradiend components into Local Navigation components
_phi_l.x = _phi_NED.x*cos(ToRad(tmp_psi_ac/100.0)) + _phi_NED.y*sin(ToRad(tmp_psi_ac/100.0));
_phi_l.y = - _phi_NED.x*sin(ToRad(tmp_psi_ac/100.0)) + _phi_NED.y*cos(ToRad(tmp_psi_ac/100.0));
//Correct potential function to point forward
/* Note:
Corrected by using airspeed instead of relative distance... almost a predictor of where the goal "will" be in a second
/
float airspeed_est;
if(ahrs->airspeed_estimate(&airspeed_est))
{
_phi_l.x = 2*(_lambda.x/100.0)*(airspeed_est);
}
else
{
_phi_l.x = 2*(_lambda.x/100.0)*10; //Not the most elegant way to do this... 10 is an average airspeed for the skysurfer
}
//Corrected potential rotated back into NED frame from Local Navigation frame
_phi_c_NED.x= _phi_l.x*cos(ToRad(tmp_psi_ac/100.0)) - _phi_l.y*sin(ToRad(tmp_psi_ac/100.0));
_phi_c_NED.y= _phi_l.x*sin(ToRad(tmp_psi_ac/100.0)) + _phi_l.y*cos(ToRad(tmp_psi_ac/100.0));
//Normalize the corrected potential gradient vector
_Nphi_c_NED = _phi_c_NED.normalized();
//Modify regime mask to reflect that both the first bit and the second bit are 1 (near-field regime, and using a corrected
_regime_mask = 0x03;
}
//Make sure regime mask reflects that only
else _regime_mask = 0x01; */
}
else
{
//Regime mask should reflect that we are not in the near-field, and we therefore aren't using a corrected potential field
_regime_mask = 0x00;
}
}
else
{
//If the magnitude of the gradient is not greater than 0, make the norm 0 to avoid divide by 0 problems
_Nphi_NED.zero();
_Nphi_c_NED.zero();
}
float tmp_dNorth_com;
float tmp_dEast_com;
float tmp_dalt_com;
float tmp_dspd_com;
if(tmp_d2L<_chi*100)
{
//Calculate the North and East Offset [m]
tmp_dNorth_com = _VWP_offset*_Nphi_c_NED.x;
tmp_dEast_com = _VWP_offset*_Nphi_c_NED.y;
//Calculate the change in altitude [m]
//k_alt_V is the equivalent of a derivative gain- set to 0 for simplicity.
//tmp_dalt_com = _VWP_Z_offset*_Nphi_NED.z;
tmp_dalt_com = tmp_dL.z/100.0;
_phi_norm = _Nphi_c_NED;
}
else
{ //Far-Field Case
tmp_dNorth_com = _VWP_offset*_Nphi_NED.x;
tmp_dEast_com = _VWP_offset*_Nphi_NED.y;
//tmp_dalt_com = _VWP_Z_offset*_Nphi_NED.z;
tmp_dalt_com = tmp_dL.z/100.0;
_phi_norm = _Nphi_NED;
}
const Location* p_current_location = p_ac->get_loc();
_next_VWP = *p_current_location;
location_offset(&_next_VWP, tmp_dNorth_com, tmp_dEast_com);
_next_VWP.alt+=(int32_t)(tmp_dalt_com*100); //double-check sign here.
constrain(_next_VWP.alt,PFG_MIN_ALT,PFG_MAX_ALT);
if(tmp_d2L<_chi*100)
{ //Near-Field Case
//New speed matching/ Speed from PFG command algorithm
_next_airspeed_com = int32_t(100*((tmp_gV_LNAV.x/100.0*(PFG_K_P_dV/100.0)) +(_Nphi_l.x*(PFG_K_I_dV/100.0))));
constrain(_next_airspeed_com,PFG_MIN_AIRSPEED_CM,PFG_MAX_AIRSPEED_CM);
}
else
{ //Far-Field Case
_next_airspeed_com = PFG_MAX_AIRSPEED_CM; //In far-field, set airspeed to maximum allowable (to catch up)
}
}
const Location* pf_field::get_VWP(){
return (const Location*)&_next_VWP;
}
const int32_t* pf_field::get_new_speed(){
return (const int32_t*)&_next_airspeed_com;
}
const Vector3f* pf_field::get_pfg_att(){
return (const Vector3f*)&_phi_a;
}
const Vector3f* pf_field::get_pfg_rep(){
return (const Vector3f*)&_phi_r;
}
const Vector3f* pf_field::get_pfg_norm(){
return (const Vector3f*)&_phi_norm;
}
uint8_t pf_field::get_regime_mask(){
return _regime_mask;
}
pf_field PF_FIELD;
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t AC_WPNav::get_bearing_to_target() const
{
return get_bearing_cd(_inav->get_position(), _target);
}
/// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t AC_WPNav::get_wp_bearing_to_destination() const
{
return get_bearing_cd(_inav.get_position(), _destination);
}
void Copter::ModeFollow::run()
{
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
make_safe_spool_down();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// re-use guided mode's velocity controller
// Note: this is safe from interference from GCSs and companion computer's whose guided mode
// position and velocity requests will be ignored while the vehicle is not in guided mode
// variables to be sent to velocity controller
Vector3f desired_velocity_neu_cms;
bool use_yaw = false;
float yaw_cd = 0.0f;
Vector3f dist_vec; // vector to lead vehicle
Vector3f dist_vec_offs; // vector to lead vehicle + offset
Vector3f vel_of_target; // velocity of lead vehicle
if (g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target)) {
// convert dist_vec_offs to cm in NEU
const Vector3f dist_vec_offs_neu(dist_vec_offs.x * 100.0f, dist_vec_offs.y * 100.0f, -dist_vec_offs.z * 100.0f);
// calculate desired velocity vector in cm/s in NEU
const float kp = g2.follow.get_pos_p().kP();
desired_velocity_neu_cms.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp);
desired_velocity_neu_cms.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp);
desired_velocity_neu_cms.z = (-vel_of_target.z * 100.0f) + (dist_vec_offs_neu.z * kp);
// scale desired velocity to stay within horizontal speed limit
float desired_speed_xy = safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y));
if (!is_zero(desired_speed_xy) && (desired_speed_xy > pos_control->get_max_speed_xy())) {
const float scalar_xy = pos_control->get_max_speed_xy() / desired_speed_xy;
desired_velocity_neu_cms.x *= scalar_xy;
desired_velocity_neu_cms.y *= scalar_xy;
desired_speed_xy = pos_control->get_max_speed_xy();
}
// limit desired velocity to be between maximum climb and descent rates
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -fabsf(pos_control->get_max_speed_down()), pos_control->get_max_speed_up());
// unit vector towards target position (i.e. vector to lead vehicle + offset)
Vector3f dir_to_target_neu = dist_vec_offs_neu;
const float dir_to_target_neu_len = dir_to_target_neu.length();
if (!is_zero(dir_to_target_neu_len)) {
dir_to_target_neu /= dir_to_target_neu_len;
}
// create horizontal desired velocity vector (required for slow down calculations)
Vector2f desired_velocity_xy_cms(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y);
// create horizontal unit vector towards target (required for slow down calculations)
Vector2f dir_to_target_xy(desired_velocity_xy_cms.x, desired_velocity_xy_cms.y);
if (!dir_to_target_xy.is_zero()) {
dir_to_target_xy.normalize();
}
// slow down horizontally as we approach target (use 1/2 of maximum deceleration for gentle slow down)
const float dist_to_target_xy = Vector2f(dist_vec_offs_neu.x, dist_vec_offs_neu.y).length();
copter.avoid.limit_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy() * 0.5f, desired_velocity_xy_cms, dir_to_target_xy, dist_to_target_xy, copter.G_Dt);
// limit the horizontal velocity to prevent fence violations
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy(), desired_velocity_xy_cms, G_Dt);
// copy horizontal velocity limits back to 3d vector
desired_velocity_neu_cms.x = desired_velocity_xy_cms.x;
desired_velocity_neu_cms.y = desired_velocity_xy_cms.y;
// limit vertical desired_velocity_neu_cms to slow as we approach target (we use 1/2 of maximum deceleration for gentle slow down)
const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt);
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -des_vel_z_max, des_vel_z_max);
// get avoidance adjusted climb rate
desired_velocity_neu_cms.z = get_avoidance_adjusted_climbrate(desired_velocity_neu_cms.z);
// calculate vehicle heading
switch (g2.follow.get_yaw_behave()) {
case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: {
const Vector3f dist_vec_xy(dist_vec.x, dist_vec.y, 0.0f);
if (dist_vec_xy.length() > 1.0f) {
yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy);
use_yaw = true;
}
break;
}
case AP_Follow::YAW_BEHAVE_SAME_AS_LEAD_VEHICLE: {
float target_hdg = 0.0f;
if (g2.follow.get_target_heading_deg(target_hdg)) {
yaw_cd = target_hdg * 100.0f;
use_yaw = true;
}
break;
}
case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: {
const Vector3f vel_vec(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y, 0.0f);
if (vel_vec.length() > 100.0f) {
yaw_cd = get_bearing_cd(Vector3f(), vel_vec);
use_yaw = true;
}
break;
}
case AP_Follow::YAW_BEHAVE_NONE:
default:
// do nothing
break;
}
}
// log output at 10hz
uint32_t now = AP_HAL::millis();
bool log_request = false;
if ((now - last_log_ms >= 100) || (last_log_ms == 0)) {
log_request = true;
last_log_ms = now;
}
// re-use guided mode's velocity controller (takes NEU)
Copter::ModeGuided::set_velocity(desired_velocity_neu_cms, use_yaw, yaw_cd, false, 0.0f, false, log_request);
Copter::ModeGuided::run();
}
// update L1 control for loitering
void AP_L1_Control::update_loiter(const struct Location ¢er_WP, float radius, int8_t loiter_direction)
{
struct Location _current_loc;
// Calculate guidance gains used by PD loop (used during circle tracking)
float omega = (6.2832f / _L1_period);
float Kx = omega * omega;
float Kv = 2.0f * _L1_damping * omega;
// Calculate L1 gain required for specified damping (used during waypoint capture)
float K_L1 = 4.0f * _L1_damping * _L1_damping;
//Get current position and velocity
_ahrs->get_position(&_current_loc);
// update _target_bearing_cd
_target_bearing_cd = get_bearing_cd(&_current_loc, ¢er_WP);
Vector2f _groundspeed_vector = _ahrs->groundspeed_vector();
//Calculate groundspeed
float groundSpeed = _groundspeed_vector.length();
// Calculate time varying control parameters
// Calculate the L1 length required for specified period
// 0.3183099 = 1/pi
_L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;
//Convert current location and WP positionsto 2D vectors in lat and long
Vector2f A_air((_current_loc.lat*1.0e-7f), (_current_loc.lng*1.0e-7f));
Vector2f A_v((center_WP.lat*1.0e-7f), (center_WP.lng*1.0e-7f));
//Calculate the NE position of the aircraft relative to WP A
A_air = _geo2planar(A_v, A_air)*RADIUS_OF_EARTH;
//Calculate the unit vector from WP A to aircraft
Vector2f A_air_unit = (A_air).normalized();
//Calculate Nu to capture center_WP
float xtrackVelCap = A_air_unit % _groundspeed_vector; // Velocity across line - perpendicular to radial inbound to WP
float ltrackVelCap = - (_groundspeed_vector * A_air_unit); // Velocity along line - radial inbound to WP
float Nu = atan2f(xtrackVelCap,ltrackVelCap);
Nu = constrain_float(Nu, -1.5708f, +1.5708f); //Limit Nu to +- Pi/2
//Calculate lat accln demand to capture center_WP (use L1 guidance law)
float latAccDemCap = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);
//Calculate radial position and velocity errors
float xtrackVelCirc = -ltrackVelCap; // Radial outbound velocity - reuse previous radial inbound velocity
float xtrackErrCirc = A_air.length() - radius; // Radial distance from the loiter circle
// keep crosstrack error for reporting
_crosstrack_error = xtrackErrCirc;
//Calculate PD control correction to circle waypoint
float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv);
//Calculate tangential velocity
float velTangent = xtrackVelCap * float(loiter_direction);
//Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way
if ( velTangent < 0.0f ) {
latAccDemCircPD = _maxf(latAccDemCircPD , 0.0f);
}
// Calculate centripetal acceleration demand
float latAccDemCircCtr = velTangent * velTangent / _maxf((0.5f * radius) , (radius + xtrackErrCirc));
//Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand
float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr);
// Perform switchover between 'capture' and 'circle' modes at the point where the commands cross over to achieve a seamless transfer
// Only fly 'capture' mode if outside the circle
if ((latAccDemCap < latAccDemCirc && loiter_direction > 0 && xtrackErrCirc > 0.0f) | (latAccDemCap > latAccDemCirc && loiter_direction < 0 && xtrackErrCirc > 0.0f)) {
_latAccDem = latAccDemCap;
_WPcircle = false;
_bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track
_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
} else {
_latAccDem = latAccDemCirc;
_WPcircle = true;
_bearing_error = 0.0f; // bearing error (radians), +ve to left of track
_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point
}
}
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t AC_WPNav::get_loiter_bearing_to_target() const
{
return get_bearing_cd(_inav->get_position(), _pos_control.get_pos_target());
}
// update L1 control for waypoint navigation
// this function costs about 3.5 milliseconds on a AVR2560
void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP)
{
struct Location _current_loc;
float Nu;
float xtrackVel;
float ltrackVel;
// Calculate L1 gain required for specified damping
float K_L1 = 4.0f * _L1_damping * _L1_damping;
// Get current position and velocity
_ahrs->get_position(&_current_loc);
// update _target_bearing_cd
_target_bearing_cd = get_bearing_cd(&_current_loc, &next_WP);
Vector2f _groundspeed_vector = _ahrs->groundspeed_vector();
//Calculate groundspeed
float groundSpeed = _groundspeed_vector.length();
// Calculate time varying control parameters
// Calculate the L1 length required for specified period
// 0.3183099 = 1/1/pipi
_L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;
//Convert current location and WP positions to 2D vectors in lat and long
Vector2f A_air((_current_loc.lat*1.0e-7f), (_current_loc.lng*1.0e-7f));
Vector2f A_v((prev_WP.lat*1.0e-7f), (prev_WP.lng*1.0e-7f));
Vector2f B_v((next_WP.lat*1.0e-7f), (next_WP.lng*1.0e-7f));
//Calculate the NE position of the aircraft and WP B relative to WP A
A_air = _geo2planar(A_v, A_air)*RADIUS_OF_EARTH;
Vector2f AB = _geo2planar(A_v, B_v)*RADIUS_OF_EARTH;
//Calculate the unit vector from WP A to WP B
Vector2f AB_unit = (AB).normalized();
// calculate distance to target track, for reporting
_crosstrack_error = AB_unit % A_air;
//Determine if the aircraft is behind a +-135 degree degree arc centred on WP A
//and further than L1 distance from WP A. Then use WP A as the L1 reference point
//Otherwise do normal L1 guidance
float WP_A_dist = A_air.length();
float alongTrackDist = A_air * AB_unit;
if (WP_A_dist > _L1_dist && alongTrackDist/(WP_A_dist + 1.0f) < -0.7071f) {
//Calc Nu to fly To WP A
Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft
xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line
ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line
Nu = atan2f(xtrackVel,ltrackVel);
_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
} else { //Calc Nu to fly along AB line
//Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints)
xtrackVel = _groundspeed_vector % AB_unit; // Velocity cross track
ltrackVel = _groundspeed_vector * AB_unit; // Velocity along track
float Nu2 = atan2f(xtrackVel,ltrackVel);
//Calculate Nu1 angle (Angle to L1 reference point)
float xtrackErr = A_air % AB_unit;
float sine_Nu1 = xtrackErr/_maxf(_L1_dist , 0.1f);
//Limit sine of Nu1 to provide a controlled track capture angle of 45 deg
sine_Nu1 = constrain_float(sine_Nu1, -0.7854f, 0.7854f);
float Nu1 = asinf(sine_Nu1);
Nu = Nu1 + Nu2;
_nav_bearing = atan2f(AB_unit.y, AB_unit.x) + Nu1; // bearing (radians) from AC to L1 point
}
//Limit Nu to +-pi
Nu = constrain_float(Nu, -1.5708f, +1.5708f);
_latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);
// Waypoint capture status is always false during waypoint following
_WPcircle = false;
_bearing_error = Nu; // bearing error angle (radians), +ve to left of track
}