Lines Matching refs:inv_obj
81 inv_obj.acc_state = SF_STARTUP_SETTLE;
168 inv_obj.compass_test_bias[i] =
169 -(long)(tmp * inv_obj.compass_sens / 16384.0f);
230 if (inv_obj.resetting_compass == 1) {
238 inv_obj.resetting_compass = 0;
243 if (inv_obj.compass_sensor_data[i] > magMax[i]) {
244 magMax[i] = inv_obj.compass_sensor_data[i];
246 if (inv_obj.compass_sensor_data[i] < magMin[i]) {
247 magMin[i] = inv_obj.compass_sensor_data[i];
251 inv_obj.compass_sensor_data,
256 (long long)40 * 1073741824 / inv_obj.compass_sens) {
263 (CAL_RUN, inv_obj.compass_sensor_data,
265 inv_obj.got_compass_bias = 1;
266 inv_obj.compass_accuracy = 3;
268 inv_obj.compass_bias_error[i] = 35;
270 if (inv_obj.compass_state == SF_UNCALIBRATED)
271 inv_obj.compass_state = SF_STARTUP_SETTLE;
272 inv_set_compass_bias(inv_obj.compass_test_bias);
282 inv_obj.compass_sensor_data,
297 inv_obj.compass_sensor_data,
302 if (inv_obj.acc_state != SF_STARTUP_SETTLE) {
304 inv_obj.acc_state = SF_DISTURBANCE; //No accels, fast swing
308 && ((inv_obj.acc_state == SF_DISTURBANCE)
309 || (inv_obj.acc_state == SF_SLOW_SETTLE))) {
312 inv_obj.acc_state = SF_FAST_SETTLE;
316 if (inv_obj.acc_state == SF_DISTURBANCE) {
319 inv_obj.acc_state = SF_SLOW_SETTLE;
322 } else if (inv_obj.acc_state == SF_SLOW_SETTLE) {
325 inv_obj.acc_state = SF_NORMAL;
328 } else if (inv_obj.acc_state == SF_FAST_SETTLE) {
331 inv_obj.acc_state = SF_NORMAL;
338 if (inv_obj.acc_state == SF_STARTUP_SETTLE) {
342 inv_obj.acc_state = SF_NORMAL;
345 } else if ((inv_obj.acc_state == SF_NORMAL)
346 || (inv_obj.acc_state == SF_SLOW_SETTLE)) {
347 *accSF = inv_obj.accel_sens * 64; //Normal
348 } else if ((inv_obj.acc_state == SF_DISTURBANCE)) {
349 *accSF = inv_obj.accel_sens * 64; //Don't use accel (should be 0)
350 } else if (inv_obj.acc_state == SF_FAST_SETTLE) {
351 *accSF = inv_obj.accel_sens * 512; //Amplify accel
354 *accSF = inv_obj.accel_sens * 128;
391 if (result && !inv_obj.external_slave_callback) {
405 if (inv_obj.got_init_compass_bias == 0) {
406 inv_obj.got_init_compass_bias = 1;
408 inv_obj.init_compass_bias[i] = magSensorData[i];
412 inv_obj.compass_sensor_data[i] = (long)magSensorData[i];
413 inv_obj.compass_sensor_data[i] -=
414 inv_obj.init_compass_bias[i];
415 tmp[i] = ((long long)inv_obj.compass_sensor_data[i])
416 * inv_obj.compass_sens / 16384;
417 tmp[i] -= inv_obj.compass_bias[i];
418 tmp[i] = (tmp[i] * inv_obj.compass_scale[i]) / 65536L;
424 inv_obj.compass_cal[i * 3 + j];
426 inv_obj.compass_calibrated_data[i] =
427 (long) (tmp64 / inv_obj.compass_sens);
430 if ((inv_obj.compass_state == 1) &&
431 (inv_obj.compass_overunder == 0)) {
440 if (inv_obj.compass_overunder) {
448 inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0;
457 inv_obj
465 if (inv_obj.external_slave_callback) {
466 result = inv_obj.external_slave_callback(&inv_obj);
476 fcin[0] = 1000*((float)inv_obj.compass_calibrated_data[0] /65536.f);
477 fcin[1] = 1000*((float)inv_obj.compass_calibrated_data[1] /65536.f);
478 fcin[2] = 1000*((float)inv_obj.compass_calibrated_data[2] /65536.f);
482 inv_obj.compass_calibrated_data[0] = (long)(fcout[0]*65536.f/1000.f);
483 inv_obj.compass_calibrated_data[1] = (long)(fcout[1]*65536.f/1000.f);
484 inv_obj.compass_calibrated_data[2] = (long)(fcout[2]*65536.f/1000.f);
490 (float)inv_obj.compass_calibrated_data[0] /
492 (float)inv_obj.compass_calibrated_data[1] /
494 (float)inv_obj.compass_calibrated_data[2] /
563 inv_obj.pressure = pressureSensorData[0];
580 MLUpdateCompassCalibration3DOF(CAL_RESET, inv_obj.compass_sensor_data, 1);
582 inv_obj.compass_bias_error[0] = P_INIT;
583 inv_obj.compass_bias_error[1] = P_INIT;
584 inv_obj.compass_bias_error[2] = P_INIT;
585 inv_obj.compass_accuracy = 0;
587 inv_obj.got_compass_bias = 0;
588 inv_obj.got_init_compass_bias = 0;
589 inv_obj.compass_state = SF_UNCALIBRATED;
590 inv_obj.resetting_compass = 1;
