diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index da1938ffc6..e85db3f472 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -707,6 +707,22 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) return; // do not publish the setpoint } + if (_control_mode.flag_control_auto_enabled && _vehicle_status.is_vtol && _vehicle_status.in_transition_mode) { + if (_control_mode.flag_control_position_enabled) { + _control_mode_current = FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_POSITION; + + } else { + _control_mode_current = FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_FIXED; + } + + return; + } + + if (_control_mode.flag_control_manual_enabled && _vehicle_status.is_vtol && _vehicle_status.in_transition_mode) { + _control_mode_current = FW_POSCTRL_MODE_MANUAL_VTOL_TRANSITION; + return; + } + FW_POSCTRL_MODE commanded_position_control_mode = _control_mode_current; _skipping_takeoff_detection = false; @@ -716,40 +732,24 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) { - _control_mode_current = FW_POSCTRL_MODE_AUTO; + _control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF; - // in this case we want the waypoint handled as a position setpoint -- a submode in control_auto() - _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - - } else { - _control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF; - - if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) { - // skip takeoff detection when switching from any other mode, auto or manual, - // while already in air. - // TODO: find a better place for / way of doing this - _skipping_takeoff_detection = true; - } + if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) { + // skip takeoff detection when switching from any other mode, auto or manual, + // while already in air. + // TODO: find a better place for / way of doing this + _skipping_takeoff_detection = true; } } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - - if (!_vehicle_status.in_transition_mode) { - - // Use _position_setpoint_previous_valid to determine if landing should be straight or circular. - // Straight landings are currently only possible in Missions, and there the previous WP - // is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false. - if (_position_setpoint_previous_valid) { - _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT; - - } else { - _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR; - } + // Use _position_setpoint_previous_valid to determine if landing should be straight or circular. + // Straight landings are currently only possible in Missions, and there the previous WP + // is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false. + if (_position_setpoint_previous_valid) { + _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT; } else { - // in this case we want the waypoint handled as a position setpoint -- a submode in control_auto() - _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR; } } else { @@ -769,8 +769,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) _time_in_fixed_bank_loiter = now; } - if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s) - && !_vehicle_status.in_transition_mode) { + if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s)) { if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) { // Need to init because last loop iteration was in a different mode events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical, @@ -833,36 +832,24 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now) } void -FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_setpoint_s ¤t_sp) +FixedwingPositionControl::set_position_setpoint_for_vtol_transition(const position_setpoint_s ¤t_sp) { // TODO: velocity, altitude, or just a heading hold position mode should be used for this, not position // shifting hacks - if (_vehicle_status.in_transition_to_fw) { + if (!PX4_ISFINITE(_transition_waypoint(0))) { + double lat_transition, lon_transition; - if (!PX4_ISFINITE(_transition_waypoint(0))) { - double lat_transition, lon_transition; + // Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the path navigation controller can track + // during the transition. Use the current yaw setpoint to determine the transition heading, as that one in turn + // is set to the transition heading by Navigator, or current yaw if setpoint is not valid. + const float transition_heading = PX4_ISFINITE(current_sp.yaw) ? current_sp.yaw : _yaw; + waypoint_from_heading_and_distance(_current_latitude, _current_longitude, transition_heading, HDG_HOLD_DIST_NEXT, + &lat_transition, + &lon_transition); - // Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the path navigation controller can track - // during the transition. Use the current yaw setpoint to determine the transition heading, as that one in turn - // is set to the transition heading by Navigator, or current yaw if setpoint is not valid. - const float transition_heading = PX4_ISFINITE(current_sp.yaw) ? current_sp.yaw : _yaw; - waypoint_from_heading_and_distance(_current_latitude, _current_longitude, transition_heading, HDG_HOLD_DIST_NEXT, - &lat_transition, - &lon_transition); - - _transition_waypoint(0) = lat_transition; - _transition_waypoint(1) = lon_transition; - } - - - current_sp.lat = _transition_waypoint(0); - current_sp.lon = _transition_waypoint(1); - - } else { - /* reset transition waypoint, will be set upon entering front transition */ - _transition_waypoint(0) = static_cast(NAN); - _transition_waypoint(1) = static_cast(NAN); + _transition_waypoint(0) = lat_transition; + _transition_waypoint(1) = lon_transition; } } @@ -871,16 +858,13 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next) { - position_setpoint_s current_sp = pos_sp_curr; - move_position_setpoint_for_vtol_transition(current_sp); - - const uint8_t position_sp_type = handle_setpoint_type(current_sp); + const uint8_t position_sp_type = handle_setpoint_type(pos_sp_curr); _position_sp_type = position_sp_type; if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER - || current_sp.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { - publishOrbitStatus(current_sp); + || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + publishOrbitStatus(pos_sp_curr); } switch (position_sp_type) { @@ -891,15 +875,15 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto break; case position_setpoint_s::SETPOINT_TYPE_POSITION: - control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp); + control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); break; case position_setpoint_s::SETPOINT_TYPE_VELOCITY: - control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp); + control_auto_velocity(control_interval, curr_pos, ground_speed, pos_sp_curr); break; case position_setpoint_s::SETPOINT_TYPE_LOITER: - control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next); + control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr, pos_sp_next); break; } @@ -917,10 +901,60 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto _att_sp.pitch_body = get_tecs_pitch(); if (!_vehicle_status.in_transition_to_fw) { - publishLocalPositionSetpoint(current_sp); + publishLocalPositionSetpoint(pos_sp_curr); } } +void FixedwingPositionControl::control_auto_transition_position(const Vector2d &curr_pos, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_curr) +{ + set_position_setpoint_for_vtol_transition(pos_sp_curr); + + Vector2d curr_wp = Vector2d(_transition_waypoint(0), _transition_waypoint(1)); + Vector2d prev_wp{curr_pos}; + + Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1)); + Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1)); + + _npfg.setAirspeedNom(_airspeed); // We stop the motor, so use the current airspeed as estimation. + _npfg.setAirspeedMax(_airspeed); + + navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + + _att_sp.roll_body = _npfg.getRollSetpoint(); + + _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + + _att_sp.pitch_body = radians(_param_fw_psp_off.get()); // set constant pitch angle + + _att_sp.thrust_body[0] = 0.0f; + + +} + +void FixedwingPositionControl::control_auto_transition_fixed() +{ + _att_sp.roll_body = radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + + _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + + _att_sp.pitch_body = radians(_param_fw_psp_off.get()); // set constant pitch angle + + _att_sp.thrust_body[0] = 0.0f; +} + +void FixedwingPositionControl::control_manual_transition() +{ + _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + + _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + + _att_sp.pitch_body = radians(_param_fw_psp_off.get()); // set constant pitch angle + + _att_sp.thrust_body[0] = 0.0f; +} + void FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval) { @@ -2092,23 +2126,14 @@ FixedwingPositionControl::control_manual_position(const float control_interval, float FixedwingPositionControl::get_tecs_pitch() { - if (_tecs_is_running) { - return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get()); - } + return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get()); - // return level flight pitch offset to prevent stale tecs state when it's not running - return radians(_param_fw_psp_off.get()); } float FixedwingPositionControl::get_tecs_thrust() { - if (_tecs_is_running) { - return min(_tecs.get_throttle_setpoint(), 1.f); - } - - // return 0 to prevent stale tecs state when it's not running - return 0.0f; + return min(_tecs.get_throttle_setpoint(), 1.f); } void @@ -2301,6 +2326,12 @@ FixedwingPositionControl::Run() _flaps_setpoint = 0.f; _spoilers_setpoint = 0.f; + /* reset transition waypoint, will be set upon entering transition */ + if (_control_mode_current != FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_POSITION) { + _transition_waypoint(0) = static_cast(NAN); + _transition_waypoint(1) = static_cast(NAN); + } + // by default we don't want yaw to be contoller directly with rudder _att_sp.fw_control_yaw_wheel = false; @@ -2352,6 +2383,16 @@ FixedwingPositionControl::Run() break; } + case FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_FIXED: { + control_auto_transition_fixed(); + break; + } + + case FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_POSITION: { + control_auto_transition_position(curr_pos, ground_speed, _pos_sp_triplet.current); + break; + } + case FW_POSCTRL_MODE_MANUAL_POSITION: { control_manual_position(control_interval, curr_pos, ground_speed); break; @@ -2362,6 +2403,11 @@ FixedwingPositionControl::Run() break; } + case FW_POSCTRL_MODE_MANUAL_VTOL_TRANSITION: { + control_manual_transition(); + break; + } + case FW_POSCTRL_MODE_OTHER: { _att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get()); @@ -2508,15 +2554,10 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva const float desired_max_sinkrate, const float desired_max_climbrate, bool disable_underspeed_detection, float hgt_rate_sp) { - _tecs_is_running = true; // do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition // (it should also not run during VTOL blending because airspeed is too low still) if (_vehicle_status.is_vtol) { - if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) { - _tecs_is_running = false; - } - if (_vehicle_status.in_transition_mode) { // we're in transition _was_in_transition = true; @@ -2547,12 +2588,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva } } - if (!_tecs_is_running) { - // next time we run TECS we should reinitialize states - _reinitialize_tecs = true; - return; - } - // We need an altitude lock to calculate the TECS control if (_local_pos.timestamp == 0) { _reinitialize_tecs = true; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index fe4e6aa580..ec28415a56 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -242,8 +242,11 @@ private: FW_POSCTRL_MODE_AUTO_TAKEOFF, FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT, FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR, + FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_POSITION, + FW_POSCTRL_MODE_AUTO_VTOL_TRANSITION_FIXED, FW_POSCTRL_MODE_MANUAL_POSITION, FW_POSCTRL_MODE_MANUAL_ALTITUDE, + FW_POSCTRL_MODE_MANUAL_VTOL_TRANSITION, FW_POSCTRL_MODE_OTHER } _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed @@ -390,7 +393,6 @@ private: TECS _tecs; bool _reinitialize_tecs{true}; - bool _tecs_is_running{false}; hrt_abstime _time_last_tecs_update{0}; // [us] // VTOL / TRANSITION @@ -511,12 +513,12 @@ private: void update_in_air_states(const hrt_abstime now); /** - * @brief Moves the current position setpoint to a value far ahead of the current vehicle yaw when in a VTOL + * @brief Set the current position setpoint to a value far ahead of the current vehicle yaw when in a VTOL * transition. * * @param[in,out] current_sp Current position setpoint */ - void move_position_setpoint_for_vtol_transition(position_setpoint_s ¤t_sp); + void set_position_setpoint_for_vtol_transition(const position_setpoint_s ¤t_sp); /** * @brief Changes the position setpoint type to achieve the desired behavior in some instances. @@ -541,6 +543,26 @@ private: void control_auto(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next); + /** + * @brief Automatic position control for VTOL transition + * + * @param curr_pos Current 2D local position vector of vehicle [m] + * @param ground_speed Local 2D ground speed of vehicle [m/s] + * @param pos_sp_curr current position setpoint + */ + void control_auto_transition_position(const Vector2d &curr_pos, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_curr); + + /** + * @brief Automatic fixed bank control for VTOL transition + */ + void control_auto_transition_fixed(); + + /** + * @brief Manual control for for VTOL transition + */ + void control_manual_transition(); + /** * @brief Controls altitude and airspeed for a fixed-bank loiter. *