From b77487d69cf9dd3770d088b764b9ffd1d140ea2f Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 25 Oct 2021 11:48:05 +0200 Subject: [PATCH] Fixed-wing Position controller: add modes for auto altitude and auto descend - bit of clean up - add GPS failsafe mode auto_altitude, that will keep current altitude with a fixed-bank angle for some time, then switches to auto_descend that will descend with constant sink rate of 0.5m/s - params controlling GPS failsafe are not FW params: NAV_GPSF_R --> FW_GPSF_R and NAV_GPSF_LT --> FW_GPSF_LT Signed-off-by: Silvan Fuhrer --- src/lib/parameters/param_translation.cpp | 14 ++ .../FixedwingPositionControl.cpp | 173 ++++++++++++++---- .../FixedwingPositionControl.hpp | 24 ++- .../fw_pos_control_l1_params.c | 29 +++ 4 files changed, 197 insertions(+), 43 deletions(-) diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index ac6ae5d122..1744082ced 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -195,6 +195,20 @@ bool param_modify_on_import(bson_node_t node) } } + // 2021-10-21: translate NAV_GPSF_LT to FW_GPSF_LT and NAV_GPSF_R to FW_GPSF_R + { + if (strcmp("NAV_GPSF_LT", node->name) == 0) { + strcpy(node->name, "FW_GPSF_LT"); + PX4_INFO("copying %s -> %s", "NAV_GPSF_LT", "FW_GPSF_LT"); + } + + if (strcmp("NAV_GPSF_R", node->name) == 0) { + strcpy(node->name, "FW_GPSF_R"); + PX4_INFO("copying and inverting sign %s -> %s", "NAV_GPSF_R", "FW_GPSF_R"); + } + } + + // translate (SPI) calibration ID parameters. This can be removed after the next release (current release=1.10) if (node->type != BSON_INT32) { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 325370ee0e..99c3d13f04 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -236,7 +236,6 @@ FixedwingPositionControl::vehicle_command_poll() abort_landing(true); } } - } } @@ -493,7 +492,7 @@ FixedwingPositionControl::status_publish() pos_ctrl_status.timestamp = hrt_absolute_time(); - pos_ctrl_status.type = _type; + pos_ctrl_status.type = _position_sp_type; _pos_ctrl_status_pub.publish(pos_ctrl_status); } @@ -671,7 +670,7 @@ FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_lim } void -FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid) +FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid) { /* only run position controller in fixed-wing mode and during transitions for VTOL */ if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) { @@ -679,11 +678,41 @@ FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid) return; // do not publish the setpoint } - if ((_control_mode.flag_control_auto_enabled || _control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) { + if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) || + _control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) { _control_mode_current = FW_POSCTRL_MODE_AUTO; - } else if (_control_mode.flag_control_velocity_enabled && _control_mode.flag_control_altitude_enabled) { - if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { + } else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_climb_rate_enabled) { + + // reset timer the first time we switch into this mode + if (_control_mode_current != FW_POSCTRL_MODE_AUTO_ALTITUDE && _control_mode_current != FW_POSCTRL_MODE_AUTO_CLIMBRATE) { + _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 (_control_mode_current != FW_POSCTRL_MODE_AUTO_ALTITUDE) { + // Need to init because last loop iteration was in a different mode + _hold_alt = _current_altitude; + mavlink_log_critical(&_mavlink_log_pub, "Start loiter with fixed bank angle.\t"); + events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical, + "Start loiter with fixed bank angle"); + } + + _control_mode_current = FW_POSCTRL_MODE_AUTO_ALTITUDE; + + } else { + if (_control_mode_current != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) { + mavlink_log_critical(&_mavlink_log_pub, "Start descending.\t"); + events::send(events::ID("fixedwing_position_control_descend"), events::Log::Critical, "Start descending"); + } + + _control_mode_current = FW_POSCTRL_MODE_AUTO_CLIMBRATE; + } + + + } else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) { + if (_control_mode_current != FW_POSCTRL_MODE_MANUAL_POSITION) { /* Need to init because last loop iteration was in a different mode */ _hold_alt = _current_altitude; _hdg_hold_yaw = _yaw; // yaw is not controlled, so set setpoint to current yaw @@ -696,15 +725,16 @@ FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid) _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } - _control_mode_current = FW_POSCTRL_MODE_POSITION; + _control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION; - } else if (_control_mode.flag_control_altitude_enabled) { - if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) { + } else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_altitude_enabled) { + if (_control_mode_current != FW_POSCTRL_MODE_MANUAL_POSITION + && _control_mode_current != FW_POSCTRL_MODE_MANUAL_ALTITUDE) { /* Need to init because last loop iteration was in a different mode */ _hold_alt = _current_altitude; } - _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; + _control_mode_current = FW_POSCTRL_MODE_MANUAL_ALTITUDE; } else { _control_mode_current = FW_POSCTRL_MODE_OTHER; @@ -724,9 +754,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; // by default we don't use flaps - /* no throttle limit as default */ - float throttle_max = 1.0f; - /* save time when airplane is in air */ if (!_was_in_air && !_landed) { _was_in_air = true; @@ -767,11 +794,9 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c publishOrbitStatus(pos_sp_curr); } - const uint8_t position_sp_type = handle_setpoint_type(pos_sp_curr.type, pos_sp_curr); + _position_sp_type = handle_setpoint_type(pos_sp_curr.type, pos_sp_curr); - _type = position_sp_type; // for publication - - switch (position_sp_type) { + switch (_position_sp_type) { case position_setpoint_s::SETPOINT_TYPE_IDLE: _att_sp.thrust_body[0] = 0.0f; _att_sp.roll_body = 0.0f; @@ -796,12 +821,12 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c } /* reset landing state */ - if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) { + if (_position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) { reset_landing_state(); } /* reset takeoff/launch state */ - if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + if (_position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -823,7 +848,7 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _runway_takeoff.runwayTakeoffEnabled()) { - _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, min(get_tecs_thrust(), throttle_max)); + _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust()); } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { @@ -833,10 +858,10 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c /* Copy thrust and pitch values from tecs */ if (_landed) { // when we are landed state we want the motor to spin at idle speed - _att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), throttle_max); + _att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), 1.f); } else { - _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); + _att_sp.thrust_body[0] = get_tecs_thrust(); } } @@ -854,8 +879,68 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c if (use_tecs_pitch) { _att_sp.pitch_body = get_tecs_pitch(); } +} - _last_manual = !_control_mode.flag_control_position_enabled; +void +FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &now) +{ + // only control altitude and airspeed ("fixed-bank loiter") + + tecs_update_pitch_throttle(now, _hold_alt, + _param_fw_airspd_trim.get(), + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), + _param_fw_thr_max.get(), + _param_fw_thr_cruise.get(), + false, + _param_fw_p_lim_min.get()); + + _att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + _att_sp.yaw_body = 0.f; + + if (_landed) { + _att_sp.thrust_body[0] = _param_fw_thr_min.get(); + + } else { + _att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get()); + } + + _att_sp.pitch_body = get_tecs_pitch(); +} + +void +FixedwingPositionControl::control_auto_descend(const hrt_abstime &now) +{ + // only control height rate + + // Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover, + // but not letting it drift too far away. + const float descend_rate = -0.5f; + + tecs_update_pitch_throttle(now, _hold_alt, + _param_fw_airspd_trim.get(), + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), + _param_fw_thr_max.get(), + _param_fw_thr_cruise.get(), + false, + _param_fw_p_lim_min.get(), + tecs_status_s::TECS_MODE_NORMAL, + descend_rate); + + _att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + _att_sp.yaw_body = 0.f; + + if (_landed) { + _att_sp.thrust_body[0] = _param_fw_thr_min.get(); + + } else { + _att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get()); + } + + _att_sp.pitch_body = get_tecs_pitch(); } uint8_t @@ -927,6 +1012,11 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons } } + // set to type loiter during VTOL transitions in Land mode (to not start FW landing logic) + if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _vehicle_status.in_transition_mode) { + position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER; + } + return position_sp_type; } @@ -1641,7 +1731,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec } void -FixedwingPositionControl::control_altitude(const hrt_abstime &now, const Vector2d &curr_pos, +FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed) { /* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ @@ -1693,12 +1783,10 @@ FixedwingPositionControl::control_altitude(const hrt_abstime &now, const Vector2 } _att_sp.pitch_body = get_tecs_pitch(); - - _last_manual = !_control_mode.flag_control_position_enabled; } void -FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2d &curr_pos, +FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed) { const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f); @@ -1813,8 +1901,6 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 } _att_sp.pitch_body = get_tecs_pitch(); - - _last_manual = !_control_mode.flag_control_position_enabled; } float @@ -1832,7 +1918,7 @@ float FixedwingPositionControl::get_tecs_thrust() { if (_is_tecs_running) { - return _tecs.get_throttle_setpoint(); + return min(_tecs.get_throttle_setpoint(), 1.f); } // return 0 to prevent stale tecs state when it's not running @@ -1959,7 +2045,7 @@ FixedwingPositionControl::Run() Vector2d curr_pos(_current_latitude, _current_longitude); Vector2f ground_speed(_local_pos.vx, _local_pos.vy); - set_control_mode_current(_pos_sp_triplet.current.valid); + set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid); switch (_control_mode_current) { case FW_POSCTRL_MODE_AUTO: { @@ -1968,13 +2054,23 @@ FixedwingPositionControl::Run() break; } - case FW_POSCTRL_MODE_POSITION: { - control_position(_local_pos.timestamp, curr_pos, ground_speed); + case FW_POSCTRL_MODE_AUTO_ALTITUDE: { + control_auto_fixed_bank_alt_hold(_local_pos.timestamp); break; } - case FW_POSCTRL_MODE_ALTITUDE: { - control_altitude(_local_pos.timestamp, curr_pos, ground_speed); + case FW_POSCTRL_MODE_AUTO_CLIMBRATE: { + control_auto_descend(_local_pos.timestamp); + break; + } + + case FW_POSCTRL_MODE_MANUAL_POSITION: { + control_manual_position(_local_pos.timestamp, curr_pos, ground_speed); + break; + } + + case FW_POSCTRL_MODE_MANUAL_ALTITUDE: { + control_manual_altitude(_local_pos.timestamp, curr_pos, ground_speed); break; } @@ -2008,7 +2104,8 @@ FixedwingPositionControl::Run() if (_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled || - _control_mode.flag_control_altitude_enabled) { + _control_mode.flag_control_altitude_enabled || + _control_mode.flag_control_climb_rate_enabled) { const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -2025,6 +2122,7 @@ FixedwingPositionControl::Run() } _l1_control.reset_has_guidance_updated(); + _last_manual = !_control_mode.flag_control_position_enabled; } perf_end(_loop_perf); @@ -2168,7 +2266,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo bool in_air_alt_control = (!_landed && (_control_mode.flag_control_auto_enabled || _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_altitude_enabled)); + _control_mode.flag_control_altitude_enabled || + _control_mode.flag_control_climb_rate_enabled)); /* update TECS vehicle state estimates */ _tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_alt_control, diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 60cba3c122..d0d9ed7c1d 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -257,14 +257,18 @@ private: float _manual_control_setpoint_altitude{0.0f}; float _manual_control_setpoint_airspeed{0.0f}; + hrt_abstime _time_in_fixed_bank_loiter{0}; + ECL_L1_Pos_Controller _l1_control; TECS _tecs; - uint8_t _type{0}; + uint8_t _position_sp_type{0}; enum FW_POSCTRL_MODE { FW_POSCTRL_MODE_AUTO, - FW_POSCTRL_MODE_POSITION, - FW_POSCTRL_MODE_ALTITUDE, + FW_POSCTRL_MODE_AUTO_ALTITUDE, + FW_POSCTRL_MODE_AUTO_CLIMBRATE, + FW_POSCTRL_MODE_MANUAL_POSITION, + FW_POSCTRL_MODE_MANUAL_ALTITUDE, FW_POSCTRL_MODE_OTHER } _control_mode_current{FW_POSCTRL_MODE_OTHER}; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode. @@ -327,6 +331,10 @@ private: void control_auto(const hrt_abstime &now, 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); + + void control_auto_fixed_bank_alt_hold(const hrt_abstime &now); + void control_auto_descend(const hrt_abstime &now); + void control_auto_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); void control_auto_loiter(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, @@ -338,8 +346,8 @@ private: void control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); - void control_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed); - void control_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed); + void control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed); + void control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed); float get_tecs_pitch(); float get_tecs_thrust(); @@ -350,7 +358,7 @@ private: void reset_takeoff_state(bool force = false); void reset_landing_state(); Vector2f get_nav_speed_2d(const Vector2f &ground_speed); - void set_control_mode_current(bool pos_sp_curr_valid); + void set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid); void publishOrbitStatus(const position_setpoint_s pos_sp); @@ -424,6 +432,10 @@ private: (ParamBool) _param_fw_posctl_inv_st, + + (ParamInt) _param_nav_gpsf_lt, + (ParamFloat) _param_nav_gpsf_r, + // external parameters (ParamInt) _param_fw_arsp_mode, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 4c6f6daccc..156006326a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -825,3 +825,32 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP, 3.0f); * @group FW TECS */ PARAM_DEFINE_FLOAT(FW_T_SINK_R_SP, 2.0f); + +/** + * GPS failure loiter time + * + * The time in seconds the system should do open loop loiter and wait for GPS recovery + * before it starts descending. Set to 0 to disable. Roll angle is set to FW_GPSF_R. + * Does only apply for fixed-wing vehicles or VTOLs with NAV_FORCE_VT set to 0. + * + * @unit s + * @min 0 + * @max 3600 + * @group Mission + */ +PARAM_DEFINE_INT32(FW_GPSF_LT, 30); + +/** + * GPS failure fixed roll angle + * + * Roll in degrees during the loiter after the vehicle has lost GPS in an auto mode (e.g. mission or loiter). + * Does only apply for fixed-wing vehicles or VTOLs with NAV_FORCE_VT set to 0. + * + * @unit deg + * @min 0.0 + * @max 30.0 + * @decimal 1 + * @increment 0.5 + * @group Mission + */ +PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f);