Beispiel #1
0
 double getProbabilityOfValidState(void)
 {
     checkSetup();
     if (probabilityOfValidState_ < 0.0)
         probabilityOfValidState_ = si_->probabilityOfValidState(magic::TEST_STATE_COUNT);
     return probabilityOfValidState_;
 }
Beispiel #2
0
 double getAverageValidMotionLength(void)
 {
     checkSetup();
     if (averageValidMotionLength_ < 0.0)
         averageValidMotionLength_ = si_->averageValidMotionLength(magic::TEST_STATE_COUNT);
     return averageValidMotionLength_;
 }
 status_t GstPlayer::setVideoSurface (const sp < ISurface > &surface)
 {
   LOGV ("GstPlayer setVideoSurface(%p)", surface.get ());
   checkSetup ();
   mGstDriver->setVideoSurface (surface);
   return OK;
 }
 double getAverageValidMotionLength()
 {
     base::SpaceInformationPtr si = wsi_.lock();
     checkSetup(si);
     if (si && averageValidMotionLength_ < 0.0)
         averageValidMotionLength_ = si->averageValidMotionLength(magic::TEST_STATE_COUNT);
     return averageValidMotionLength_;
 }
 double getProbabilityOfValidState()
 {
     base::SpaceInformationPtr si = wsi_.lock();
     checkSetup(si);
     if (si && probabilityOfValidState_ < 0.0)
         probabilityOfValidState_ = si->probabilityOfValidState(magic::TEST_STATE_COUNT);
     return probabilityOfValidState_;
 }
 status_t GstPlayer::setDataSource (const char *url)
 {
   LOGI ("GstPlayer setDataSource(%s)", url);
   checkSetup ();
   mGstDriver->setDataSource (url);
   mGstDriver->setAudioSink (mAudioSink);
   return OK;
 }
 GstPlayer::GstPlayer ()
 {
   LOGV ("GstPlayer constructor");
   mGstDriver = new GstDriver ((MediaPlayerInterface *) this);
   mSetupDone = false;
   checkSetup ();
   //LOGV("GstPlayer constructor exit");
 }
 status_t GstPlayer::prepareAsync ()
 {
   LOGV ("GstPlayer prepareAsync");
   checkSetup ();
   // No need to run a sequence of commands.
   // The only command needed to run is PLAYER_PREPARE.
   mGstDriver->prepareAsync ();
   return OK;
 }
  status_t GstPlayer::setDataSource (int fd, int64_t offset, int64_t length)
  {

    LOGI ("GstPlayer setDataSource(%d, %lld, %lld)", fd, offset, length);
    checkSetup ();

    mGstDriver->setFdDataSource (fd, offset, length);
    mGstDriver->setAudioSink (mAudioSink);
    return OK;
  }
Beispiel #10
0
 void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
 {
     checkSetup();
     if (!proj)
     {
         msg_.inform("Attempting to use default projection.");
         proj = si_->getStateSpace()->getDefaultProjection();
     }
     if (!proj)
         throw Exception(msg_.getPrefix(), "No projection evaluator specified");
     proj->setup();
 }
 void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj, const std::string &context)
 {
     base::SpaceInformationPtr si = wsi_.lock();
     checkSetup(si);
     if (!proj && si)
     {
         OMPL_INFORM("%sAttempting to use default projection.", context.c_str());
         proj = si->getStateSpace()->getDefaultProjection();
     }
     if (!proj)
         throw Exception("No projection evaluator specified");
     proj->setup();
 }
 status_t GstPlayer::prepare ()
 {
   // prepare
   LOGV ("GstPlayer prepare");
   checkSetup ();
   mGstDriver->prepareSync ();
   {
     int width;
     int height;
     mGstDriver->getVideoSize (&width, &height);
     sendEvent (MEDIA_SET_VIDEO_SIZE, width, height);
   }
   return OK;
 }
