diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 23c0c466f4..0e5b78789f 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -687,6 +687,23 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 mission_throttle = pos_sp_curr.cruising_throttle; } + float tecs_fw_thr_min; + float tecs_fw_thr_max; + float tecs_fw_mission_throttle; + + if (mission_throttle < _param_fw_thr_min.get()) { + /* enable gliding with this waypoint */ + _tecs.set_speed_weight(2.0f); + tecs_fw_thr_min = 0.0; + tecs_fw_thr_max = 0.0; + tecs_fw_mission_throttle = 0.0; + + } else { + tecs_fw_thr_min = _param_fw_thr_min.get(); + tecs_fw_thr_max = _param_fw_thr_max.get(); + tecs_fw_mission_throttle = mission_throttle; + } + const float acc_rad = _l1_control.switch_distance(500.0f); uint8_t position_sp_type = pos_sp_curr.type; @@ -782,23 +799,6 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 _att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.yaw_body = _l1_control.nav_bearing(); - float tecs_fw_thr_min; - float tecs_fw_thr_max; - float tecs_fw_mission_throttle; - - if (mission_throttle < _param_fw_thr_min.get()) { - /* enable gliding with this waypoint */ - _tecs.set_speed_weight(2.0f); - tecs_fw_thr_min = 0.0; - tecs_fw_thr_max = 0.0; - tecs_fw_mission_throttle = 0.0; - - } else { - tecs_fw_thr_min = _param_fw_thr_min.get(); - tecs_fw_thr_max = _param_fw_thr_max.get(); - tecs_fw_mission_throttle = mission_throttle; - } - tecs_update_pitch_throttle(now, position_sp_alt, calculate_target_airspeed(mission_airspeed, ground_speed), radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()), @@ -856,24 +856,6 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 _tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get()); } - - float tecs_fw_thr_min; - float tecs_fw_thr_max; - float tecs_fw_mission_throttle; - - if (mission_throttle < _param_fw_thr_min.get()) { - /* enable gliding with this waypoint */ - _tecs.set_speed_weight(2.0f); - tecs_fw_thr_min = 0.0; - tecs_fw_thr_max = 0.0; - tecs_fw_mission_throttle = 0.0; - - } else { - tecs_fw_thr_min = _param_fw_thr_min.get(); - tecs_fw_thr_max = _param_fw_thr_max.get(); - tecs_fw_mission_throttle = _param_fw_thr_cruise.get(); - } - tecs_update_pitch_throttle(now, alt_sp, calculate_target_airspeed(mission_airspeed, ground_speed), radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()),