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

RTL: use dest.yaw instead of a separate heading_sp

This commit is contained in:
Matthias Grob 2024-02-12 15:12:59 +01:00
parent b19e35ec7c
commit cb03835124
3 changed files with 29 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -928,16 +928,15 @@ MissionBlock::initialize()
_mission_item.origin = ORIGIN_ONBOARD; _mission_item.origin = ORIGIN_ONBOARD;
} }
void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius, void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest,
float heading_sp) const float loiter_radius) const
{ {
item.nav_cmd = NAV_CMD_LOITER_TO_ALT; item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
item.lat = dest.lat; item.lat = dest.lat;
item.lon = dest.lon; item.lon = dest.lon;
item.altitude = dest.alt; item.altitude = dest.alt;
item.altitude_is_relative = false; item.altitude_is_relative = false;
item.yaw = dest.yaw;
item.yaw = heading_sp;
item.acceptance_radius = _navigator->get_acceptance_radius(); item.acceptance_radius = _navigator->get_acceptance_radius();
item.time_inside = 0.0f; item.time_inside = 0.0f;
@ -947,7 +946,7 @@ void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const Destina
} }
void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time, void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time,
float loiter_radius, float heading_sp) const float loiter_radius) const
{ {
const bool autocontinue = (loiter_time > -FLT_EPSILON); const bool autocontinue = (loiter_time > -FLT_EPSILON);
@ -972,8 +971,7 @@ void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const Destinat
item.loiter_radius = loiter_radius; item.loiter_radius = loiter_radius;
} }
void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest, void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest) const
float heading_sp) const
{ {
item.nav_cmd = NAV_CMD_WAYPOINT; item.nav_cmd = NAV_CMD_WAYPOINT;
item.lat = dest.lat; item.lat = dest.lat;
@ -986,17 +984,16 @@ void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const Dest
item.time_inside = 0.f; item.time_inside = 0.f;
item.origin = ORIGIN_ONBOARD; item.origin = ORIGIN_ONBOARD;
item.yaw = heading_sp; item.yaw = dest.yaw;
} }
void MissionBlock::setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, void MissionBlock::setLandMissionItem(mission_item_s &item, const DestinationPosition &dest) const
const float heading_sp) const
{ {
item.nav_cmd = NAV_CMD_LAND; item.nav_cmd = NAV_CMD_LAND;
item.lat = dest.lat; item.lat = dest.lat;
item.lon = dest.lon; item.lon = dest.lon;
item.altitude = dest.alt; item.altitude = dest.alt;
item.yaw = heading_sp; item.yaw = dest.yaw;
item.acceptance_radius = _navigator->get_acceptance_radius(); item.acceptance_radius = _navigator->get_acceptance_radius();
item.time_inside = 0.0f; item.time_inside = 0.0f;
item.autocontinue = true; item.autocontinue = true;

10
src/modules/navigator/mission_block.h
View File

@ -205,16 +205,14 @@ protected:
*/ */
void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode); void set_vtol_transition_item(struct mission_item_s *item, const uint8_t new_mode);
void setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius, void setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius) const;
float heading_sp) const;
void setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time, void setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time,
float loiter_radius, float heading_sp) const; float loiter_radius) const;
void setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest, void setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest) const;
float heading_sp) const;
void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, const float heading_sp) const; void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest) const;
void startPrecLand(uint16_t land_precision); void startPrecLand(uint16_t land_precision);

39
src/modules/navigator/rtl_direct.cpp
View File

