ardupilot/ArduPlane/mode_fbwa.cpp

46 lines
1.7 KiB
C++

#include "mode.h"
#include "Plane.h"
bool ModeFBWA::_enter()
{
plane.throttle_allows_nudging = false;
plane.auto_throttle_mode = false;
plane.auto_navigation_mode = false;
return true;
}
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) > 1700);
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");
}
}
}
}