double getProbabilityOfValidState(void) { checkSetup(); if (probabilityOfValidState_ < 0.0) probabilityOfValidState_ = si_->probabilityOfValidState(magic::TEST_STATE_COUNT); return probabilityOfValidState_; }
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; }
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; }
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; }