Line data    Source code

       1             : #include "Plane.h"
       2             : 
       3             : //Function that will read the radio data, limit servos and trigger a failsafe
       4             : // ----------------------------------------------------------------------------
       5             : 
       6             : /*
       7             :   allow for runtime change of control channel ordering
       8             :  */
       9       55049 : void Plane::set_control_channels(void)
      10             : {
      11             :     // the library guarantees that these are non-nullptr:
      12       55049 :     if (g.rudder_only) {
      13             :         // in rudder only mode the roll and rudder channels are the
      14             :         // same.
      15           0 :         channel_roll = &rc().get_yaw_channel();
      16             :     } else {
      17       55049 :         channel_roll = &rc().get_roll_channel();
      18             :     }
      19       55049 :     channel_pitch    = &rc().get_pitch_channel();
      20       55049 :     channel_throttle = &rc().get_throttle_channel();
      21       55049 :     channel_rudder   = &rc().get_yaw_channel();
      22             : 
      23             :     // set rc channel ranges
      24       55049 :     channel_roll->set_angle(SERVO_MAX);
      25       55049 :     channel_pitch->set_angle(SERVO_MAX);
      26       55049 :     channel_rudder->set_angle(SERVO_MAX);
      27       55049 :     if (!have_reverse_thrust()) {
      28             :         // normal operation
      29       55049 :         channel_throttle->set_range(100);
      30             :     } else {
      31             :         // reverse thrust
      32           0 :         if (have_reverse_throttle_rc_option) {
      33             :             // when we have a reverse throttle RC option setup we use throttle
      34             :             // as a range, and rely on the RC switch to get reverse thrust
      35           0 :             channel_throttle->set_range(100);
      36             :         } else {
      37           0 :             channel_throttle->set_angle(100);
      38             :         }
      39           0 :         SRV_Channels::set_angle(SRV_Channel::k_throttle, 100);
      40           0 :         SRV_Channels::set_angle(SRV_Channel::k_throttleLeft, 100);
      41           0 :         SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 100);
      42             :     }
      43             : 
      44             :     // update flap and airbrake channel assignment
      45       55049 :     channel_flap     = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FLAP);
      46       55049 :     channel_airbrake = rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRBRAKE);
      47             : 
      48             : #if HAL_QUADPLANE_ENABLED
      49             :     // update manual forward throttle channel assignment
      50       55049 :     quadplane.rc_fwd_thr_ch = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FWD_THR);
      51             : #endif
      52             : 
      53       55049 :     bool set_throttle_esc_scaling = true;
      54             : #if HAL_QUADPLANE_ENABLED
      55       55049 :     set_throttle_esc_scaling = !quadplane.enable;
      56             : #endif
      57       55049 :     if (set_throttle_esc_scaling) {
      58             :         // setup correct scaling for ESCs like the UAVCAN ESCs which
      59             :         // take a proportion of speed. For quadplanes we use AP_Motors
      60             :         // scaling
      61       33277 :         g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttleLeft);
      62       33277 :         g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttleRight);
      63       33277 :         g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle);
      64             :     }
      65       55049 : }
      66             : 
      67             : /*
      68             :   initialise RC input channels
      69             :  */
      70         692 : void Plane::init_rc_in()
      71             : {
      72             :     // set rc dead zones
      73         692 :     channel_roll->set_default_dead_zone(30);
      74         692 :     channel_pitch->set_default_dead_zone(30);
      75         692 :     channel_rudder->set_default_dead_zone(30);
      76         692 :     channel_throttle->set_default_dead_zone(30);
      77         692 : }
      78             : 
      79             : /*
      80             :   initialise RC output for main channels. This is done early to allow
      81             :   for BRD_SAFETY_DEFLT=0 and early servo control
      82             :  */
      83         692 : void Plane::init_rc_out_main()
      84             : {
      85             :     /*
      86             :       change throttle trim to minimum throttle. This prevents a
      87             :       configuration error where the user sets CH3_TRIM incorrectly and
      88             :       the motor may start on power up
      89             :      */
      90         692 :     if (!have_reverse_thrust()) {
      91         692 :         SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttle);
      92         692 :         SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttleLeft);
      93         692 :         SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttleRight);
      94             :     }
      95             : 
      96         692 :     SRV_Channels::set_failsafe_limit(SRV_Channel::k_aileron, SRV_Channel::Limit::TRIM);
      97         692 :     SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator, SRV_Channel::Limit::TRIM);
      98         692 :     SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::TRIM);
      99         692 :     SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::TRIM);
     100         692 :     SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::TRIM);
     101         692 :     SRV_Channels::set_failsafe_limit(SRV_Channel::k_rudder, SRV_Channel::Limit::TRIM);
     102             : 
     103         692 : }
     104             : 
     105             : /*
     106             :   initialise RC output channels for aux channels
     107             :  */
     108         691 : void Plane::init_rc_out_aux()
     109             : {
     110         691 :     AP::srv().enable_aux_servos();
     111             : 
     112         691 :     servos_output();
     113             :     
     114             :     // setup PWM values to send if the FMU firmware dies
     115             :     // allows any VTOL motors to shut off
     116         691 :     SRV_Channels::setup_failsafe_trim_all_non_motors();
     117         691 : }
     118             : 
     119     2736769 : void Plane::read_radio()
     120             : {
     121     2736769 :     if (!rc().read_input()) {
     122       26336 :         control_failsafe();
     123       26336 :         return;
     124             :     }
     125             : 
     126     2710433 :     if (!failsafe.rc_failsafe)
     127             :     {
     128     2709781 :         failsafe.AFS_last_valid_rc_ms = millis();
     129             :     }
     130             : 
     131     2710433 :     if (rc_throttle_value_ok()) {
     132     2709780 :         failsafe.last_valid_rc_ms = millis();
     133             :     }
     134             : 
     135     2710433 :     control_failsafe();
     136             : 
     137             : #if AP_FENCE_ENABLED
     138     2710433 :     const bool stickmixing = fence_stickmixing();
     139             : #else
     140             :     const bool stickmixing = true;
     141             : #endif
     142     2710433 :     airspeed_nudge_cm = 0;
     143     2710433 :     throttle_nudge = 0;
     144     2710433 :     if (g.throttle_nudge
     145     5420866 :         && channel_throttle->get_control_in() > 50
     146     5420866 :         && stickmixing) {
     147      373021 :         float nudge = (channel_throttle->get_control_in() - 50) * 0.02f;
     148      373021 :         if (TECS_controller.use_airspeed()) {
     149      356551 :             airspeed_nudge_cm = (aparm.airspeed_max - aparm.airspeed_cruise) * nudge * 100;
     150             :         } else {
     151       16470 :             throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
     152             :         }
     153             :     }
     154             : 
     155             : #if HAL_QUADPLANE_ENABLED
     156             :     // potentially swap inputs for tailsitters
     157     2710433 :     quadplane.tailsitter.check_input();
     158             : #endif
     159             : 
     160             : #if AP_TUNING_ENABLED
     161             :     // check for transmitter tuning changes
     162     2710433 :     tuning.check_input(control_mode->mode_number());
     163             : #endif
     164             : }
     165             : 
     166     6312884 : int16_t Plane::rudder_input(void)
     167             : {
     168     6312884 :     if (g.rudder_only != 0) {
     169             :         // in rudder only mode we discard rudder input and get target
     170             :         // attitude from the roll channel.
     171           0 :         return 0;
     172             :     }
     173             : 
     174     6312884 :     if ((flight_option_enabled(FlightOptions::DIRECT_RUDDER_ONLY)) &&
     175           0 :         !(control_mode == &mode_manual || control_mode == &mode_stabilize || control_mode == &mode_acro)) {
     176             :         // the user does not want any input except in these modes
     177           0 :         return 0;
     178             :     }
     179             : 
     180     6312884 :     if (stick_mixing_enabled()) {
     181     5443744 :         return channel_rudder->get_control_in();
     182             :     }
     183             : 
     184      869140 :     return 0;
     185             :     
     186             : }
     187             : 
     188     2738698 : void Plane::control_failsafe()
     189             : {
     190     2738698 :     if (rc_failsafe_active()) {
     191             :         // we do not have valid RC input. Set all primary channel
     192             :         // control inputs to the trim value and throttle to min
     193        2897 :         channel_roll->set_radio_in(channel_roll->get_radio_trim());
     194        2897 :         channel_pitch->set_radio_in(channel_pitch->get_radio_trim());
     195        2897 :         channel_rudder->set_radio_in(channel_rudder->get_radio_trim());
     196             : 
     197             :         // note that we don't set channel_throttle->radio_in to radio_trim,
     198             :         // as that would cause throttle failsafe to not activate
     199        2897 :         channel_roll->set_control_in(0);
     200        2897 :         channel_pitch->set_control_in(0);
     201        2897 :         channel_rudder->set_control_in(0);
     202             : 
     203        2897 :         airspeed_nudge_cm = 0;
     204        2897 :         throttle_nudge = 0;
     205             : 
     206        2897 :         switch (control_mode->mode_number()) {
     207             : #if HAL_QUADPLANE_ENABLED
     208           0 :             case Mode::Number::QSTABILIZE:
     209             :             case Mode::Number::QHOVER:
     210             :             case Mode::Number::QLOITER:
     211             :             case Mode::Number::QLAND: // throttle is ignored, but reset anyways
     212             :             case Mode::Number::QRTL:  // throttle is ignored, but reset anyways
     213             :             case Mode::Number::QACRO:
     214             : #if QAUTOTUNE_ENABLED
     215             :             case Mode::Number::QAUTOTUNE:
     216             : #endif
     217           0 :                 if (quadplane.available() && quadplane.motors->get_desired_spool_state() > AP_Motors::DesiredSpoolState::GROUND_IDLE) {
     218             :                     // set half throttle to avoid descending at maximum rate, still has a slight descent due to throttle deadzone
     219           0 :                     channel_throttle->set_control_in(channel_throttle->get_range() / 2);
     220           0 :                     break;
     221             :                 }
     222             :                 FALLTHROUGH;
     223             : #endif
     224             :             default:
     225        2897 :                 channel_throttle->set_control_in(0);
     226        2897 :                 break;
     227             :         }
     228             :     }
     229             : 
     230     2738698 :     const bool allow_failsafe_bypass = !arming.is_armed() && !is_flying() && (rc().enabled_protocols() != 0);
     231     2738698 :     const bool has_had_input = rc().has_had_rc_receiver() || rc().has_had_rc_override();
     232     2738698 :     if ((g.throttle_fs_enabled != ThrFailsafe::Enabled && !failsafe.rc_failsafe) || (allow_failsafe_bypass && !has_had_input)) {
     233             :         // If throttle fs not enabled and not in failsafe, or 
     234             :         // not flying and disarmed, don't trigger failsafe check until RC has been received for the first time  
     235       73990 :         return;
     236             :     }
     237             : 
     238     2664708 :     if (rc_failsafe_active()) {
     239             :         // we detect a failsafe from radio
     240             :         // throttle has dropped below the mark
     241        2569 :         failsafe.throttle_counter++;
     242        2569 :         if (failsafe.throttle_counter == 10) {
     243           7 :             gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe %s", "on");
     244           7 :             failsafe.rc_failsafe = true;
     245           7 :             AP_Notify::flags.failsafe_radio = true;
     246             :         }
     247        2569 :         if (failsafe.throttle_counter > 10) {
     248        1281 :             failsafe.throttle_counter = 10;
     249             :         }
     250     2662139 :     } else if(failsafe.throttle_counter > 0) {
     251             :         // we are no longer in failsafe condition
     252             :         // but we need to recover quickly
     253        1230 :         failsafe.throttle_counter--;
     254        1230 :         if (failsafe.throttle_counter > 3) {
     255           7 :             failsafe.throttle_counter = 3;
     256             :         }
     257        1230 :         if (failsafe.throttle_counter == 1) {
     258         494 :             gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe %s", "off");
     259         736 :         } else if(failsafe.throttle_counter == 0) {
     260         722 :             failsafe.rc_failsafe = false;
     261         722 :             AP_Notify::flags.failsafe_radio = false;
     262             :         }
     263             :     }
     264             : }
     265             : 
     266           0 : void Plane::trim_radio()
     267             : {
     268           0 :     if (failsafe.rc_failsafe) {
     269             :         // can't trim if we don't have valid input
     270           0 :         return;
     271             :     }
     272             : 
     273           0 :     if (plane.control_mode != &mode_manual) {
     274           0 :         gcs().send_text(MAV_SEVERITY_ERROR, "trim failed, not in manual mode");
     275           0 :         return;
     276             :     }
     277             : 
     278           0 :     if (labs(channel_roll->get_control_in()) > (channel_roll->get_range() * 0.2) ||
     279           0 :             labs(channel_pitch->get_control_in()) > (channel_pitch->get_range() * 0.2)) {
     280             :         // don't trim for extreme values - if we attempt to trim
     281             :         // more than 20 percent range left then assume the
     282             :         // sticks are not properly centered. This also prevents
     283             :         // problems with starting APM with the TX off
     284           0 :         gcs().send_text(MAV_SEVERITY_ERROR, "trim failed, large roll and pitch input");
     285           0 :         return;
     286             :     }
     287             : 
     288           0 :     if (degrees(ahrs.get_gyro().length()) > 30.0) {
     289             :         // rotating more than 30 deg/second
     290           0 :         gcs().send_text(MAV_SEVERITY_ERROR, "trim failed, large movement");
     291           0 :         return;
     292             :     }
     293             : 
     294             :     // trim main surfaces
     295           0 :     SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_aileron);
     296           0 :     SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_elevator);
     297           0 :     SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_rudder);
     298             : 
     299             :     // trim elevons
     300           0 :     SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_elevon_left);
     301           0 :     SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_elevon_right);
     302             : 
     303             :     // trim vtail
     304           0 :     SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_vtail_left);
     305           0 :     SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_vtail_right);
     306             :     
     307           0 :     if (is_zero(SRV_Channels::get_output_scaled(SRV_Channel::k_rudder))) {
     308             :         // trim differential spoilers if no rudder input
     309           0 :         SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerLeft1);
     310           0 :         SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerLeft2);
     311           0 :         SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerRight1);
     312           0 :         SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerRight2);
     313             :     }
     314             : 
     315           0 :     if (is_zero(SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto)) &&
     316           0 :         is_zero(SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap))) {
     317             :         // trim flaperons if no flap input
     318           0 :         SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_left);
     319           0 :         SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_right);
     320             :     }
     321             : 
     322             :     // now save input trims, as these have been moved to the outputs
     323           0 :     channel_roll->set_and_save_trim();
     324           0 :     channel_pitch->set_and_save_trim();
     325           0 :     channel_rudder->set_and_save_trim();
     326             : 
     327           0 :     gcs().send_text(MAV_SEVERITY_NOTICE, "trim complete");
     328             : }
     329             : 
     330             : /*
     331             :   check if throttle value is within allowed range
     332             :  */
     333    40394869 : bool Plane::rc_throttle_value_ok(void) const
     334             : {
     335    40394869 :     if (g.throttle_fs_enabled == ThrFailsafe::Disabled) {
     336           0 :         return true;
     337             :     }
     338    40394868 :     if (channel_throttle->get_reverse()) {
     339           0 :         return channel_throttle->get_radio_in() < g.throttle_fs_value;
     340             :     }
     341    40394868 :     return channel_throttle->get_radio_in() > g.throttle_fs_value;
     342             : }
     343             : 
     344             : /*
     345             :   return true if throttle level is below throttle failsafe threshold
     346             :   or RC input is invalid
     347             :  */
     348    37684436 : bool Plane::rc_failsafe_active(void) const
     349             : {
     350    37684436 :     if (!rc_throttle_value_ok()) {
     351        6349 :         return true;
     352             :     }
     353    37678087 :     if (millis() - failsafe.last_valid_rc_ms > rc().get_fs_timeout_ms()) {
     354             :         // we haven't had a valid RC frame for RC_FS_TIMEOUT seconds
     355       45929 :         return true;
     356             :     }
     357    37632157 :     return false;
     358             : }
     359             : 
     360             : /*
     361             :   expo handling for MANUAL, ACRO and TRAINING modes
     362             :  */
     363     2056307 : static float channel_expo(RC_Channel *chan, int8_t expo, bool use_dz)
     364             : {
     365     2056307 :     if (chan == nullptr) {
     366           0 :         return 0;
     367             :     }
     368     2056307 :     float rin = use_dz? chan->get_control_in() : chan->get_control_in_zero_dz();
     369     2056307 :     return SERVO_MAX * expo_curve(constrain_float(expo*0.01, 0, 1), rin/SERVO_MAX);
     370             : }
     371             : 
     372      690126 : float Plane::roll_in_expo(bool use_dz) const
     373             : {
     374      690126 :     return channel_expo(channel_roll, g2.man_expo_roll, use_dz);
     375             : }
     376             : 
     377      690126 : float Plane::pitch_in_expo(bool use_dz) const
     378             : {
     379      690126 :     return channel_expo(channel_pitch, g2.man_expo_pitch, use_dz);
     380             : }
     381             : 
     382      676055 : float Plane::rudder_in_expo(bool use_dz) const
     383             : {
     384      676055 :     return channel_expo(channel_rudder, g2.man_expo_rudder, use_dz);
     385             : }
     386             : 
     387           0 : bool Plane::throttle_at_zero(void) const
     388             : {
     389             :     /*
     390             :       true if throttle stick is at idle position...if throttle trim has been moved
     391             :        to center stick area in conjunction with sprung throttle, cannot use in_trim, must use rc_min
     392             :     */
     393           0 :     const bool center_trim = flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM);
     394           0 :     if (center_trim && channel_throttle->in_trim_dz()) {
     395           0 :         return true;
     396             :     }
     397           0 :     if (!center_trim && channel_throttle->in_min_dz()) {
     398           0 :         return true;
     399             :     }
     400           0 :     return false;
     401             : }