Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : 
       4     1114368 : void ModeFBWA::update()
       5             : {
       6             :     // set nav_roll and nav_pitch using sticks
       7     1114368 :     plane.nav_roll_cd  = plane.channel_roll->norm_input() * plane.roll_limit_cd;
       8     1114368 :     plane.update_load_factor();
       9     1114368 :     float pitch_input = plane.channel_pitch->norm_input();
      10     1114368 :     if (pitch_input > 0) {
      11       33064 :         plane.nav_pitch_cd = pitch_input * plane.aparm.pitch_limit_max*100;
      12             :     } else {
      13     1081304 :         plane.nav_pitch_cd = -(pitch_input * plane.pitch_limit_min*100);
      14             :     }
      15     1114368 :     plane.adjust_nav_pitch_throttle();
      16     1114368 :     plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100);
      17     1114368 :     if (plane.fly_inverted()) {
      18           0 :         plane.nav_pitch_cd = -plane.nav_pitch_cd;
      19             :     }
      20     1114368 :     if (plane.failsafe.rc_failsafe && plane.g.fs_action_short == FS_ACTION_SHORT_FBWA) {
      21             :         // FBWA failsafe glide
      22           0 :         plane.nav_roll_cd = 0;
      23           0 :         plane.nav_pitch_cd = 0;
      24           0 :         SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::MIN);
      25             :     }
      26     1114368 :     RC_Channel *chan = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FBWA_TAILDRAGGER);
      27     1114368 :     if (chan != nullptr) {
      28             :         // check for the user enabling FBWA taildrag takeoff mode
      29           0 :         bool tdrag_mode = chan->get_aux_switch_pos() == RC_Channel::AuxSwitchPos::HIGH;
      30           0 :         if (tdrag_mode && !plane.auto_state.fbwa_tdrag_takeoff_mode) {
      31           0 :             if (plane.auto_state.highest_airspeed < plane.g.takeoff_tdrag_speed1) {
      32           0 :                 plane.auto_state.fbwa_tdrag_takeoff_mode = true;
      33           0 :                 plane.gcs().send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode");
      34             :             }
      35             :         }
      36             :     }
      37     1114368 : }
      38             : 
      39     1094623 : void ModeFBWA::run()
      40             : {
      41             :     // Run base class function and then output throttle
      42     1094623 :     Mode::run();
      43             : 
      44     1094623 :     output_pilot_throttle();
      45     1094623 : }