Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   reverse thrust support functions
      17             :  */
      18             : #include "Plane.h"
      19             : 
      20             : // Check USE_REV_THRUST paramater for the given reverse thrust option
      21           0 : bool Plane::reverse_thrust_enabled(UseReverseThrust use_reverse_thrust_option) const
      22             : {
      23           0 :     return (g.use_reverse_thrust.get() & int32_t(use_reverse_thrust_option)) != 0;
      24             : }
      25             : 
      26             : /*
      27             :   see if reverse thrust should be allowed in the current flight state
      28             :  */
      29           0 : bool Plane::allow_reverse_thrust(void) const
      30             : {
      31             :     // Reverse thrust is not configured, so it should never be allowed
      32           0 :     if (!have_reverse_thrust()) {
      33           0 :         return false;
      34             :     }
      35             : 
      36             :     // check if we should allow reverse thrust
      37           0 :     bool allow = false;
      38             : 
      39           0 :     switch (control_mode->mode_number()) {
      40           0 :     case Mode::Number::AUTO:
      41             :         {
      42           0 :         uint16_t nav_cmd = mission.get_current_nav_cmd().id;
      43             : 
      44             :         // never allow reverse thrust during takeoff
      45           0 :         if (nav_cmd == MAV_CMD_NAV_TAKEOFF) {
      46           0 :             return false;
      47             :         }
      48             : 
      49             :         // always allow regardless of mission item
      50           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::AUTO_ALWAYS);
      51             : 
      52             :         // landing
      53           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::AUTO_LAND_APPROACH) &&
      54             :                 (nav_cmd == MAV_CMD_NAV_LAND);
      55             : 
      56             :         // LOITER_TO_ALT
      57           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::AUTO_LOITER_TO_ALT) &&
      58             :                 (nav_cmd == MAV_CMD_NAV_LOITER_TO_ALT);
      59             : 
      60             :         // any Loiter (including LOITER_TO_ALT)
      61           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::AUTO_LOITER_ALL) &&
      62           0 :                     (nav_cmd == MAV_CMD_NAV_LOITER_TIME ||
      63           0 :                      nav_cmd == MAV_CMD_NAV_LOITER_TO_ALT ||
      64           0 :                      nav_cmd == MAV_CMD_NAV_LOITER_TURNS ||
      65             :                      nav_cmd == MAV_CMD_NAV_LOITER_UNLIM);
      66             : 
      67             :         // waypoints
      68           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::AUTO_WAYPOINT) &&
      69           0 :                     (nav_cmd == MAV_CMD_NAV_WAYPOINT ||
      70             :                      nav_cmd == MAV_CMD_NAV_SPLINE_WAYPOINT);
      71             : 
      72             :         // we are on a landing pattern
      73           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::AUTO_LANDING_PATTERN) &&
      74           0 :                 mission.get_in_landing_sequence_flag();
      75             :         }
      76           0 :         break;
      77             : 
      78           0 :     case Mode::Number::LOITER:
      79           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::LOITER);
      80           0 :         break;
      81           0 :     case Mode::Number::RTL:
      82           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::RTL);
      83           0 :         break;
      84           0 :     case Mode::Number::CIRCLE:
      85           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::CIRCLE);
      86           0 :         break;
      87           0 :     case Mode::Number::CRUISE:
      88           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::CRUISE);
      89           0 :         break;
      90           0 :     case Mode::Number::FLY_BY_WIRE_B:
      91           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::FBWB);
      92           0 :         break;
      93           0 :     case Mode::Number::AVOID_ADSB:
      94             :     case Mode::Number::GUIDED:
      95           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::GUIDED);
      96           0 :         break;
      97           0 :     case Mode::Number::TAKEOFF:
      98           0 :         allow = false;
      99           0 :         break;
     100           0 :     case Mode::Number::FLY_BY_WIRE_A:
     101           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::FBWA);
     102           0 :         break;
     103           0 :     case Mode::Number::ACRO:
     104           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::ACRO);
     105           0 :         break;
     106           0 :     case Mode::Number::STABILIZE:
     107           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::STABILIZE);
     108           0 :         break;
     109           0 :     case Mode::Number::THERMAL:
     110           0 :         allow |= reverse_thrust_enabled(UseReverseThrust::THERMAL);
     111           0 :         break;
     112           0 :     default:
     113             :         // all other control_modes allow independent of mask(MANUAL)
     114           0 :         allow = true;
     115           0 :         break;
     116             :     }
     117             : 
     118           0 :     return allow;
     119             : }
     120             : 
     121             : /*
     122             :   return true if we are configured to support reverse thrust
     123             :  */
     124     2745040 : bool Plane::have_reverse_thrust(void) const
     125             : {
     126     2745040 :     return aparm.throttle_min < 0;
     127             : }
     128             : 
     129             : /*
     130             :   return control in from the radio throttle channel.
     131             :  */
     132    12061705 : float Plane::get_throttle_input(bool no_deadzone) const
     133             : {
     134    12061705 :     if (!rc().has_valid_input()) {
     135             :         // Return 0 if there is no valid input
     136       41542 :         return 0.0;
     137             :     }
     138             :     float ret;
     139    12020163 :     if (no_deadzone) {
     140     1769113 :         ret = channel_throttle->get_control_in_zero_dz();
     141             :     } else {
     142    10251050 :         ret = channel_throttle->get_control_in();
     143             :     }
     144    12020163 :     if (reversed_throttle) {
     145             :         // RC option for reverse throttle has been set
     146          32 :         ret = -ret;
     147             :     }
     148    12020163 :     return ret;
     149             : }
     150             : 
     151             : /*
     152             :   return control in from the radio throttle channel with curve giving mid-stick equal to TRIM_THROTTLE.
     153             :  */
     154     1131208 : float Plane::get_adjusted_throttle_input(bool no_deadzone) const
     155             : {
     156     1131208 :     if (!rc().has_valid_input()) {
     157             :         // Return 0 if there is no valid input
     158          85 :         return 0.0;
     159             :     }
     160     2262246 :     if ((plane.channel_throttle->get_type() != RC_Channel::ControlType::RANGE) ||
     161     1131123 :         (flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)) == 0) {
     162     1131123 :        return  get_throttle_input(no_deadzone);
     163             :     }
     164           0 :     float ret = channel_throttle->get_range() * throttle_curve(aparm.throttle_cruise * 0.01, 0, 0.5 + 0.5*channel_throttle->norm_input());
     165           0 :     if (reversed_throttle) {
     166             :         // RC option for reverse throttle has been set
     167           0 :         return -ret;
     168             :     }
     169           0 :     return ret;
     170             : }