MultiWii

Code: Select all

// baro only
# define GROUND_CONTACT  1  

// sonar
//#landingGearMounOffsett 15 
//# define GROUND_CONTACT  (0>=(sonarAlt - landingGearMounOffsett))  


    #if defined(FAILSAFE)
      if ( failsafeCnt > (5*FAILSAVE_DELAY) && armed==1) {              
        if (BaroPID>0){  
          // under target altitude and moving up
          AltHold -= 35;  // set target altitude down 
          failsafeCnt=5*FAILSAVE_DELAY; // until we reach target altitude again
        }else{
          //over target altitude
          if ((failsafeCnt > (5*FAILSAVE_OFF_DELAY) )  || GROUND_CONTACT ){
               //if we cannot reach target altitude , or landed
               // maybe initial throttle was too high, time to stop anyway
              armed = 0;   // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
              okToArm = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
          }
        }
        failsafeEvents++;
      }
      failsafeCnt++;
    #endif
 

Code: Select all

if(setfailsafe==0){
        for(i=0; i<3; i++) rcData[i] = MIDRC; 
        // if we are near hovering speed or in baro mode , do not change throttle
        // else ensure to engage failsafe with safe throttle 
        if( (10>=(rcCommand[THROTTLE]-FAILSAVE_THR0TTLE)) && (baroMode==0)){
          rcCommand[THROTTLE]=FAILSAVE_THR0TTLE;
        }
        rcData[THROTTLE] = rcCommand[THROTTLE];
        setfailsafe=1;
        baroMode = 1;

      }

Code: Select all

    
#define FAILSAVE_BARO_OFF_DELAY (FAILSAVE_OFF_DELAY+20)

#define FAILSAVE_PID_STEP 0  
#define FAILSAVE_ALT_STEP 35

//#define FAILSAVE_PID_STEP 15
//#define FAILSAVE_ALT_STEP 25

        if ((BaroPID-FAILSAVE_PID_STEP)>0){  
          // under target altitude and moving up
          AltHold -= FAILSAVE_ALT_STEP;  // set target altitude down 
          failsafeCnt=5*FAILSAVE_DELAY; // until we reach target altitude again
        }else{
          //over target altitude
          if ((failsafeCnt > (5*FAILSAVE_BARO_OFF_DELAY) )  || GROUND_CONTACT ){
              armed = 0;   // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
              okToArm = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
          }
        }