// probe and initialise the sensor bool AP_Airspeed_I2C::init() { _dev = hal.i2c_mgr->get_device(MS4525D0_I2C_BUS, MS4525D0_I2C_ADDR); // take i2c bus sempahore if (!_dev || !_dev->get_semaphore()->take(200)) { return false; } // lots of retries during probe _dev->set_retries(5); _measure(); hal.scheduler->delay(10); _collect(); _dev->get_semaphore()->give(); // drop to 2 retries for runtime _dev->set_retries(2); if (_last_sample_time_ms != 0) { _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_Airspeed_I2C::_timer, bool)); return true; }
void _oneMetropolisStepWithMeasurement(int* config, double* delta, double beta, int m, int acstep, double* E1,double* E2,double* M1,double* M2){ int i; for (i=0; i<(m*m); i++){ _oneMetropolisStep(config, delta, beta, m, acstep); } _measure(config, m, E1, E2, M1, M2); }
void RcInput::_cycle() { _measure(); if (!_shouldExit) { work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, USEC2TICK(RCINPUT_MEASURE_INTERVAL_US)); } }
// 1kHz timer void AP_Airspeed_I2C::_timer(void) { AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); if (!i2c_sem->take_nonblocking()) return; if (_measurement_started_ms == 0) { _measure(); i2c_sem->give(); return; } if ((hal.scheduler->millis() - _measurement_started_ms) > 10) { _collect(); // start a new measurement _measure(); } i2c_sem->give(); }
int BBBlueADC::init() { rc_init(); int ret = DriverFramework::VirtDevObj::init(); if (ret != PX4_OK) { PX4_ERR("init failed"); return ret; } _measure(); // start the initial conversion so that the test command right // after the start command can return values return PX4_OK; }
// probe and initialise the sensor bool AP_Airspeed_I2C::init(void) { // get pointer to i2c bus semaphore AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); // take i2c bus sempahore if (!i2c_sem->take(200)) return false; _measure(); hal.scheduler->delay(10); _collect(); i2c_sem->give(); if (_last_sample_time_ms != 0) { //hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_Airspeed_I2C::_timer)); return true; } return false; }
int MS5803_I2C::ioctl(unsigned operation, unsigned &arg) { int ret; switch (operation) { case IOCTL_RESET: ret = _reset(); break; case IOCTL_MEASURE: ret = _measure(arg); break; default: ret = EINVAL; } return ret; }
int MS5611_I2C::ioctl(device::file_t *handlep, int cmd, unsigned long arg) { int ret; switch (cmd) { case IOCTL_RESET: ret = _reset(); break; case IOCTL_MEASURE: ret = _measure(arg); break; default: ret = EINVAL; } return ret; }
//Actual Kalman Filter equations void ukf::stepUKF(std::chrono::milliseconds dT, Eigen::VectorXd measurement, Eigen::VectorXd control){ for(auto j = std::chrono::milliseconds(0); j < (dT>_dT ? dT : _dT); j+=_dT){ _prevState= _state; _prevCovar= _covar; _statistic = 1/(2*(_states+_lambda)); _sqrtP = _covar.sqrt(); double coeff = std::sqrt(_states+_lambda); _sigmaPoints.col(0) = _prevState; _sigmaPoints.block(0, 1, _states, _states) = coeff*_sqrtP; _sigmaPoints.block(0, 1+_states, _states, _states) = -1*coeff*_sqrtP; for(auto i = 1; i < 2*_states+1; i++){ _sigmaPoints.col(i) += _prevState; } for(auto i = 0; i < 2*_states+1; i++){ _sigmaPoints.col(i) = _transform(_sigmaPoints.col(i), static_cast<double>(std::min(_dT, dT).count())); } _state = _sigmaPoints.col(0); for(auto i = 1; i < 2*_states+1; i++){ _state+= _sigmaPoints.col(i); } _state *= _statistic; _covar = _Q; for(auto i = 0; i < 2*_states+1; i++){ _covar += (_sigmaPoints.col(i) - _prevState)*(static_cast<Eigen::MatrixXd>(_sigmaPoints.col(i) - _prevState)).transpose(); } _state *= _statistic; for(auto i = 0; i < 2*_states+1; i++){ _measureSigmaPoints.col(i) = _measure(_sigmaPoints.col(i), static_cast<double>(std::min(_dT, dT).count())); } } }
int MS5611_SPI::ioctl(unsigned operation, unsigned &arg) { int ret; switch (operation) { case IOCTL_RESET: ret = _reset(); break; case IOCTL_MEASURE: ret = _measure(arg); break; default: ret = EINVAL; } if (ret != OK) { errno = ret; return -1; } return 0; }
void TouchManager::scheduler(){ // call every 10 ms to measure measure uint32_t now = wdtCounter; if (now > (previousGatetime + MEASURE_TICKS)){ _measure(); } }