fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 22:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator/FlightTaskAuto yaw handling improvements/simplifications (#22532)

Browse Source 
* PositionSetpoint: remove yaw_valid field

* Navigator: set yaw setpoint to NAN for Takeoff

Don't set a yaw setpoint for takeoff, as Navigator doesn't handle the yaw reset.
The yaw setpoint generation is handled by FlightTaskAuto.

* PositionSetpoint.msg: remove disable_weather_vane and instead only use the yaw field

Strictly follow the concept that if the position_setpoint.yaw is set, then
follow it the controller, and otherwise let the controller set it as it
thinks it's best.

* Navigator: remove logic that sets yaw to be accepted in TAKEOFF

No longer needed as during Takeoff we anyway don't set a yaw setpoint.

* PositionSetpoint.msg: remove yawspeed_valid

* PositionSetpoint.msg: remove yawspeed

* Navigator: set yaw setpoint to NAN instead of current

In set_takeoff and set_land_item, as well as for VTOL transition.
The flight tasks then set the yaw corresponding to the current yaw.

* Navigator: change get_yaw_acceptance into a bool

* PositionSetpoint.msg: improve comment for yaw

* MissionBlock: remove unnecessary code from set_vtol_transition_item

* Navigator: clean up calculate_breaking_stop(), set yaw to NAN

* Navigator: set yaw to NAN in variouls places where not specifc setpoint is desired

* Navigator: set yaw to NAN in reset_position_setpoint()

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Silvan Fuhrer
2023-12-21 16:50:13 +01:00
committed by GitHub
co-authored by Matthias Grob
parent d872ef87da
commit 7e22b47b85
9 changed files with 38 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_base.cpp
+2 -7
View File
@@ -654,9 +654,6 @@ bool MissionBase::position_setpoint_equal(const position_setpoint_s *p1, const p
(fabs(p1->lon - p2->lon) < DBL_EPSILON) &&
(fabsf(p1->alt - p2->alt) < FLT_EPSILON) &&
((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) &&
(p1->yaw_valid == p2->yaw_valid) &&
(fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) &&
(p1->yawspeed_valid == p2->yawspeed_valid) &&
(fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) &&
(p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) &&
(fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) &&
@@ -773,13 +770,11 @@ MissionBase::heading_sp_update()
_mission_item.yaw = yaw;
pos_sp_triplet->current.yaw = _mission_item.yaw;
pos_sp_triplet->current.yaw_valid = true;
} else {
if (!pos_sp_triplet->current.yaw_valid) {
_mission_item.yaw = _navigator->get_local_position()->heading;
if (!PX4_ISFINITE(pos_sp_triplet->current.yaw)) {
_mission_item.yaw = NAN;
pos_sp_triplet->current.yaw = _mission_item.yaw;
pos_sp_triplet->current.yaw_valid = true;
}
}