dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

* 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
parent d872ef87da
commit 7e22b47b85
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 38 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
msg/PositionSetpoint.msg
View File

@ -22,11 +22,7 @@ float32 vz # local velocity setpoint in m/s in NED
float64 lat # latitude, in deg
float64 lon # longitude, in deg
float32 alt # altitude AMSL, in m
float32 yaw # yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw
bool yaw_valid # true if yaw setpoint valid
float32 yawspeed # yawspeed (only for multirotors, in rad/s)
bool yawspeed_valid # true if yawspeed setpoint valid
float32 yaw # yaw (only in hover), in rad [-PI..PI), NaN = leave to flight task
float32 loiter_radius # loiter major axis radius in m
float32 loiter_minor_radius # loiter minor axis radius (used for non-circular loiter shapes) in m
@ -39,5 +35,3 @@ float32 acceptance_radius # navigation acceptance_radius if we're doing waypoi
float32 cruising_speed # the generally desired cruising speed (not a hard constraint)
bool gliding_enabled # commands the vehicle to glide if the capability is available (fixed wing only)
float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint), only has an effect for rover
bool disable_weather_vane # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind

12
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -466,7 +466,7 @@ bool FlightTaskAuto::_evaluateTriplets()
}
// activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane)
_weathervane.setNavigatorForceDisabled(_sub_triplet_setpoint.get().current.disable_weather_vane);
_weathervane.setNavigatorForceDisabled(PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw));
// Calculate the current vehicle state and check if it has updated.
State previous_state = _current_state;
@ -481,7 +481,7 @@ bool FlightTaskAuto::_evaluateTriplets()
_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
_triplet_next_wp,
_sub_triplet_setpoint.get().next.yaw,
_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN,
(float)NAN,
_weathervane.isActive(), _sub_triplet_setpoint.get().current.type);
_obstacle_avoidance.checkAvoidanceProgress(
_position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt));
@ -509,13 +509,7 @@ bool FlightTaskAuto::_evaluateTriplets()
_yaw_setpoint = NAN;
_yawspeed_setpoint = 0.f;
} else if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane)
&& _sub_triplet_setpoint.get().current.yaw_valid) {
// Use the yaw computed in Navigator except during takeoff because
// Navigator is not handling the yaw reset properly.
// But: use if from Navigator during takeoff if disable_weather_vane is true,
// because we're then aligning to the transition waypoint.
// TODO: fix in navigator
} else if (PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw)) {
_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
_yawspeed_setpoint = NAN;

17
src/modules/navigator/mission.cpp
View File

@ -322,8 +322,7 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
_mission_item.lat = _global_pos_sub.get().lat;
_mission_item.lon = _global_pos_sub.get().lon;
/* hold heading for takeoff items */
_mission_item.yaw = _navigator->get_local_position()->heading;
_mission_item.yaw = NAN; // FlightTaskAuto handles yaw directly
_mission_item.altitude = _mission_init_climb_altitude_amsl;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
@ -366,9 +365,6 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
!_land_detected_sub.get().landed) {
/* disable weathervane before front transition for allowing yaw to align */
pos_sp_triplet->current.disable_weather_vane = true;
/* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */
_mission_item.yaw = get_bearing_to_next_waypoint(
_global_pos_sub.get().lat, _global_pos_sub.get().lon,
@ -389,16 +385,13 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
!_land_detected_sub.get().landed) {
/* re-enable weather vane again after alignment */
pos_sp_triplet->current.disable_weather_vane = false;
/* check if the vtol_takeoff waypoint is on top of us */
if (do_need_move_to_takeoff()) {
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF;
}
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
_mission_item.yaw = _navigator->get_local_position()->heading;
_mission_item.yaw = NAN;
// keep current setpoints (FW position controller generates wp to track during transition)
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
@ -428,9 +421,6 @@ void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_ite
&& !_land_detected_sub.get().landed
&& (num_found_items > 0u)) {
/* disable weathervane before front transition for allowing yaw to align */
pos_sp_triplet->current.disable_weather_vane = true;
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING;
set_align_mission_item(&_mission_item, &next_mission_items[0u]);
@ -445,9 +435,6 @@ void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_ite
new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT;
/* re-enable weather vane again after alignment */
pos_sp_triplet->current.disable_weather_vane = false;
pos_sp_triplet->previous = pos_sp_triplet->current;
// keep current setpoints (FW position controller generates wp to track during transition)
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;

9
src/modules/navigator/mission_base.cpp
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;
}
}

34
src/modules/navigator/mission_block.cpp
View File

