Skip to content

Add FW loiter size and orientation control #24625

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

Open
wants to merge 5 commits into
base: main
Choose a base branch
from
+26 −8
Open

Add FW loiter size and orientation control #24625

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
4b82bcd
Add loiter size and orientation
ryanjAA Mar 27, 2025
8a02797
Fixed formatting
ryanjAA Mar 31, 2025
d58e3b9
Update src/modules/navigator/navigator_main.cpp
ryanjAA Apr 29, 2025
3f4fbe4
Update src/modules/navigator/navigator_main.cpp
ryanjAA Apr 29, 2025
ac4faa2
Merge branch 'main' into patch-15
sfuhrer Apr 30, 2025
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
34 changes: 26 additions & 8 deletions src/modules/navigator/navigator_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -310,17 +310,35 @@ void Navigator::run()
rep->current.cruising_speed = cmd.param1;
}

rep->current.cruising_throttle = get_cruising_throttle();
rep->current.acceptance_radius = get_acceptance_radius();

// Go on and check which changes had been requested
if (PX4_ISFINITE(cmd.param4)) {
rep->current.yaw = cmd.param4;
// (3) Loiter radius (param3)
if (PX4_ISFINITE(cmd.param3) && cmd.param3 > FLT_EPSILON) {
rep->current.loiter_radius = fabsf(cmd.param3);

} else {
// Use current or default if param3 not valid
rep->current.loiter_radius = get_loiter_radius();
}

// (4) Loiter direction / yaw (param4)
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
// param4 >= 1 => counter-clockwise, else default CW
rep->current.loiter_direction_counter_clockwise = (PX4_ISFINITE(cmd.param4) && (cmd.param4 >= FLT_EPSILON));
rep->current.yaw = NAN;

} else {
// multicopter => param4 = yaw
if (PX4_ISFINITE(cmd.param4)) {
rep->current.yaw = cmd.param4;

} else {
rep->current.yaw = NAN;
}
}


rep->current.cruising_throttle = get_cruising_throttle();
rep->current.acceptance_radius = get_acceptance_radius();

if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
// Position change with optional altitude change
rep->current.lat = cmd.param5;
Expand Down Expand Up @@ -392,7 +410,7 @@ void Navigator::run()
rep->current.loiter_pattern = position_setpoint_s::LOITER_TYPE_ORBIT;
}

rep->current.loiter_direction_counter_clockwise = curr->current.loiter_direction_counter_clockwise;
rep->current.loiter_direction_counter_clockwise = (PX4_ISFINITE(cmd.param4) ? (cmd.param4 >= 1.0f) : false);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we really need this separate handling? In the only_alt_change_requested case we, as the name says, want to keep everything beside the altitude as it was set before, no?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I added this so when a 'change-altitude-only' command is sent (i.e. param3 and param4 omitted), we explicitly apply the vehicle’s default loiter radius and direction, exactly the behavior people expect today. Without this, the vehicle would continue using the previous radius/direction settings, which can be surprising.

Also, this makes the behavior match the MAVLink spec’s intent that omitted parameters revert to defaults.

That said, I don't actually care but I do think it's the right thing.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ryanjAA I still challenge that it's expected that if you send a "change altitude" through QGC the loiter direction changes (see screen-recording).
Could QGC send a full GoTo setpoint again? Probably, but it's currently not doing it, thus this change changes the current behavior.

Screencast.from.30-04-25.17.48.25.webm

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also, this makes the behavior match the MAVLink spec’s intent that omitted parameters revert to defaults.

But then the lat/long fields should also not being assumed but reset to e.g. the current vehicle position. Currently there it keeps the lat/long from the previous setpoint.

}

rep->previous.timestamp = hrt_absolute_time();
Expand Down Expand Up @@ -474,7 +492,7 @@ void Navigator::run()
rep->current.loiter_radius = get_loiter_radius();
}

rep->current.loiter_direction_counter_clockwise = curr->current.loiter_direction_counter_clockwise;
rep->current.loiter_direction_counter_clockwise = (PX4_ISFINITE(cmd.param4) ? (cmd.param4 >= 1.0f) : false);

rep->previous.timestamp = hrt_absolute_time();

Expand Down
Loading