Beispiel #13
0
Inst *CmovnsIRBuilder::process(AnalysisProcessor &ap) const {
  checkSetup();

  Inst *inst = new Inst(ap.getThreadID(), this->address, this->disas);

  try {
    this->templateMethod(ap, *inst, this->operands, "CMOVNS");
    ap.incNumberOfExpressions(inst->numberOfElements()); /* Used for statistics */
    ControlFlow::rip(*inst, ap, this->nextAddress);
  }
  catch (std::exception &e) {
    delete inst;
    throw;
  }

  return inst;
}
Inst *PcmpeqbIRBuilder::process(void) const {
  checkSetup();

  Inst *inst = new Inst(ap.getThreadID(), this->address, this->disas);

  try {
    this->templateMethod(*inst, this->operands, "PCMPEQB");
    ControlFlow::rip(*inst, this->nextAddress);
    ap.incNumberOfExpressions(inst->numberOfExpressions()); /* Used for statistics */
  }
  catch (std::exception &e) {
    delete inst;
    throw;
  }

  return inst;
}
/*!
* @brief	Initializes the whole system and runs the desired application
*
* This is the main function of the project. It calls initialization functions
* of the MCU and the sensors. In the infinite loop it repeatedly checks
* the USART module read buffer and Streams sensor data periodically (100 ms) via USART.
*
*/
int main(void)
{
	/********************* Initialize global variables **********************/

	bmf055_input_state = USART_INPUT_STATE_PRINT_DATA;
	
	/************************* Initializations ******************************/
	
	/*Initialize SAMD20 MCU*/
	system_init();
	
	/*Initialize clock module of SAMD20 MCU - Internal RC clock*/
	//clock_initialize(); // done via conf_clocks.h --> ASF
	
	/*SPI master for communicating with sensors*/
	spi_initialize();
	
	/*eeprom emulator for configuration storage */
	eeprom_emulator_initialize();
	
	/*Initialize timers */
	tc_initialize();
	
	/*Initialize UART for communication with PC*/
	usart_initialize();
	
	/*Enable the system interrupts*/
	system_interrupt_enable_global();/* All interrupts have a priority of level 0 which is the highest. */
	
	/* Initialize the sensors */
	bmf055_sensors_initialize();
	
	readEEPROM();
	checkFirstTime(0);
	//readEEPROM();
	
	configureReceiver();
	initSensors();
	previousTime = micros();
	calibratingG = 400;
	f.SMALL_ANGLES_25=1; // important for gyro only conf
  if(conf.copterType == 0){//0=Bi,1=Tri,2=QUADP,3=QUADX,4=Y4,5=Y6,6=H6P,7=H6X,8=Vtail4
    MULTITYPE      = 4;
    NUMBER_MOTOR   = 2;
  }
  if(conf.copterType == 1){
    MULTITYPE      = 1;
    NUMBER_MOTOR   = 3;
  }
  if(conf.copterType == 2){
    MULTITYPE      = 2;
    NUMBER_MOTOR   = 4;
  }
  if(conf.copterType == 3){
    MULTITYPE      = 3;
    NUMBER_MOTOR   = 4;
  }
  if(conf.copterType == 4){
    MULTITYPE      = 9;
    NUMBER_MOTOR   = 4;
  }
  if(conf.copterType == 5){
    MULTITYPE      = 6;
    NUMBER_MOTOR   = 6;
  }
  if(conf.copterType == 6){
    MULTITYPE      = 7;
    NUMBER_MOTOR   = 6;
  }
  if(conf.copterType == 7){
    MULTITYPE      = 10;
    NUMBER_MOTOR   = 6;
  }     
  if(conf.copterType == 8){
    MULTITYPE      = 17;
    NUMBER_MOTOR   = 4;
  }
  
  initOutput();
	
	/************************** Infinite Loop *******************************/
	while (true)
	{

		
		static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
  static uint8_t beepon = 0;
  uint8_t axis,i;
  int16_t error,errorAngle;
  int16_t delta,deltaSum;
  int16_t PTerm=0,ITerm=0,PTermACC=0,ITermACC=0,PTermGYRO=0,ITermGYRO=0,DTerm=0;
  static int16_t lastGyro[3] = {0,0,0};
  static int16_t delta1[3],delta2[3];
  static int16_t errorGyroI[3] = {0,0,0};
  static int16_t errorAngleI[2] = {0,0};
  static uint32_t rcTime  = 0;
  static uint32_t BeepTime  = 0;
  static uint8_t stickarmed = 0;
  //static int16_t initialThrottleHold;
  
  if(!rcOptions[BOXARM] && stickarmed == 0 && f.ARMED == 0){
    if(rcData[YAW]<conf.MINCHECK && rcData[ROLL]>conf.MAXCHECK){
      conf.calibState=1;
      writeParams(1);
      while(true){
        //blinkLED(10,30,1);
      }      
    }
  } 
 
  while(SetupMode == 1){
    checkSetup();
  }
 
 
  if(conf.RxType == 1 || conf.RxType == 2){
    if (rcFrameComplete) computeRC();
  }
  
  if(!rcOptions[BOXARM] && stickarmed == 0) {
    f.ARMED = 0;
  }

  if (currentTime > rcTime ) { // 50Hz
    rcTime = currentTime + 20000;
    if(failsave < 250)failsave++;
    debug[0] = failsave;
    if(conf.RxType != 1 && conf.RxType != 2){
      computeRC();
    }
  
    if ((rcData[THROTTLE] < conf.MINCHECK && s3D == 0) || (rcData[THROTTLE] > (1500-conf.MIDDLEDEADBAND) && rcData[THROTTLE] < (1500+conf.MIDDLEDEADBAND) && s3D == 1 && f.ARMED == 0)) {
      errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0;
      errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;

      rcDelayCommand++;
      if (rcData[YAW] < conf.MINCHECK && rcData[PITCH] < conf.MINCHECK && !f.ARMED) {
        if (rcDelayCommand == 20 && failsave < 20) {
          calibratingG=400;
        }
      }else if (rcData[YAW] > conf.MAXCHECK && rcData[PITCH] > conf.MAXCHECK && !f.ARMED) {
        if (rcDelayCommand == 20) {
          previousTime = micros();
        }
      }else if (conf.activate[BOXARM] > 0) {
        if ( rcOptions[BOXARM] && f.OK_TO_ARM && good_calib) {
	  f.ARMED = 1;
          stickarmed = 0;
        } else if (f.ARMED) f.ARMED = 0;
        rcDelayCommand = 0;
        
      
      } else if ( (rcData[YAW] < conf.MINCHECK ) && f.ARMED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 0) {
        if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
      } else if ( (rcData[YAW] > conf.MAXCHECK ) && rcData[PITCH] < conf.MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 0) {
        if (rcDelayCommand == 20 && good_calib) {
	  f.ARMED = 1;
          stickarmed = 1;
        }
  
       } else if ( (rcData[ROLL] < conf.MINCHECK ) && f.ARMED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 1) {
        if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
      } else if ( (rcData[ROLL] > conf.MAXCHECK ) && rcData[PITCH] < conf.MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 1) {
        if (rcDelayCommand == 20 && good_calib) {
	  f.ARMED = 1;
          stickarmed = 1;
        }       
        
        
      } else
        rcDelayCommand = 0;
    } else if (rcData[THROTTLE] > conf.MAXCHECK && !f.ARMED) {
      if (rcData[YAW] < conf.MINCHECK && rcData[PITCH] < conf.MINCHECK) {        // throttle=max, yaw=left, pitch=min
        if (rcDelayCommand == 20) calibratingA=400;
        rcDelayCommand++;
      } else if (rcData[PITCH] > conf.MAXCHECK) {
         conf.angleTrim[PITCH]+=2;writeParams(1);
      } else if (rcData[PITCH] < conf.MINCHECK) {
         conf.angleTrim[PITCH]-=2;writeParams(1);
      } else if (rcData[ROLL] > conf.MAXCHECK) {
         conf.angleTrim[ROLL]+=2;writeParams(1);
      } else if (rcData[ROLL] < conf.MINCHECK) {
         conf.angleTrim[ROLL]-=2;writeParams(1);
      } else {
        rcDelayCommand = 0;
      }
    }
    
    

    uint16_t auxState = 0;
    for(i=0;i<4;i++)
      auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (1300<rcData[AUX1+i] && rcData[AUX1+i]<1700)<<(3*i+1) | (rcData[AUX1+i]>1700)<<(3*i+2);
    for(i=0;i<CHECKBOXITEMS;i++)
      rcOptions[i] = (auxState & conf.activate[i])>0;
      
      
     if(failsave > 200 && f.ARMED){
      rcOptions[BOXACC] = 1;
      s3D = 0;
      rcData[THROTTLE] = 1190;
      rcCommand[THROTTLE] = 1190;
    }

    if (rcOptions[BOXACC] && s3D == 0) { 
      // bumpless transfer to Level mode
      if (!f.ACC_MODE) {
        errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
        f.ACC_MODE = 1;
      }  
    } else {
      // failsafe support
      f.ACC_MODE = 0;
    }
    if (rcOptions[BOXBEEP]) { 
      f.FSBEEP = 1;
      if(currentTime > BeepTime){
        BeepTime = currentTime + 50000;
        if(beepon == 0){
          if(conf.RxType == 0){
            //digitalWrite(A2,HIGH); 
          }else{
            //digitalWrite(8,HIGH); 
          }          
          beepon = 1;
        }else{
          if(conf.RxType == 0){
            //digitalWrite(A2,LOW); 
          }else{
            //digitalWrite(8,LOW); 
          }
          beepon = 0;
        }
      }
    } else {
      f.FSBEEP = 0;
      if(conf.RxType == 0){
        //digitalWrite(A2,LOW); 
      }else{
        //digitalWrite(8,LOW); 
      }
    }    

    
    if (rcOptions[BOXHORIZON] && s3D == 0) { 
      // bumpless transfer to Horizon mode
      if (!f.HORIZON_MODE) {
        errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
        f.HORIZON_MODE = 1;
      }  
    } else {
      f.HORIZON_MODE = 0;
    }
    
    if (rcOptions[BOX3D] && conf.F3D == 1) {  
      if(f.ARMED == 0 && s3D == 0){
        s3D = 1;
        f.ACC_MODE = 0;
        f.HORIZON_MODE = 0;
      }
    } else if(f.ARMED == 0){
      s3D = 0;
    }   
    

    if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1;
  }
 
  computeIMU();
  int16_t prop;
  if (f.HORIZON_MODE)
    prop = max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])); // range [0;500]  
  
  
  if (f.ACC_MODE){
    if(Zadd > 0)Zadd--;
    if(Zadd < 0)Zadd++;
  }else{
    Zadd = 0;
  }

  

  //**** PITCH & ROLL & YAW PID ****    
  for(axis=0;axis<3;axis++) {
    if ((f.ACC_MODE || f.HORIZON_MODE) && axis<2 ) { //LEVEL MODE
      // 50 degrees max inclination
      errorAngle = constrain(2*rcCommand[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
      #ifdef LEVEL_PDF
        PTermACC      = -(int32_t)angle[axis]*conf.P8[PIDLEVEL]/100 ;
      #else  
        PTermACC      = (int32_t)errorAngle*conf.P8[PIDLEVEL]/100 ;                          // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768   16 bits is ok for result
      #endif
      PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5);

      errorAngleI[axis]  = constrain(errorAngleI[axis]+errorAngle,-10000,+10000);    // WindUp     //16 bits is ok here
      ITermACC           = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12;            // 32 bits is needed for calculation:10000*I8 could exceed 32768   16 bits is ok for result
    }
    if ( !f.ACC_MODE || f.HORIZON_MODE || axis == 2 ) { // MODE relying on GYRO or YAW axis
      if (abs(rcCommand[axis])<350) error =          rcCommand[axis]*10*8/conf.P8[axis] ; // 16 bits is needed for calculation: 350*10*8 = 28000      16 bits is ok for result if P8>2 (P>0.2)
                               else error = (int32_t)rcCommand[axis]*10*8/conf.P8[axis] ; // 32 bits is needed for calculation: 500*5*10*8 = 200000   16 bits is ok for result if P8>2 (P>0.2)
      error -= gyroData[axis];

      PTermGYRO = rcCommand[axis];
      
      errorGyroI[axis]  = constrain(errorGyroI[axis]+error,-16000,+16000);          // WindUp   16 bits is ok here
      if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
      ITermGYRO         = (errorGyroI[axis]/125*conf.I8[axis])>>6;                                   // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
    }
    if ( f.HORIZON_MODE && axis<2) {
      PTerm = ((int32_t)PTermACC*(500-prop) + (int32_t)PTermGYRO*prop)/500;
      ITerm = ((int32_t)ITermACC*(500-prop) + (int32_t)ITermGYRO*prop)/500;
    } else {
      if ( f.ACC_MODE && axis<2) {
        PTerm = PTermACC;
        ITerm = ITermACC;
      } else {
        PTerm = PTermGYRO;
        ITerm = ITermGYRO;
      }
    }
    if (abs(gyroData[axis])<160) PTerm -=          gyroData[axis]*dynP8[axis]/10/8; // 16 bits is needed for calculation   160*200 = 32000         16 bits is ok for result
                            else PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; // 32 bits is needed for calculation   

    delta          = gyroData[axis] - lastGyro[axis];                               // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
    lastGyro[axis] = gyroData[axis];
    deltaSum       = delta1[axis]+delta2[axis]+delta;
    delta2[axis]   = delta1[axis];
    delta1[axis]   = delta;
 
    if (abs(deltaSum)<640) DTerm = (deltaSum*dynD8[axis])>>5;                       // 16 bits is needed for calculation 640*50 = 32000           16 bits is ok for result 
                      else DTerm = ((int32_t)deltaSum*dynD8[axis])>>5;              // 32 bits is needed for calculation
                      
    axisPID[axis] =  PTerm + ITerm - DTerm;
  }