diff --git a/msg/VehicleStatus.msg b/msg/VehicleStatus.msg index 6756da8129..3a2fdf02bc 100644 --- a/msg/VehicleStatus.msg +++ b/msg/VehicleStatus.msg @@ -42,7 +42,7 @@ uint8 NAVIGATION_STATE_FREE5 = 7 uint8 NAVIGATION_STATE_FREE4 = 8 uint8 NAVIGATION_STATE_FREE3 = 9 uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode -uint8 NAVIGATION_STATE_FREE2 = 11 +uint8 NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV = 11 # Takeoff without navigation (no lateral position control) uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 diff --git a/src/drivers/osd/atxxxx/atxxxx.cpp b/src/drivers/osd/atxxxx/atxxxx.cpp index a5acabd391..889127b982 100644 --- a/src/drivers/osd/atxxxx/atxxxx.cpp +++ b/src/drivers/osd/atxxxx/atxxxx.cpp @@ -405,6 +405,7 @@ OSDatxxxx::get_flight_mode(uint8_t nav_state) case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV: flight_mode = "AUTO"; break; diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 3ea6c0ac27..cafebef942 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -294,6 +294,7 @@ void CrsfRc::Run() case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV: flight_mode = "Auto"; break; diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 60286e0f40..eeff96b52d 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -159,6 +159,7 @@ bool CRSFTelemetry::send_flight_mode() case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV: flight_mode = "Auto"; break; diff --git a/src/lib/modes/ui.hpp b/src/lib/modes/ui.hpp index 9e4156ae00..5cd89c599d 100644 --- a/src/lib/modes/ui.hpp +++ b/src/lib/modes/ui.hpp @@ -61,7 +61,8 @@ static inline uint32_t getValidNavStates() (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) | (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND) | (1u << vehicle_status_s::NAVIGATION_STATE_ORBIT) | - (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF); + (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) | + (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV); static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "update valid nav states"); } @@ -78,7 +79,7 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { "8: unallocated", "9: unallocated", "Acro", - "11: UNUSED", + "Takeoff w/o Navigation", "Descend", "Termination", "Offboard", diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a151b15989..b230b7b3cf 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -409,6 +409,10 @@ int Commander::custom_command(int argc, char *argv[]) send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LAND); + } else if (!strcmp(argv[1], "auto:takeoff_no_nav")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF_NO_NAV); + } else if (!strcmp(argv[1], "auto:precland")) { send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND); @@ -842,6 +846,10 @@ Commander::handle_command(const vehicle_command_s &cmd) desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; break; + case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF_NO_NAV: + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV; + break; + case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8: desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1); break; @@ -2017,6 +2025,11 @@ void Commander::checkForMissionUpdate() } else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { // Transition to loiter when the mission is cleared and/or finished, and we are still in mission mode. _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER); + + } else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV) { + // Transition to loiter when the takeoff is completed (force into the Loiter, if not possible do failsafe). + _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, ModeChangeSource::ModeExecutor, false, + true); } } } diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index fbebc7b93d..9bb82003f7 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -106,6 +106,15 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode.flag_control_allocation_enabled = true; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV: + vehicle_control_mode.flag_control_auto_enabled = true; + vehicle_control_mode.flag_control_altitude_enabled = true; + vehicle_control_mode.flag_control_climb_rate_enabled = true; + vehicle_control_mode.flag_control_attitude_enabled = true; + vehicle_control_mode.flag_control_rates_enabled = true; + vehicle_control_mode.flag_control_allocation_enabled = true; + break; + case vehicle_status_s::NAVIGATION_STATE_DESCEND: vehicle_control_mode.flag_control_auto_enabled = true; vehicle_control_mode.flag_control_climb_rate_enabled = true; diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 32eb5b1e81..f23e61e32d 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -95,7 +95,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_attitude); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position); - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position); + // setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_alt); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_wind_and_flight_time_compliance); @@ -112,6 +112,11 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) setRequirement(vehicle_status_s::NAVIGATION_STATE_ACRO, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_ACRO, flags.mode_req_manual_control); + // NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV, flags.mode_req_local_alt); + // NAVIGATION_STATE_DESCEND setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_attitude); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 534d62807f..2e856a2352 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -424,7 +424,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, const bool rc_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Offboard); const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV) && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold); const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard || rc_loss_ignored_takeoff || ignore_link_failsafe || _manual_control_lost_at_arming; diff --git a/src/modules/commander/module.yaml b/src/modules/commander/module.yaml index d2113d9359..2a64ea5041 100644 --- a/src/modules/commander/module.yaml +++ b/src/modules/commander/module.yaml @@ -38,6 +38,7 @@ parameters: 8: Stabilized 12: Follow Me 13: Precision Land + 16: Takeoff w/o navigation 100: External Mode 1 101: External Mode 2 102: External Mode 3 diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e4936982ff..ceb9e655d7 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -64,6 +64,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05 PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND, + PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF_NO_NAV, PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF, PX4_CUSTOM_SUB_MODE_EXTERNAL1, PX4_CUSTOM_SUB_MODE_EXTERNAL2, @@ -140,6 +141,11 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state) custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF_NO_NAV; + break; + case vehicle_status_s::NAVIGATION_STATE_DESCEND: custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index ff7e110cf3..f0a680cd50 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -755,6 +755,17 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) _control_mode_current = FW_POSCTRL_MODE_AUTO; } + } else if (_control_mode.flag_control_auto_enabled + && _control_mode.flag_control_altitude_enabled + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + _control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV; + + if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV && !_landed) { + // skip takeoff detection when switching from any other mode, auto or manual, + // while already in air. + _skipping_takeoff_detection = true; + } + } else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_climb_rate_enabled && _control_mode.flag_armed // only enter this modes if armed, as pure failsafe modes @@ -1728,6 +1739,117 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } } +void +FixedwingPositionControl::control_auto_takeoff_no_nav(const hrt_abstime &now, const float control_interval, + const float current_setpoint_altitude_amsl) +{ + if (!_control_mode.flag_armed) { + reset_takeoff_state(); + } + + // for now taking current position setpoint altitude as clearance altitude. this is the altitude we need to + // clear all occlusions in the takeoff path + const float clearance_altitude_amsl = current_setpoint_altitude_amsl; + + // set the altitude to something above the clearance altitude to ensure the vehicle climbs past the value + // (navigator will accept the takeoff as complete once crossing the clearance altitude) + const float altitude_setpoint_amsl = clearance_altitude_amsl + kClearanceAltitudeBuffer; + + const float takeoff_airspeed = (_param_fw_tko_airspd.get() > FLT_EPSILON) ? _param_fw_tko_airspd.get() : + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); + + float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); + + if (takeoff_airspeed < adjusted_min_airspeed) { + // adjust underspeed detection bounds for takeoff airspeed + _tecs.set_equivalent_airspeed_min(takeoff_airspeed); + adjusted_min_airspeed = takeoff_airspeed; + } + + const bool is_low_height = checkLowHeightConditions(); + + float pitch_body_sp = radians(_takeoff_pitch_min.get()); + + if (_runway_takeoff.runwayTakeoffEnabled()) { + // not supported // TODO add user warning (event) and exit/thrust 0 + + } else { + /* Perform launch detection */ + if (!_skipping_takeoff_detection && _param_fw_laun_detcn_on.get() && + _launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) { + + if (_control_mode.flag_armed) { + /* Perform launch detection */ + + /* Detect launch using body X (forward) acceleration */ + _launchDetector.update(control_interval, _body_acceleration_x); + } + + } else { + /* no takeoff detection --> fly */ + _launchDetector.forceSetFlyState(); + } + + if (!_launch_detected && _launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH) { + _launch_detected = true; + _airspeed_slew_rate_controller.setForcedValue(takeoff_airspeed); + } + + /* Set control values depending on the detection state */ + if (_launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH) { + /* Launch has been detected, hence we have to control the plane. */ + + const Vector2f ground_speed(0.f, 0.f); // TODO remove + + float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed, ground_speed, + true); + + const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? + _param_fw_thr_idle.get() : _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; + + tecs_update_pitch_throttle(control_interval, + altitude_setpoint_amsl, + target_airspeed, + radians(_takeoff_pitch_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), + max_takeoff_throttle, + _param_sinkrate_target.get(), + _performance_model.getMaximumClimbRate(_air_density), + is_low_height, + disable_underspeed_handling); + + pitch_body_sp = get_tecs_pitch(); + + } else { + /* Tell the attitude controller to stop integrating while we are waiting for the launch */ + _att_sp.reset_integral = true; + } + + if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) { + // explicitly set idle throttle until motors are enabled + _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); + + } else { + _att_sp.thrust_body[0] = get_tecs_thrust(); + } + + const float roll_body = 0.f; + const float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw (if available) + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body_sp, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + + launch_detection_status_s launch_detection_status; + launch_detection_status.timestamp = now; + launch_detection_status.launch_detection_state = _launchDetector.getLaunchDetected(); + _launch_detection_status_pub.publish(launch_detection_status); + } + + _flaps_setpoint = _param_fw_flaps_to_scl.get(); +} + void FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const float control_interval, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) @@ -2633,7 +2755,8 @@ FixedwingPositionControl::Run() reset_landing_state(); } - if (_control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF) { + if (_control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF + && _control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV) { reset_takeoff_state(); } @@ -2678,6 +2801,11 @@ FixedwingPositionControl::Run() break; } + case FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV: { + control_auto_takeoff_no_nav(_local_pos.timestamp, control_interval, _pos_sp_triplet.current.alt); + break; + } + case FW_POSCTRL_MODE_MANUAL_POSITION: { control_manual_position(control_interval, curr_pos, ground_speed); break; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 5600e71fcd..340ef448d5 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -255,6 +255,7 @@ private: FW_POSCTRL_MODE_AUTO_ALTITUDE, FW_POSCTRL_MODE_AUTO_CLIMBRATE, FW_POSCTRL_MODE_AUTO_TAKEOFF, + FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV, FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT, FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR, FW_POSCTRL_MODE_AUTO_PATH, @@ -658,6 +659,16 @@ private: void control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &global_position, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr); + /** + * @brief Controls automatic takeoff without navigation. + * + * @param now Current system time [us] + * @param control_interval Time since last position control call [s] + * @param altitude_setpoint_amsl Altitude setpoint, AMSL [m] + */ + void control_auto_takeoff_no_nav(const hrt_abstime &now, const float control_interval, + const float altitude_setpoint_amsl); + /** * @brief Controls automatic landing with straight approach. * diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 2c98f108a0..2b4630bdda 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -571,6 +571,7 @@ int8_t ManualControl::navStateFromParam(int32_t param_value) case 13: return vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; case 14: return vehicle_status_s::NAVIGATION_STATE_ORBIT; case 15: return vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF; + case 16: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV; case 100: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL1; case 101: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL2; diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index c55cae3f07..14327a34c3 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -47,6 +47,7 @@ set(NAVIGATOR_SOURCES rtl_mission_fast.cpp rtl_mission_fast_reverse.cpp takeoff.cpp + takeoff_no_nav.cpp land.cpp precland.cpp mission_feasibility_checker.cpp diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 7fece40a4e..9088bc70be 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -49,6 +49,7 @@ #include "navigator_mode.h" #include "rtl.h" #include "takeoff.h" +#include "takeoff_no_nav.h" #if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF #include "vtol_takeoff.h" #endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF @@ -92,7 +93,7 @@ using namespace time_literals; /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 8 +#define NAVIGATOR_MODE_ARRAY_SIZE 9 class Navigator : public ModuleBase, public ModuleParams { @@ -362,6 +363,7 @@ private: #if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF VtolTakeoff _vtol_takeoff; /**< class for handling VEHICLE_CMD_NAV_VTOL_TAKEOFF command */ #endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF + TakeoffNoNav _takeoff_no_nav; /**< class for handling takeoff commands without navigation */ Land _land; /**< class for handling land commands */ PrecLand _precland; /**< class for handling precision land commands */ RTL _rtl; /**< class that handles RTL */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 807ff1284d..1d3228fc97 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -79,6 +79,7 @@ Navigator::Navigator() : #if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF _vtol_takeoff(this), #endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF + _takeoff_no_nav(this), _land(this), _precland(this), _rtl(this) @@ -92,6 +93,8 @@ Navigator::Navigator() : _navigation_mode_array[5] = &_precland; #if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF _navigation_mode_array[6] = &_vtol_takeoff; + _navigation_mode_array[7] = &_takeoff_no_nav; + #endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF /* iterate through navigation modes and initialize _mission_item for each */ @@ -806,6 +809,11 @@ void Navigator::run() break; #endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV: + _pos_sp_triplet_published_invalid_once = false; + navigation_mode_new = &_takeoff_no_nav; + break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_land; diff --git a/src/modules/navigator/takeoff_no_nav.cpp b/src/modules/navigator/takeoff_no_nav.cpp new file mode 100644 index 0000000000..a9cd7ebaeb --- /dev/null +++ b/src/modules/navigator/takeoff_no_nav.cpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file takeoff_no_nav.cpp + * + * Helper class to do a takeoff without lateral navigation + * + */ + +#include "takeoff_no_nav.h" +#include "navigator.h" //TODO remove? + +TakeoffNoNav::TakeoffNoNav(Navigator *navigator) : + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF_NO_NAV) +{ +} + +void TakeoffNoNav::on_activation() +{ + _navigator->reset_cruising_speed(); + _navigator->set_cruising_throttle(); + set_takeoff_position(); +} + +void TakeoffNoNav::on_active() +{ + if (_local_altitude_at_takeoff - _navigator->get_local_position()->z > _navigator->get_param_mis_takeoff_alt()) { + // Takeoff done - reset any potentially valid reposition triplet which was not handled. + // We do this to avoid random loiter locations after switching to loiter mode after this. + 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); + + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + _navigator->mode_completed(getNavigatorStateId()); + } +} + +void TakeoffNoNav::set_takeoff_position() +{ + // for now only support takeoff to MIS_TAKEOFF_ALT //TODO add mavlink interface + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + _local_altitude_at_takeoff = _navigator->get_local_position()->z; + + const float takeoff_altitude_amsl = _navigator->get_global_position()->alt + _navigator->get_param_mis_takeoff_alt(); + mavlink_log_info(_navigator->get_mavlink_log_pub(), + "Using default takeoff altitude: %.1f m\t", (double)_navigator->get_param_mis_takeoff_alt()); + + events::send(events::ID("navigator_takeoff_no_nav_default_alt"), {events::Log::Info, events::LogInternal::Info}, + "Using default takeoff altitude: {1:.2m}", + _navigator->get_param_mis_takeoff_alt()); + + _navigator->get_mission_result()->finished = false; + _navigator->set_mission_result_updated(); + + pos_sp_triplet->current.lat = NAN; + pos_sp_triplet->current.lon = NAN; + pos_sp_triplet->current.alt = + takeoff_altitude_amsl; // need to think about how to interface with the controller without global position + pos_sp_triplet->current.yaw = NAN; + pos_sp_triplet->current.loiter_radius = NAN; + pos_sp_triplet->current.loiter_direction_counter_clockwise = NAN; + pos_sp_triplet->current.acceptance_radius = NAN; + pos_sp_triplet->current.alt_acceptance_radius = NAN; + pos_sp_triplet->current.cruising_speed = NAN; + pos_sp_triplet->current.cruising_throttle = NAN; + pos_sp_triplet->current.gliding_enabled = false; + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.timestamp = hrt_absolute_time(); + + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} diff --git a/src/modules/navigator/takeoff_no_nav.h b/src/modules/navigator/takeoff_no_nav.h new file mode 100644 index 0000000000..aeb4392df8 --- /dev/null +++ b/src/modules/navigator/takeoff_no_nav.h @@ -0,0 +1,61 @@ +/*************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file takeoff_no_nav.h + * + * Helper class to do a takeoff without lateral navigation + * + */ + +#pragma once + +#include "navigator_mode.h" +#include "mission_block.h" + +#include + +#include +class TakeoffNoNav : public MissionBlock +{ +public: + TakeoffNoNav(Navigator *navigator); + ~TakeoffNoNav() = default; + + void on_activation() override; + void on_active() override; + +private: + + void set_takeoff_position(); + float _local_altitude_at_takeoff{0.f}; +};