diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dc53c6928d..228d2da04f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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 diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 19ab9e8732..90e7f86b26 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -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; diff --git a/src/modules/navigator/takeoff.h b/src/modules/navigator/takeoff.h index 6fb5d8ea19..95736f6dbf 100644 --- a/src/modules/navigator/takeoff.h +++ b/src/modules/navigator/takeoff.h @@ -42,6 +42,7 @@ #include "navigator_mode.h" #include "mission_block.h" +#include 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(NAN), static_cast(NAN)}; + float _loiter_altitude_msl{NAN}; };