@ -411,7 +411,7 @@ MissionBlock::is_mission_item_reached_or_completed()
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))
&& _navigator->get_yaw_to_be_accepted(_mission_item.yaw)
&& _navigator->get_local_position()->heading_good_for_control) {
const float yaw_err = wrap_pi(_mission_item.yaw - _navigator->get_local_position()->heading);
@ -423,14 +423,6 @@ MissionBlock::is_mission_item_reached_or_completed()
_waypoint_yaw_reached = true;
}
// Always accept yaw during takeoff
// TODO: Ideally Navigator would handle a yaw reset and adjust its yaw setpoint, making the
// following no longer necessary.
// FlightTaskAuto is currently also ignoring the yaw setpoint during takeoff and thus "handling" it.
if (_mission_item.nav_cmd == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
_waypoint_yaw_reached = true;
}
/* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */
if (!_waypoint_yaw_reached && _mission_item.force_heading &&
(_navigator->get_yaw_timeout() >= FLT_EPSILON) &&
@ -668,7 +660,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
sp->lon = item.lon;
sp->alt = get_absolute_altitude_for_item(item);
sp->yaw = item.yaw;
sp->yaw_valid = PX4_ISFINITE(item.yaw);
sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) :
_navigator->get_loiter_radius();
sp->loiter_direction_counter_clockwise = item.loiter_radius < 0;
@ -704,6 +695,10 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
} else {
sp->type = position_setpoint_s::SETPOINT_TYPE_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.
sp->yaw = NAN;
}
break;
@ -746,6 +741,7 @@ MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *it
item->altitude = pos_sp_triplet->current.alt;
item->loiter_radius = pos_sp_triplet->current.loiter_direction_counter_clockwise ?
-pos_sp_triplet->current.loiter_radius : pos_sp_triplet->current.loiter_radius;
item->yaw = pos_sp_triplet->current.yaw;
}
void
@ -765,8 +761,8 @@ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
}
item->altitude = loiter_altitude_amsl;
item->loiter_radius = _navigator->get_loiter_radius();
item->yaw = NAN;
}
void
@ -774,10 +770,11 @@ MissionBlock::setLoiterItemFromCurrentPositionWithBreaking(struct mission_item_s
{
setLoiterItemCommonFields(item);
_navigator->calculate_breaking_stop(item->lat, item->lon, item->yaw);
_navigator->calculate_breaking_stop(item->lat, item->lon);
item->altitude = _navigator->get_global_position()->alt;
item->loiter_radius = _navigator->get_loiter_radius();
item->yaw = NAN;
}
void
@ -801,7 +798,7 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude)
/* use current position */
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->yaw = _navigator->get_local_position()->heading;
item->yaw = NAN;
item->altitude = abs_altitude;
item->altitude_is_relative = false;
@ -831,7 +828,7 @@ MissionBlock::set_land_item(struct mission_item_s *item)
// set land item to current position
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->yaw = _navigator->get_local_position()->heading;
item->yaw = NAN;
item->altitude = 0;
item->altitude_is_relative = false;
@ -863,14 +860,7 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_
{
item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
item->params[0] = (float) new_mode;
item->params[1] = 0.0f;
// Keep yaw from previous mission item if valid, as that is containing the transition heading.
// If not valid use current yaw as yaw setpoint
if (!PX4_ISFINITE(item->yaw)) {
item->yaw = _navigator->get_local_position()->heading; // ideally that would be course and not heading
}
item->params[1] = 0.0f; // not immediate transition
item->autocontinue = true;
}

9
src/modules/navigator/navigator.h
View File

@ -239,14 +239,13 @@ public:
void set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; }
/**
* Get the yaw acceptance given the current mission item
* Get if the yaw acceptance is required at the current mission item
*
* @param mission_item_yaw the yaw to use in case the controller-derived radius is finite
*
* @return the yaw at which the next waypoint should be used or NaN if the yaw at a waypoint
* should be ignored
* @return true if the yaw acceptance is required, false if not required
*/
float get_yaw_acceptance(float mission_item_yaw);
bool get_yaw_to_be_accepted(float mission_item_yaw);
orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; }
@ -279,7 +278,7 @@ public:
void release_gimbal_control();
void set_gimbal_neutral();
void calculate_breaking_stop(double &lat, double &lon, float &yaw);
void calculate_breaking_stop(double &lat, double &lon);
void stop_capturing_images();
void disable_camera_trigger();

36
src/modules/navigator/navigator_main.cpp
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)

1
src/modules/navigator/takeoff.cpp
View File

@ -130,7 +130,6 @@ Takeoff::set_takeoff_position()
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.yaw_valid = true;
pos_sp_triplet->next.valid = false;
if (rep->current.valid) {

2
src/modules/navigator/vtol_takeoff.cpp
View File

@ -75,7 +75,6 @@ VtolTakeoff::on_active()
_mission_item.lon, _loiter_location(0), _loiter_location(1)));
_mission_item.force_heading = true;
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->current.disable_weather_vane = true;
pos_sp_triplet->current.cruising_speed = -1.f;
_navigator->set_position_setpoint_triplet_updated();
@ -182,7 +181,6 @@ VtolTakeoff::set_takeoff_position()
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.yaw_valid = true;
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();