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