@ -186,9 +186,9 @@ void RtlDirect::set_rtl_item()
.lat = _global_pos_sub.get().lat, .lat = _global_pos_sub.get().lat,
.lon = _global_pos_sub.get().lon, .lon = _global_pos_sub.get().lon,
.alt = _rtl_alt, .alt = _rtl_alt,
.yaw = _param_wv_en.get() ? NAN : _navigator->get_local_position()->heading,
}; };
const float heading_sp = _param_wv_en.get() ? NAN : _navigator->get_local_position()->heading; setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius());
setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius(), heading_sp);
_rtl_state = RTLState::MOVE_TO_LOITER; _rtl_state = RTLState::MOVE_TO_LOITER;
break; break;
@ -199,18 +199,18 @@ void RtlDirect::set_rtl_item()
.lat = _land_approach.lat, .lat = _land_approach.lat,
.lon = _land_approach.lon, .lon = _land_approach.lon,
.alt = _rtl_alt, .alt = _rtl_alt,
.yaw = _destination.yaw,
}; };
// For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status // For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status
// can be displayed on groundstation and the WP is accepted once within loiter radius // can be displayed on groundstation and the WP is accepted once within loiter radius
if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
setLoiterHoldMissionItem(_mission_item, dest, 0.f, _land_approach.loiter_radius_m, NAN); dest.yaw = NAN;
setLoiterHoldMissionItem(_mission_item, dest, 0.f, _land_approach.loiter_radius_m);
} else { } else {
// already set final yaw if close to destination and WV is disabled // already set final yaw if close to destination and WV is disabled
const float heading_sp = (is_close_to_destination && !_param_wv_en.get()) ? _destination.yaw : NAN; dest.yaw = (is_close_to_destination && !_param_wv_en.get()) ? _destination.yaw : NAN;
setMoveToPositionMissionItem(_mission_item, dest, heading_sp); setMoveToPositionMissionItem(_mission_item, dest);
} }
_rtl_state = RTLState::LOITER_DOWN; _rtl_state = RTLState::LOITER_DOWN;
@ -223,12 +223,10 @@ void RtlDirect::set_rtl_item()
.lat = _land_approach.lat, .lat = _land_approach.lat,
.lon = _land_approach.lon, .lon = _land_approach.lon,
.alt = loiter_altitude, .alt = loiter_altitude,
.yaw = _destination.yaw, .yaw = !_param_wv_en.get() ? _destination.yaw : NAN, // set final yaw if WV is disabled
}; };
// set final yaw if WV is disabled setLoiterToAltMissionItem(_mission_item, dest, _land_approach.loiter_radius_m);
const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN;
setLoiterToAltMissionItem(_mission_item, dest, _land_approach.loiter_radius_m, heading_sp);
pos_sp_triplet->next.valid = true; pos_sp_triplet->next.valid = true;
pos_sp_triplet->next.lat = _destination.lat; pos_sp_triplet->next.lat = _destination.lat;
@ -252,13 +250,11 @@ void RtlDirect::set_rtl_item()
.lat = _land_approach.lat, .lat = _land_approach.lat,
.lon = _land_approach.lon, .lon = _land_approach.lon,
.alt = loiter_altitude, .alt = loiter_altitude,
.yaw = _destination.yaw, .yaw = !_param_wv_en.get() ? _destination.yaw : NAN, // set final yaw if WV is disabled
}; };
// set final yaw if WV is disabled // set final yaw if WV is disabled
const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN; setLoiterHoldMissionItem(_mission_item, dest, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m);
setLoiterHoldMissionItem(_mission_item, dest, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m,
heading_sp);
if (_param_rtl_land_delay.get() < -FLT_EPSILON) { if (_param_rtl_land_delay.get() < -FLT_EPSILON) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t");
@ -280,8 +276,9 @@ void RtlDirect::set_rtl_item()
DestinationPosition dest{_destination}; DestinationPosition dest{_destination};
dest.alt = loiter_altitude; dest.alt = loiter_altitude;
dest.yaw = NAN;
setMoveToPositionMissionItem(_mission_item, dest, NAN); setMoveToPositionMissionItem(_mission_item, dest);
// Prepare for transition // Prepare for transition
_mission_item.vtol_back_transition = true; _mission_item.vtol_back_transition = true;
@ -310,10 +307,9 @@ void RtlDirect::set_rtl_item()
case RTLState::MOVE_TO_LAND_HOVER: { case RTLState::MOVE_TO_LAND_HOVER: {
DestinationPosition dest{_destination}; DestinationPosition dest{_destination};
dest.alt = loiter_altitude; dest.alt = loiter_altitude;
dest.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if WV is disabled
// set final yaw if WV is disabled setMoveToPositionMissionItem(_mission_item, dest);
const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN;
setMoveToPositionMissionItem(_mission_item, dest, heading_sp);
_navigator->reset_position_setpoint(pos_sp_triplet->previous); _navigator->reset_position_setpoint(pos_sp_triplet->previous);
_rtl_state = RTLState::LAND; _rtl_state = RTLState::LAND;
@ -322,10 +318,9 @@ void RtlDirect::set_rtl_item()
} }
case RTLState::LAND: { case RTLState::LAND: {
DestinationPosition dest{_destination};
// set final yaw if WV is disabled dest.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if WV is disabled
const float heading_sp = !_param_wv_en.get() ? _destination.yaw : NAN; setLandMissionItem(_mission_item, dest);
setLandMissionItem(_mission_item, _destination, heading_sp);
_mission_item.land_precision = _param_rtl_pld_md.get(); _mission_item.land_precision = _param_rtl_pld_md.get();