fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-07 06:18:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Only run fixedwing guidance controller in external modes

Browse Source
This commit is contained in:
JaeyoungLim
2026-01-10 07:14:03 -08:00
parent 87574986d5
commit 870cd34d17
3 changed files with 11 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp
+5 -4
View File
@@ -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();
src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp
+2 -2
View File
@@ -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 &current_waypoint);
src/modules/fw_mode_manager/FixedWingModeManager.cpp
+4 -4
View File
@@ -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)