fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 08:13:34 +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/navigator_main.cpp
+14 -22
View File
@@ -309,11 +309,9 @@ void Navigator::run()
// Go on and check which changes had been requested
if (PX4_ISFINITE(cmd.param4)) {
rep->current.yaw = cmd.param4;
rep->current.yaw_valid = true;
} else {
rep->current.yaw = NAN;
rep->current.yaw_valid = false;
}
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
@@ -348,8 +346,7 @@ void Navigator::run()
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
calculate_breaking_stop(rep->current.lat, rep->current.lon, rep->current.yaw);
rep->current.yaw_valid = true;
calculate_breaking_stop(rep->current.lat, rep->current.lon);
} else {
// For fixedwings we can use the current vehicle's position to define the loiter point
@@ -446,7 +443,6 @@ void Navigator::run()
rep->current.cruising_throttle = get_cruising_throttle();
rep->current.acceptance_radius = get_acceptance_radius();
rep->current.yaw = NAN;
rep->current.yaw_valid = false;
// Position is not changing, thus we keep the setpoint
rep->current.lat = PX4_ISFINITE(curr->current.lat) ? curr->current.lat : get_global_position()->lat;
@@ -458,8 +454,7 @@ void Navigator::run()
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
calculate_breaking_stop(rep->current.lat, rep->current.lon, rep->current.yaw);
rep->current.yaw_valid = true;
calculate_breaking_stop(rep->current.lat, rep->current.lon);
}
if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) {
@@ -599,19 +594,18 @@ void Navigator::run()
rep->current.cruising_speed = -1.f; // reset to default
if (home_global_position_valid()) {
// Only set yaw if we know the true heading
// We assume that the heading is valid when the global position is valid because true heading
// is required to fuse NE (e.g.: GNSS) data. // TODO: we should be more explicit here
rep->current.yaw = cmd.param4;
rep->previous.valid = true;
rep->previous.timestamp = hrt_absolute_time();
} else {
rep->current.yaw = get_local_position()->heading;
rep->previous.valid = false;
}
// Don't set a yaw setpoint for takeoff, as Navigator doesn't handle the yaw reset.
// The yaw setpoint generation is handled by FlightTaskAuto.
rep->current.yaw = NAN;
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
rep->current.lat = cmd.param5;
rep->current.lon = cmd.param6;
@@ -1039,8 +1033,7 @@ void Navigator::geofence_breach_check()
}
rep->current.timestamp = hrt_absolute_time();
rep->current.yaw = get_local_position()->heading;
rep->current.yaw_valid = true;
rep->current.yaw = NAN;
rep->current.lat = loiter_latitude;
rep->current.lon = loiter_longitude;
rep->current.alt = loiter_altitude_amsl;
@@ -1157,13 +1150,13 @@ void Navigator::reset_position_setpoint(position_setpoint_s &sp)
sp.timestamp = hrt_absolute_time();
sp.lat = static_cast<double>(NAN);
sp.lon = static_cast<double>(NAN);
sp.yaw = NAN;
sp.loiter_radius = get_loiter_radius();
sp.acceptance_radius = get_default_acceptance_radius();
sp.cruising_speed = get_cruising_speed();
sp.cruising_throttle = get_cruising_throttle();
sp.valid = false;
sp.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
sp.disable_weather_vane = false;
sp.loiter_direction_counter_clockwise = false;
}
@@ -1193,7 +1186,7 @@ float Navigator::get_acceptance_radius()
return acceptance_radius;
}
float Navigator::get_yaw_acceptance(float mission_item_yaw)
bool Navigator::get_yaw_to_be_accepted(float mission_item_yaw)
{
float yaw = mission_item_yaw;
@@ -1205,7 +1198,7 @@ float Navigator::get_yaw_acceptance(float mission_item_yaw)
yaw = pos_ctrl_status.yaw_acceptance;
}
return yaw;
return PX4_ISFINITE(yaw);
}
void Navigator::load_fence_from_file(const char *filename)
@@ -1470,21 +1463,20 @@ bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos)
return true;
}
void Navigator::calculate_breaking_stop(double &lat, double &lon, float &yaw)
void Navigator::calculate_breaking_stop(double &lat, double &lon)
{
// For multirotors we need to account for the braking distance, otherwise the vehicle will overshoot and go back
float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx);
const float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx);
// predict braking distance
const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
float multirotor_braking_distance = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs,
_param_mpc_jerk_auto, _param_mpc_acc_hor, 0.6f * _param_mpc_jerk_auto);
const float multirotor_braking_distance = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs,
_param_mpc_jerk_auto, _param_mpc_acc_hor, 0.6f * _param_mpc_jerk_auto);
waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, course_over_ground,
multirotor_braking_distance, &lat, &lon);
yaw = get_local_position()->heading;
}
void Navigator::mode_completed(uint8_t nav_state, uint8_t result)