CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/ArduPlane/mode_fbwa.cpp
Views: 1798
1
#include "mode.h"

2
#include "Plane.h"

3


4
void ModeFBWA::update()

5
{

6
    // set nav_roll and nav_pitch using sticks

7
    plane.nav_roll_cd  = plane.channel_roll->norm_input() * plane.roll_limit_cd;

8
    plane.update_load_factor();

9
    float pitch_input = plane.channel_pitch->norm_input();

10
    if (pitch_input > 0) {

11
        plane.nav_pitch_cd = pitch_input * plane.aparm.pitch_limit_max*100;

12
    } else {

13
        plane.nav_pitch_cd = -(pitch_input * plane.pitch_limit_min*100);

14
    }

15
    plane.adjust_nav_pitch_throttle();

16
    plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100);

17
    if (plane.fly_inverted()) {

18
        plane.nav_pitch_cd = -plane.nav_pitch_cd;

19
    }

20
    if (plane.failsafe.rc_failsafe && plane.g.fs_action_short == FS_ACTION_SHORT_FBWA) {

21
        // FBWA failsafe glide

22
        plane.nav_roll_cd = 0;

23
        plane.nav_pitch_cd = 0;

24
        SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::MIN);

25
    }

26
    RC_Channel *chan = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FBWA_TAILDRAGGER);

27
    if (chan != nullptr) {

28
        // check for the user enabling FBWA taildrag takeoff mode

29
        bool tdrag_mode = chan->get_aux_switch_pos() == RC_Channel::AuxSwitchPos::HIGH;

30
        if (tdrag_mode && !plane.auto_state.fbwa_tdrag_takeoff_mode) {

31
            if (plane.auto_state.highest_airspeed < plane.g.takeoff_tdrag_speed1) {

32
                plane.auto_state.fbwa_tdrag_takeoff_mode = true;

33
                plane.gcs().send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode");

34
            }

35
        }

36
    }

37
}

38


39
void ModeFBWA::run()

40
{

41
    // Run base class function and then output throttle

42
    Mode::run();

43


44
    output_pilot_throttle();

45
}

46


47