fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 01:13:35 +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 void
FixedWingGuidanceControl::control_auto_path(const float control_interval, const Vector2d &curr_pos, 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; 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*/ /* 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) 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_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 // this will cause the configuration handler to publish immediately the next time an internal flight
// mode is active // mode is active
_ctrl_configuration_handler.resetLastPublishTime(); _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_prev previous position setpoint
* @param pos_sp_curr current 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, void control_auto_path(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_curr); const position_setpoint_s &pos_sp_curr);
void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint); 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)) { if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
_position_setpoint_previous_valid = PX4_ISFINITE(_pos_sp_triplet.previous.lat) _position_setpoint_previous_valid = PX4_ISFINITE(_pos_sp_triplet.previous.lat)
&& PX4_ISFINITE(_pos_sp_triplet.previous.lon) && PX4_ISFINITE(_pos_sp_triplet.previous.lon)
&& PX4_ISFINITE(_pos_sp_triplet.previous.alt); && PX4_ISFINITE(_pos_sp_triplet.previous.alt);
_position_setpoint_current_valid = PX4_ISFINITE(_pos_sp_triplet.current.lat) _position_setpoint_current_valid = PX4_ISFINITE(_pos_sp_triplet.current.lat)
&& PX4_ISFINITE(_pos_sp_triplet.current.lon) && PX4_ISFINITE(_pos_sp_triplet.current.lon)
&& PX4_ISFINITE(_pos_sp_triplet.current.alt); && PX4_ISFINITE(_pos_sp_triplet.current.alt);
_position_setpoint_next_valid = PX4_ISFINITE(_pos_sp_triplet.next.lat) _position_setpoint_next_valid = PX4_ISFINITE(_pos_sp_triplet.next.lat)
&& PX4_ISFINITE(_pos_sp_triplet.next.lon) && PX4_ISFINITE(_pos_sp_triplet.next.lon)