dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Navigator: set cruise_speed to default on entering new mode (#21503) 

The resets in the modes (eg Loiter mode) are not active yet, so manually
set rep->current.cruising_speed = -1.f if not already in the same flight mode.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-05-05 15:53:17 +02:00 committed by GitHub
parent 40324b03f4
commit d2ca763d23
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 24 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/modules/navigator/navigator_main.cpp
View File

@ -282,7 +282,13 @@ void Navigator::run()
// If no argument for ground speed, use default value.
if (cmd.param1 <= 0 || !PX4_ISFINITE(cmd.param1)) {
rep->current.cruising_speed = get_cruising_speed();
// on entering Loiter mode, reset speed setpoint to default
if (_navigation_mode != &_loiter) {
rep->current.cruising_speed = -1.f;
} else {
rep->current.cruising_speed = get_cruising_speed();
}
} else {
rep->current.cruising_speed = cmd.param1;
@ -403,7 +409,14 @@ void Navigator::run()
rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
rep->current.cruising_speed = get_cruising_speed();
// on entering Loiter mode, reset speed setpoint to default
if (_navigation_mode != &_loiter) {
rep->current.cruising_speed = -1.f;
} else {
rep->current.cruising_speed = get_cruising_speed();
}
rep->current.cruising_throttle = get_cruising_throttle();
rep->current.acceptance_radius = get_acceptance_radius();
rep->current.yaw = NAN;
@ -468,9 +481,16 @@ void Navigator::run()
rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
rep->current.loiter_radius = get_loiter_radius();
rep->current.loiter_direction_counter_clockwise = false;
rep->current.cruising_speed = get_cruising_speed();
rep->current.cruising_throttle = get_cruising_throttle();
// on entering Loiter mode, reset speed setpoint to default
if (_navigation_mode != &_loiter) {
rep->current.cruising_speed = -1.f;
} else {
rep->current.cruising_speed = get_cruising_speed();
}
if (PX4_ISFINITE(cmd.param1)) {
rep->current.loiter_radius = fabsf(cmd.param1);
rep->current.loiter_direction_counter_clockwise = cmd.param1 < 0;
@ -499,6 +519,7 @@ void Navigator::run()
rep->current.loiter_radius = get_loiter_radius();
rep->current.loiter_direction_counter_clockwise = false;
rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
rep->current.cruising_speed = -1.f; // reset to default
if (home_global_position_valid()) {
// Only set yaw if we know the true heading