|
|
|
@ -1060,7 +1060,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
@@ -1060,7 +1060,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (0/* easy on and manual control yaw non-zero */) { |
|
|
|
|
_seatbelt_hold_heading = _att.yaw + _manual.yaw; |
|
|
|
|
_seatbelt_hold_heading = _att.yaw + _manual.r; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
//XXX not used
|
|
|
|
@ -1078,12 +1078,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
@@ -1078,12 +1078,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|
|
|
|
// XXX check if ground speed undershoot should be applied here
|
|
|
|
|
float seatbelt_airspeed = _parameters.airspeed_min + |
|
|
|
|
(_parameters.airspeed_max - _parameters.airspeed_min) * |
|
|
|
|
_manual.throttle; |
|
|
|
|
_manual.z; |
|
|
|
|
|
|
|
|
|
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); |
|
|
|
|
_att_sp.roll_body = _l1_control.nav_roll(); |
|
|
|
|
_att_sp.yaw_body = _l1_control.nav_bearing(); |
|
|
|
|
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, |
|
|
|
|
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, |
|
|
|
|
seatbelt_airspeed, |
|
|
|
|
_airspeed.indicated_airspeed_m_s, eas2tas, |
|
|
|
|
false, _parameters.pitch_limit_min, |
|
|
|
@ -1099,7 +1099,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
@@ -1099,7 +1099,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (0/* seatbelt on and manual control yaw non-zero */) { |
|
|
|
|
_seatbelt_hold_heading = _att.yaw + _manual.yaw; |
|
|
|
|
_seatbelt_hold_heading = _att.yaw + _manual.r; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* if in seatbelt mode, set airspeed based on manual control */ |
|
|
|
@ -1107,10 +1107,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
@@ -1107,10 +1107,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|
|
|
|
// XXX check if ground speed undershoot should be applied here
|
|
|
|
|
float seatbelt_airspeed = _parameters.airspeed_min + |
|
|
|
|
(_parameters.airspeed_max - _parameters.airspeed_min) * |
|
|
|
|
_manual.throttle; |
|
|
|
|
_manual.z; |
|
|
|
|
|
|
|
|
|
/* user switched off throttle */ |
|
|
|
|
if (_manual.throttle < 0.1f) { |
|
|
|
|
if (_manual.z < 0.1f) { |
|
|
|
|
throttle_max = 0.0f; |
|
|
|
|
/* switch to pure pitch based altitude control, give up speed */ |
|
|
|
|
_tecs.set_speed_weight(0.0f); |
|
|
|
@ -1120,14 +1120,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
@@ -1120,14 +1120,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|
|
|
|
bool climb_out = false; |
|
|
|
|
|
|
|
|
|
/* user wants to climb out */ |
|
|
|
|
if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { |
|
|
|
|
if (_manual.x > 0.3f && _manual.z > 0.8f) { |
|
|
|
|
climb_out = true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); |
|
|
|
|
_att_sp.roll_body = _manual.roll; |
|
|
|
|
_att_sp.yaw_body = _manual.yaw; |
|
|
|
|
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, |
|
|
|
|
_att_sp.roll_body = _manual.y; |
|
|
|
|
_att_sp.yaw_body = _manual.r; |
|
|
|
|
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, |
|
|
|
|
seatbelt_airspeed, |
|
|
|
|
_airspeed.indicated_airspeed_m_s, eas2tas, |
|
|
|
|
climb_out, _parameters.pitch_limit_min, |
|
|
|
|