Line data    Source code

       1             : #pragma once
       2             : 
       3             : /*
       4             :    This program is free software: you can redistribute it and/or modify
       5             :    it under the terms of the GNU General Public License as published by
       6             :    the Free Software Foundation, either version 3 of the License, or
       7             :    (at your option) any later version.
       8             : 
       9             :    This program is distributed in the hope that it will be useful,
      10             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
      11             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      12             :    GNU General Public License for more details.
      13             : 
      14             :    You should have received a copy of the GNU General Public License
      15             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      16             :  */
      17             : /*
      18             :   Outback Challenge Failsafe module
      19             : 
      20             :   Andrew Tridgell and CanberraUAV, August 2012
      21             : */
      22             : 
      23             : #include "AP_AdvancedFailsafe_config.h"
      24             : 
      25             : #if AP_ADVANCEDFAILSAFE_ENABLED
      26             : 
      27             : #include <AP_Common/AP_Common.h>
      28             : #include <AP_Param/AP_Param.h>
      29             : #include <inttypes.h>
      30             : #include <AP_Common/Location.h>
      31             : 
      32             : class AP_AdvancedFailsafe
      33             : {
      34             : public:
      35             :     enum control_mode {
      36             :         AFS_MANUAL     = 0,
      37             :         AFS_STABILIZED = 1,
      38             :         AFS_AUTO       = 2
      39             :     };
      40             : 
      41             :     enum state {
      42             :         STATE_PREFLIGHT       = 0,
      43             :         STATE_AUTO            = 1,
      44             :         STATE_DATA_LINK_LOSS  = 2,
      45             :         STATE_GPS_LOSS        = 3
      46             :     };
      47             : 
      48             :     enum terminate_action {
      49             :         TERMINATE_ACTION_TERMINATE = 42,
      50             :         TERMINATE_ACTION_LAND      = 43
      51             :     };
      52             : 
      53             :     /* Do not allow copies */
      54             :     CLASS_NO_COPY(AP_AdvancedFailsafe);
      55             : 
      56             :     // Constructor
      57         692 :     AP_AdvancedFailsafe()
      58         692 :         {
      59         692 :             AP_Param::setup_object_defaults(this, var_info);
      60         692 :             if (_singleton != nullptr) {
      61           0 :                 AP_HAL::panic("AP_AdvancedFailsafe must be singleton");
      62             :             }
      63             : 
      64         692 :             _singleton = this;
      65         692 :             _state = STATE_PREFLIGHT;
      66         692 :             _terminate.set(0);
      67             : 
      68         692 :             _saved_wp = 0;
      69         692 :         }
      70             : 
      71             :     // get singleton instance
      72          29 :     static AP_AdvancedFailsafe *get_singleton(void) {
      73          29 :         return _singleton;
      74             :     }
      75             : 
      76           9 :     bool enabled() { return _enable; }
      77             : 
      78             :     // check that everything is OK
      79             :     void check(uint32_t last_valid_rc_ms);
      80             : 
      81             :     // generate heartbeat msgs, so external failsafe boards are happy
      82             :     // during sensor calibration
      83             :     void heartbeat(void);
      84             : 
      85             :     //return true if we should jump to _wp_comms_hold
      86             :     bool should_use_comms_hold(void);
      87             : 
      88             :     // return true if we are terminating (deliberately crashing the vehicle)
      89             :     bool should_crash_vehicle(void);
      90             : 
      91             :     // enables or disables a GCS based termination, returns true if AFS is in the desired termination state
      92             :     bool gcs_terminate(bool should_terminate, const char *reason);
      93             : 
      94             :     // called to set all outputs to termination state
      95             :     virtual void terminate_vehicle(void) = 0;
      96             : 
      97             :     // for holding parameters
      98             :     static const struct AP_Param::GroupInfo var_info[];
      99             : 
     100         303 :     bool terminating_vehicle_via_landing() const {
     101         303 :         return _terminate_action == TERMINATE_ACTION_LAND;
     102             :     };
     103             : 
     104             : protected:
     105             :     // setup failsafe values for if FMU firmware stops running
     106             :     virtual void setup_IO_failsafe(void) = 0;
     107             : 
     108             :     // return the AFS mapped control mode
     109             :     virtual enum control_mode afs_mode(void) = 0;
     110             : 
     111             :     //to force entering auto mode when datalink loss 
     112             :     virtual void set_mode_auto(void) = 0;
     113             : 
     114             :     enum state _state;
     115             : 
     116             :     AP_Int8 _enable;
     117             :     // digital output pins for communicating with the failsafe board
     118             :     AP_Int8 _heartbeat_pin;
     119             :     AP_Int8 _manual_pin;
     120             :     AP_Int8 _terminate_pin;
     121             :     AP_Int8 _terminate;
     122             :     AP_Int8 _terminate_action;
     123             : 
     124             :     // waypoint numbers to jump to on failsafe conditions
     125             :     AP_Int8 _wp_comms_hold;
     126             :     AP_Int8 _wp_gps_loss;
     127             : 
     128             :     AP_Float _qnh_pressure;
     129             :     AP_Int32 _amsl_limit;
     130             :     AP_Int32 _amsl_margin_gps;
     131             :     AP_Float _rc_fail_time_seconds;
     132             :     AP_Float _gcs_fail_time_seconds;
     133             :     AP_Int8  _max_gps_loss;
     134             :     AP_Int8  _max_comms_loss;
     135             :     AP_Int8  _enable_geofence_fs;
     136             :     AP_Int8  _enable_RC_fs;
     137             :     AP_Int8  _rc_term_manual_only;
     138             :     AP_Int8  _enable_dual_loss;
     139             :     AP_Int16  _max_range_km;
     140             : 
     141             :     bool _heartbeat_pin_value;
     142             : 
     143             :     // saved waypoint for resuming mission
     144             :     uint16_t _saved_wp;
     145             :     
     146             :     // number of times we've lost GPS
     147             :     uint8_t _gps_loss_count;
     148             : 
     149             :     // number of times we've lost data link
     150             :     uint8_t _comms_loss_count;
     151             : 
     152             :     // last comms loss time
     153             :     uint32_t _last_comms_loss_ms;
     154             : 
     155             :     // last GPS loss time
     156             :     uint32_t _last_gps_loss_ms;
     157             : 
     158             :     // have the failsafe values been setup?
     159             :     bool _failsafe_setup;
     160             : 
     161             :     Location _first_location;
     162             :     bool _have_first_location;
     163             :     uint32_t _term_range_notice_ms;
     164             : 
     165             :     bool check_altlimit(void);
     166             : 
     167             : private:
     168             :     static AP_AdvancedFailsafe *_singleton;
     169             : 
     170             :     // update maximum range check
     171             :     void max_range_update();
     172             : 
     173             :     AP_Int16 options;
     174             :     enum class Option {
     175             :         CONTINUE_AFTER_RECOVERED = (1U<<0),
     176             :         GCS_FS_ALL_AUTONOMOUS_MODES = (1U<<1),
     177             :         CONTINUE_IF_ALREADY_IN_RETURN_PATH = (1U<<2),
     178             :     };
     179           0 :     bool option_is_set(Option option) const {
     180           0 :         return (options.get() & int16_t(option)) != 0;
     181             :     }
     182             : 
     183             :     bool gps_altitude_ok() const;
     184             : };
     185             : 
     186             : namespace AP {
     187             :     AP_AdvancedFailsafe *advancedfailsafe();
     188             : };
     189             : 
     190             : #endif  // AP_ADVANCEDFAILSAFE_ENABLED