Line data    Source code

       1             : #include "Plane.h"
       2             : 
       3             : /*
       4             :  *  failsafe support
       5             :  *  Andrew Tridgell, December 2011
       6             :  */
       7             : 
       8             : /*
       9             :  *  our failsafe strategy is to detect main loop lockup and switch to
      10             :  *  passing inputs straight from the RC inputs to RC outputs.
      11             :  */
      12             : 
      13             : /*
      14             :  *  this failsafe_check function is called from the core timer interrupt
      15             :  *  at 1kHz.
      16             :  */
      17    67467802 : void Plane::failsafe_check(void)
      18             : {
      19             :     static uint16_t last_ticks;
      20             :     static uint32_t last_timestamp;
      21             :     static bool in_failsafe;
      22    67467802 :     uint32_t tnow = micros();
      23             : 
      24    67467802 :     const uint16_t ticks = scheduler.ticks();
      25    67467802 :     if (ticks != last_ticks) {
      26             :         // the main loop is running, all is OK
      27     8377547 :         last_ticks = ticks;
      28     8377547 :         last_timestamp = tnow;
      29     8377547 :         in_failsafe = false;
      30     8377547 :         return;
      31             :     }
      32             : 
      33    59090255 :     if (tnow - last_timestamp > 200000) {
      34             :         // we have gone at least 0.2 seconds since the main loop
      35             :         // ran. That means we're in trouble, or perhaps are in
      36             :         // an initialisation routine or log erase. Start passing RC
      37             :         // inputs through to outputs
      38         749 :         in_failsafe = true;
      39             :     }
      40             : 
      41    59090255 :     if (in_failsafe && tnow - last_timestamp > 20000) {
      42             : 
      43             :         // ensure we have the latest RC inputs
      44       60581 :         rc().read_input();
      45             : 
      46       60581 :         last_timestamp = tnow;
      47             : 
      48       60581 :         rc().read_input();
      49             : 
      50             : #if AP_ADVANCEDFAILSAFE_ENABLED
      51       60581 :         if (in_calibration) {
      52             :             // tell the failsafe system that we are calibrating
      53             :             // sensors, so don't trigger failsafe
      54        3426 :             afs.heartbeat();
      55             :         }
      56             : #endif
      57             : 
      58       60581 :         if (RC_Channels::get_valid_channel_count() < 5) {
      59             :             // we don't have any RC input to pass through
      60         148 :             return;
      61             :         }
      62             : 
      63             :         // pass RC inputs to outputs every 20ms
      64       60433 :         RC_Channels::clear_overrides();
      65             : 
      66       60433 :         float roll = roll_in_expo(false);
      67       60433 :         float pitch = pitch_in_expo(false);
      68       60433 :         float throttle = get_throttle_input(true);
      69       60433 :         float rudder = rudder_in_expo(false);
      70             : 
      71       60433 :         if (!arming.is_armed_and_safety_off()) {
      72       60433 :             throttle = 0;
      73             :         }
      74             :         
      75             :         // setup secondary output channels that don't have
      76             :         // corresponding input channels
      77       60433 :         SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll);
      78       60433 :         SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch);
      79       60433 :         SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder);
      80       60433 :         SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder);
      81       60433 :         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
      82             : 
      83             :         // this is to allow the failsafe module to deliberately crash 
      84             :         // the plane. Only used in extreme circumstances to meet the
      85             :         // OBC rules
      86             : #if AP_ADVANCEDFAILSAFE_ENABLED
      87       60433 :         if (afs.should_crash_vehicle()) {
      88           0 :             afs.terminate_vehicle();
      89           0 :             if (!afs.terminating_vehicle_via_landing()) {
      90           0 :                 return;
      91             :             }
      92             :         }
      93             : #endif
      94             : 
      95             :         // setup secondary output channels that do have
      96             :         // corresponding input channels
      97       60433 :         SRV_Channels::copy_radio_in_out(SRV_Channel::k_manual, true);
      98       60433 :         SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 0.0);
      99       60433 :         SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 0.0);
     100             : 
     101             :         // setup flaperons
     102       60433 :         flaperon_update();
     103             : 
     104       60433 :         servos_output();
     105             : 
     106             :         // in SITL we send through the servo outputs so we can verify
     107             :         // we're manipulating surfaces
     108             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     109       60433 :         GCS_MAVLINK *chan = gcs().chan(0);
     110       60433 :         if (HAVE_PAYLOAD_SPACE(chan->get_chan(), SERVO_OUTPUT_RAW)) {
     111       60433 :             chan->send_servo_output_raw();
     112             :         }
     113             : #endif
     114             :     }
     115             : }