void gyroSetup(void) {
// setup I2C port
gyroSetupPeripheral();
// external interrupt configuration.
// it is NOT USED at this moment.
ConfigINT1(RISING_EDGE_INT & EXT_INT_DISABLE & EXT_INT_PRI_3);
dead_zone = DEFAULT_DEAD_ZONE;
delay_ms(25); // power up delay, may not need...
gyroReset();
delay_ms(10);
//gyroWrite(REG_DLPF_FS, 0x1A); // 2000 deg/sec, 1 kHz Sampling rate, 98Hz LPF
gyroWrite(REG_DLPF_FS, 0x19); // 2000 deg/sec, 1 kHz Sampling rate, 196Hz LPF
gyroWrite(REG_INT_CFG, 0x00); // interrupt disabled
gyroWrite(REG_PWR_MGM, 0x03); // Set power management?
delay_ms(1); // PLL Settling time
is_ready = 1;
delay_ms(6); //From datasheet, standard settling time
gyroRunCalib(INITIAL_CALIB_NUM); // quick calibration
}
void startSensor(Sensor *s) {
switch (s->type) {
case SHAFT_ENCODER:
encoderReset(s->enc);
if (DEBUG) {
print("reset encoder");
}
break;
case GYROSCOPE:
gyroReset(s->gyr);
break;
case TIME:
//resetTimer(s->port, true);
startTimer(s->port, true);
break;
case IME:
imeReset(s->port);
break;
default:
break;
}
}
void Accelerometer::reset()
{
if(sensorport != 13)
gyroReset(gyro);
}
void GyroSensor::reset()
{
if(sensorport != 13)
gyroReset(gyro);
}
