From 5197be67a7974afa2763b6af618e6a4f09e2c34c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 May 2015 13:08:09 +0200 Subject: [PATCH] FW control: Add skeleton for distinct altitude control and position control flight modes Conflicts: src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp --- .../fw_att_control/fw_att_control_main.cpp | 26 ++++ .../fw_pos_control_l1_main.cpp | 143 +++++++++++++++++- 2 files changed, 167 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 8b0d452a3e..31778e9c81 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -854,6 +854,32 @@ FixedwingAttitudeControl::task_main() _yaw_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { + + /* the pilot does not want to change direction, + * take straight attitude setpoint from position controller + */ + if (fabsf(_manual.y) < 0.01f) { + roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + } else { + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + } + + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + throttle_sp = _att_sp.thrust; + + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } + + } else if (_vcontrol_mode.flag_control_altitude_enabled) { /* * Velocity should be controlled and manual is enabled. */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a50fc53bf7..ec944c8b1d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -165,7 +165,11 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - float _hold_alt; /**< hold altitude for velocity mode */ + float _hold_alt; /**< hold altitude for altitude mode */ + float _hdg_hold_yaw; /**< hold heading for velocity mode */ + bool _hdg_hold_enabled; /**< heading hold enabled */ + struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ + struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** ¤t_positi /* reset hold altitude */ _hold_alt = _global_pos.alt; + /* reset hold yaw */ + _hdg_hold_yaw = _att.yaw; /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); @@ -1251,7 +1294,103 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (_control_mode.flag_control_velocity_enabled && _control_mode.flag_control_altitude_enabled) { - /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ + /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed, + heading is set to a distant waypoint */ + + const float deadBand = (60.0f/1000.0f); + const float factor = 1.0f - deadBand; + if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { + /* Need to init because last loop iteration was in a different mode */ + _hold_alt = _global_pos.alt; + _hdg_hold_yaw = _att.yaw; + } + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ + if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { + /* reset integrators */ + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } + } + _control_mode_current = FW_POSCTRL_MODE_POSITION; + + /* Get demanded airspeed */ + float altctrl_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; + + /* Get demanded vertical velocity from pitch control */ + static bool was_in_deadband = false; + if (_manual.x > deadBand) { + float pitch = (_manual.x - deadBand) / factor; + _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; + was_in_deadband = false; + } else if (_manual.x < - deadBand) { + float pitch = (_manual.x + deadBand) / factor; + _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; + was_in_deadband = false; + } else if (!was_in_deadband) { + /* store altitude at which manual.x was inside deadBand + * The aircraft should immediately try to fly at this altitude + * as this is what the pilot expects when he moves the stick to the center */ + _hold_alt = _global_pos.alt; + was_in_deadband = true; + } + tecs_update_pitch_throttle(_hold_alt, + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed, + TECS_MODE_NORMAL); + + /* heading control */ + + if (fabsf(_manual.y) < 0.01f) { + /* heading / roll is zero, lock onto current heading */ + + // XXX calculate a waypoint in some distance + // and lock on to it + + /* just switched back from non heading-hold to heading hold */ + if (!_hdg_hold_enabled) { + _hdg_hold_enabled = true; + _hdg_hold_yaw = _att.yaw; + + get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); + } + + /* we have a valid heading hold position, are we too close? */ + if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < 1000) { + get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); + } + + math::Vector<2> prev_wp; + prev_wp(0) = (float)_hdg_hold_prev_wp.lat; + prev_wp(1) = (float)_hdg_hold_prev_wp.lon; + + math::Vector<2> curr_wp; + curr_wp(0) = (float)_hdg_hold_curr_wp.lat; + curr_wp(1) = (float)_hdg_hold_curr_wp.lon; + + /* populate l1 control setpoint */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + } else { + _hdg_hold_enabled = false; + } + + } else if (_control_mode.flag_control_altitude_enabled) { + /* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand;