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

FixedWingPositionController: Set l1 variables to NAN if no l1 guidance

Browse Source 
took place

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2020-08-06 17:07:55 +03:00 committed by Silvan Fuhrer
parent ae5d3103f4
commit 8f8304f31e
1 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -470,10 +470,19 @@ FixedwingPositionControl::status_publish()
pos_ctrl_status.nav_roll = _att_sp.roll_body;
pos_ctrl_status.nav_pitch = _att_sp.pitch_body;
pos_ctrl_status.nav_bearing = _l1_control.nav_bearing();
pos_ctrl_status.target_bearing = _l1_control.target_bearing();
pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error();
if (_l1_control.has_guidance_updated()) {
pos_ctrl_status.nav_bearing = _l1_control.nav_bearing();
pos_ctrl_status.target_bearing = _l1_control.target_bearing();
pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error();
} else {
pos_ctrl_status.nav_bearing = NAN;
pos_ctrl_status.target_bearing = NAN;
pos_ctrl_status.xtrack_error = NAN;
}
pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
@ -1865,6 +1874,8 @@ FixedwingPositionControl::Run()
}
}
_l1_control.reset_has_guidance_updated();
}
perf_end(_loop_perf);