diff --git a/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp b/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp index 56ae1c5dd2..247ecb53c0 100644 --- a/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp +++ b/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp @@ -237,7 +237,7 @@ FixedWingGuidanceControl::update_in_air_states(const hrt_abstime now) void FixedWingGuidanceControl::control_auto_path(const float control_interval, const Vector2d &curr_pos, - const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN; @@ -291,10 +291,11 @@ FixedWingGuidanceControl::Run() /* only run controller if position changed and we are not running an external mode*/ - const bool is_external_nav_state = (_vehicle_status.nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1) - && (_vehicle_status.nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8); + const bool is_external_nav_state = ((_vehicle_status.nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1) + && (_vehicle_status.nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8)) + || _vehicle_status.nav_state <= vehicle_status_s::NAVIGATION_STATE_OFFBOARD; - if (is_external_nav_state) { + if (!is_external_nav_state) { // this will cause the configuration handler to publish immediately the next time an internal flight // mode is active _ctrl_configuration_handler.resetLastPublishTime(); diff --git a/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp b/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp index 202dab5d93..2bfe5c0208 100644 --- a/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp +++ b/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp @@ -352,8 +352,8 @@ private: * @param pos_sp_prev previous position setpoint * @param pos_sp_curr current position setpoint */ - void control_auto_path(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, - const position_setpoint_s &pos_sp_curr); + void control_auto_path(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_curr); void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint); diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index ae1a3d224e..e930a4a5ce 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -2014,12 +2014,12 @@ FixedWingModeManager::Run() if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) { _position_setpoint_previous_valid = PX4_ISFINITE(_pos_sp_triplet.previous.lat) - && PX4_ISFINITE(_pos_sp_triplet.previous.lon) - && PX4_ISFINITE(_pos_sp_triplet.previous.alt); + && PX4_ISFINITE(_pos_sp_triplet.previous.lon) + && PX4_ISFINITE(_pos_sp_triplet.previous.alt); _position_setpoint_current_valid = PX4_ISFINITE(_pos_sp_triplet.current.lat) - && PX4_ISFINITE(_pos_sp_triplet.current.lon) - && PX4_ISFINITE(_pos_sp_triplet.current.alt); + && PX4_ISFINITE(_pos_sp_triplet.current.lon) + && PX4_ISFINITE(_pos_sp_triplet.current.alt); _position_setpoint_next_valid = PX4_ISFINITE(_pos_sp_triplet.next.lat) && PX4_ISFINITE(_pos_sp_triplet.next.lon)