|
|
|
@ -1689,8 +1689,6 @@ FixedwingPositionControl::control_altitude(const hrt_abstime &now, const Vector2
@@ -1689,8 +1689,6 @@ FixedwingPositionControl::control_altitude(const hrt_abstime &now, const Vector2
|
|
|
|
|
_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// decide when to use pitch setpoint from TECS because in some cases pitch
|
|
|
|
|
// setpoint is generated by other means
|
|
|
|
|
_att_sp.pitch_body = get_tecs_pitch(); |
|
|
|
|
|
|
|
|
|
_last_manual = !_control_mode.flag_control_position_enabled; |
|
|
|
@ -1811,8 +1809,6 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
@@ -1811,8 +1809,6 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
|
|
|
|
|
_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// decide when to use pitch setpoint from TECS because in some cases pitch
|
|
|
|
|
// setpoint is generated by other means
|
|
|
|
|
_att_sp.pitch_body = get_tecs_pitch(); |
|
|
|
|
|
|
|
|
|
_last_manual = !_control_mode.flag_control_position_enabled; |
|
|
|
@ -1961,7 +1957,6 @@ FixedwingPositionControl::Run()
@@ -1961,7 +1957,6 @@ FixedwingPositionControl::Run()
|
|
|
|
|
Vector2f ground_speed(_local_pos.vx, _local_pos.vy); |
|
|
|
|
|
|
|
|
|
set_control_mode_current(_pos_sp_triplet.current.valid); |
|
|
|
|
bool setpoint = true; |
|
|
|
|
|
|
|
|
|
switch (_control_mode_current) { |
|
|
|
|
case FW_POSCTRL_MODE_AUTO: { |
|
|
|
@ -1981,7 +1976,6 @@ FixedwingPositionControl::Run()
@@ -1981,7 +1976,6 @@ FixedwingPositionControl::Run()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
case FW_POSCTRL_MODE_OTHER: { |
|
|
|
|
setpoint = false; |
|
|
|
|
/* do not publish the setpoint */ |
|
|
|
|
// reset hold altitude
|
|
|
|
|
_hold_alt = _current_altitude; |
|
|
|
@ -1999,7 +1993,8 @@ FixedwingPositionControl::Run()
@@ -1999,7 +1993,8 @@ FixedwingPositionControl::Run()
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (setpoint) { |
|
|
|
|
if (_control_mode_current != FW_POSCTRL_MODE_OTHER) { |
|
|
|
|
|
|
|
|
|
if (_control_mode.flag_control_manual_enabled) { |
|
|
|
|
_att_sp.roll_body = constrain(_att_sp.roll_body, -radians(_param_fw_man_r_max.get()), |
|
|
|
|
radians(_param_fw_man_r_max.get())); |
|
|
|
|