Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

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

Browse Source 
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
1 changed files with 24 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+24 -3
View File
@@ -282,7 +282,13 @@ void Navigator::run()
// If no argument for ground speed, use default value. // If no argument for ground speed, use default value.
if (cmd.param1 <= 0 || !PX4_ISFINITE(cmd.param1)) { 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 { } else {
rep->current.cruising_speed = cmd.param1; rep->current.cruising_speed = cmd.param1;
@@ -403,7 +409,14 @@ void Navigator::run()
rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; 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.cruising_throttle = get_cruising_throttle();
rep->current.acceptance_radius = get_acceptance_radius(); rep->current.acceptance_radius = get_acceptance_radius();
rep->current.yaw = NAN; rep->current.yaw = NAN;
@@ -468,9 +481,16 @@ void Navigator::run()
rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_radius = get_loiter_radius();
rep->current.loiter_direction_counter_clockwise = false; rep->current.loiter_direction_counter_clockwise = false;
rep->current.cruising_speed = get_cruising_speed();
rep->current.cruising_throttle = get_cruising_throttle(); 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)) { if (PX4_ISFINITE(cmd.param1)) {
rep->current.loiter_radius = fabsf(cmd.param1); rep->current.loiter_radius = fabsf(cmd.param1);
rep->current.loiter_direction_counter_clockwise = cmd.param1 < 0; 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_radius = get_loiter_radius();
rep->current.loiter_direction_counter_clockwise = false; rep->current.loiter_direction_counter_clockwise = false;
rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
rep->current.cruising_speed = -1.f; // reset to default
if (home_global_position_valid()) { if (home_global_position_valid()) {
// Only set yaw if we know the true heading // Only set yaw if we know the true heading