Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Reposition: Fix previous valid state to enable proper line following #7717

Closed
wants to merge 1 commit into from
Closed

Reposition: Fix previous valid state to enable proper line following #7717

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 17 additions & 4 deletions src/modules/navigator/loiter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -153,10 +153,23 @@ Loiter::reposition()
// convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.velocity_valid = false;
pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw;
pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt;

// if previous is not valid set it to current position
// without a previous setpoint the controllers will not
// perform cross-track correction
if (pos_sp_triplet->previous.valid) {
memcpy(&pos_sp_triplet->previous, &rep->previous, sizeof(rep->previous));

} else {
pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw;
pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->previous.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
pos_sp_triplet->previous.acceptance_radius = _navigator->get_acceptance_radius();
pos_sp_triplet->previous.valid = true;
}

memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current));
pos_sp_triplet->next.valid = false;

Expand Down
53 changes: 44 additions & 9 deletions src/modules/navigator/navigator_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -385,15 +385,14 @@ Navigator::task_main()
struct position_setpoint_triplet_s *rep = get_reposition_triplet();
struct position_setpoint_triplet_s *curr = get_position_setpoint_triplet();

// store current position as previous position and goal as next
rep->previous.yaw = get_global_position()->yaw;
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;

// Store current position as previous position and the user-selected position
// as the current (goal) position
rep->current.loiter_radius = get_loiter_radius();
rep->current.loiter_direction = 1;
rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
rep->current.acceptance_radius = get_acceptance_radius();
rep->current.valid = true;
rep->next.valid = false;

// Go on and check which changes had been requested
if (PX4_ISFINITE(cmd.param4)) {
Expand Down Expand Up @@ -432,18 +431,54 @@ Navigator::task_main()
rep->current.alt = get_global_position()->alt;
}

// Check if the distance is sufficient to calculate a stable vector
// between the current and the goal position
float distance = get_distance_to_next_waypoint(rep->previous.lat, rep->previous.lon, rep->current.lat,
rep->current.lon);

if (distance > 2.0f * get_acceptance_radius()) {

// Get the vector between the current and the goal position
float v_next_n, v_next_e;
get_vector_to_next_waypoint(get_global_position()->lat, get_global_position()->lon,
rep->current.lat, rep->current.lon, &v_next_n, &v_next_e);

// Add the current speed and give the vehicle 2 seconds
// to adjust speed, so the vehicle can intercept the line
v_next_n = -v_next_n + 2.0f * get_global_position()->vel_n;
v_next_e = -v_next_e + 2.0f * get_global_position()->vel_e;

// Ensure the system tracks the line segment immediately by moving
// the previous tracking position further behind
double lat_res, lon_res;
add_vector_to_global_position(get_global_position()->lat, get_global_position()->lon,
v_next_n, v_next_e, &lat_res, &lon_res);
rep->previous.lat = lat_res;
rep->previous.lon = lon_res;

} else {
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
}

rep->previous.yaw = get_global_position()->yaw;
rep->previous.alt = get_global_position()->alt;
rep->previous.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
rep->previous.acceptance_radius = get_acceptance_radius();
rep->previous.valid = true;
rep->current.valid = true;
rep->next.valid = false;

} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
struct position_setpoint_triplet_s *rep = get_takeoff_triplet();

// store current position as previous position and goal as next
// Store current position as previous position and the user-selected position
// as the current (goal) position
rep->previous.yaw = get_global_position()->yaw;
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
rep->previous.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
rep->previous.acceptance_radius = get_acceptance_radius();
rep->previous.valid = true;

rep->current.loiter_radius = get_loiter_radius();
rep->current.loiter_direction = 1;
Expand Down