You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
36 lines
1.5 KiB
36 lines
1.5 KiB
#include "mode.h" |
|
#include "Plane.h" |
|
|
|
void ModeFBWA::update() |
|
{ |
|
// set nav_roll and nav_pitch using sticks |
|
plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd; |
|
plane.update_load_factor(); |
|
float pitch_input = plane.channel_pitch->norm_input(); |
|
if (pitch_input > 0) { |
|
plane.nav_pitch_cd = pitch_input * plane.aparm.pitch_limit_max_cd; |
|
} else { |
|
plane.nav_pitch_cd = -(pitch_input * plane.pitch_limit_min_cd); |
|
} |
|
plane.adjust_nav_pitch_throttle(); |
|
plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); |
|
if (plane.fly_inverted()) { |
|
plane.nav_pitch_cd = -plane.nav_pitch_cd; |
|
} |
|
if (plane.failsafe.rc_failsafe && plane.g.fs_action_short == FS_ACTION_SHORT_FBWA) { |
|
// FBWA failsafe glide |
|
plane.nav_roll_cd = 0; |
|
plane.nav_pitch_cd = 0; |
|
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::MIN); |
|
} |
|
if (plane.g.fbwa_tdrag_chan > 0) { |
|
// check for the user enabling FBWA taildrag takeoff mode |
|
bool tdrag_mode = (RC_Channels::get_radio_in(plane.g.fbwa_tdrag_chan-1) > RC_Channel::AUX_PWM_TRIGGER_HIGH); |
|
if (tdrag_mode && !plane.auto_state.fbwa_tdrag_takeoff_mode) { |
|
if (plane.auto_state.highest_airspeed < plane.g.takeoff_tdrag_speed1) { |
|
plane.auto_state.fbwa_tdrag_takeoff_mode = true; |
|
plane.gcs().send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode"); |
|
} |
|
} |
|
} |
|
}
|
|
|