From 06d030c0baa4f03834a74befdbae335cc9da9a23 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 19 Apr 2023 16:00:16 +0200 Subject: [PATCH] Navigator: set cruise_speed to default on entering new mode 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 --- src/modules/navigator/navigator_main.cpp | 27 +++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0817d7c7b446..cbd0bd8fa2c7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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