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

Navigator: Takeoff mode: add state 2-state takeoff for fixed-wing

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2025-06-18 13:52:10 +02:00
parent e9e5c45bf3
commit 782e510105
3 changed files with 124 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/navigator/navigator_main.cpp
View File

@ -640,6 +640,10 @@ void Navigator::run()
rep->next.valid = false;
// after the straight climbout the vehicle will establish on a loiter at this position
_takeoff.setLoiterPosition(matrix::Vector2d(cmd.param5, cmd.param6));
_takeoff.setLoiterAltitudeAmsl(cmd.param7);
// CMD_NAV_TAKEOFF is acknowledged by commander
#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF

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

@ -54,41 +54,123 @@ Takeoff::on_activation()
// reset cruising speed to default
_navigator->reset_cruising_speed();
_fw_takeoff_state = fw_takeoff_state::CLIMBOUT; // only used for fixed-wing takeoff
}
void
Takeoff::on_active()
{
struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
if (rep->current.valid) {
// reset the position
set_takeoff_position();
switch (_fw_takeoff_state) {
case fw_takeoff_state::CLIMBOUT: {
if (_navigator->get_global_position()->alt >= _loiter_altitude_msl) {
} else if (is_mission_item_reached_or_completed() && !_navigator->get_mission_result()->finished) {
_navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated();
_navigator->mode_completed(getNavigatorStateId());
setLoiterItemCommonFields(&_mission_item);
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
// set loiter item so position controllers stop doing takeoff logic
if (_navigator->get_land_detected()->landed) {
_mission_item.nav_cmd = NAV_CMD_IDLE;
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
} else {
if (pos_sp_triplet->current.valid
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
// we need the vehicle to loiter indefinitely but also we want this mission item to be reached as soon
// as the loiter is established. therefore, set a small loiter time so that the mission item will be reached quickly,
// however it will just continue loitering as there is no next mission item
_mission_item.time_inside = 1.f;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.altitude = _loiter_altitude_msl;
} else {
setLoiterItemFromCurrentPosition(&_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->current.lat = _loiter_position_lat_lon(0) > DBL_EPSILON ?
_loiter_position_lat_lon(0) : _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _loiter_position_lat_lon(1) > DBL_EPSILON ?
_loiter_position_lat_lon(1) : _navigator->get_global_position()->lon;
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
pos_sp_triplet->current.cruising_speed = -1.f;
pos_sp_triplet->current.cruising_throttle = -1.f;
_mission_item.lat = pos_sp_triplet->current.lat;
_mission_item.lon = pos_sp_triplet->current.lon;
_navigator->set_position_setpoint_triplet_updated();
reset_mission_item_reached();
_fw_takeoff_state = fw_takeoff_state::GO_TO_LOITER;
}
break;
}
case fw_takeoff_state::GO_TO_LOITER: {
const bool navigation_valid = _navigator->get_local_position()->xy_valid;
bool lateral_acceptance_reached = true;
if (navigation_valid) {
// only consider lateral acceptance if position estimation is valid
const float distance_to_loiter = get_distance_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon, _mission_item.lat, _mission_item.lon);
const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius)
&& fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) :
_navigator->get_loiter_radius();
lateral_acceptance_reached = distance_to_loiter < _navigator->get_acceptance_radius() + mission_item_loiter_radius_abs;
}
const bool vertical_acceptance_reached = _navigator->get_global_position()->alt >= _loiter_altitude_msl -
_navigator->get_altitude_acceptance_radius();
if (lateral_acceptance_reached && vertical_acceptance_reached) {
position_setpoint_triplet_s *reposition_triplet = _navigator->get_reposition_triplet();
_navigator->reset_position_setpoint(reposition_triplet->previous);
_navigator->reset_position_setpoint(reposition_triplet->current);
_navigator->reset_position_setpoint(reposition_triplet->next);
// the FW takeoff mode is completed, exit to Hold (handled by Commander)
_navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated();
_navigator->mode_completed(getNavigatorStateId());
_loiter_altitude_msl = NAN; // reset for next takeoff command
}
break;
}
default:
break;
}
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
} else { // rotary-wing takeoff
struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
_navigator->set_position_setpoint_triplet_updated();
if (rep->current.valid) {
// reset the position
set_takeoff_position();
} else if (is_mission_item_reached_or_completed() && !_navigator->get_mission_result()->finished) {
_navigator->get_mission_result()->finished = true;
_navigator->set_mission_result_updated();
_navigator->mode_completed(getNavigatorStateId());
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// set loiter item so position controllers stop doing takeoff logic
if (_navigator->get_land_detected()->landed) {
_mission_item.nav_cmd = NAV_CMD_IDLE;
} else {
if (pos_sp_triplet->current.valid
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
}
}
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
}
}
}
@ -120,6 +202,13 @@ Takeoff::set_takeoff_position()
"Already higher than takeoff altitude (not descending)");
}
if (!PX4_ISFINITE(_loiter_altitude_msl)) {
if (_navigator->get_loiter_min_alt() > FLT_EPSILON) {
_loiter_altitude_msl = math::max(_loiter_altitude_msl, takeoff_altitude_amsl + _navigator->get_loiter_min_alt());
}
}
// set current mission item to takeoff
set_takeoff_item(&_mission_item, takeoff_altitude_amsl);
_navigator->get_mission_result()->finished = false;

11
src/modules/navigator/takeoff.h
View File

@ -42,6 +42,7 @@
#include "navigator_mode.h"
#include "mission_block.h"
#include <lib/mathlib/mathlib.h>
class Takeoff : public MissionBlock
{
@ -52,7 +53,17 @@ public:
void on_activation() override;
void on_active() override;
void setLoiterPosition(matrix::Vector2d loiter_location) { _loiter_position_lat_lon = loiter_location; }
void setLoiterAltitudeAmsl(const float height_m) { _loiter_altitude_msl = height_m; }
private:
enum class fw_takeoff_state {
CLIMBOUT = 0,
GO_TO_LOITER
} _fw_takeoff_state;
void set_takeoff_position();
matrix::Vector2d _loiter_position_lat_lon{static_cast<double>(NAN), static_cast<double>(NAN)};
float _loiter_altitude_msl{NAN};
};