void control_initFilter() { int i; for (i = 0; i < 3; i++) { utilFilterInit(&controlData.pitchFilter[i], 1.0f/100.0f, 0.1f, 0.0f); utilFilterInit(&controlData.rollFilter[i], 1.0f/100.0f, 0.1f, 0.0f); } }
void max21100Init(void) { switch ((int)p[IMU_FLIP]) { case 1: max21100Data.accSign[0] = 1.0f; max21100Data.accSign[1] = -1.0f; max21100Data.accSign[2] = -1.0f; max21100Data.gyoSign[0] = 1.0f; max21100Data.gyoSign[1] = -1.0f; max21100Data.gyoSign[2] = -1.0f; break; case 2: max21100Data.accSign[0] = -1.0f; max21100Data.accSign[1] = 1.0f; max21100Data.accSign[2] = -1.0f; max21100Data.gyoSign[0] = -1.0f; max21100Data.gyoSign[1] = 1.0f; max21100Data.gyoSign[2] = -1.0f; break; case 0: default: max21100Data.accSign[0] = 1.0f; max21100Data.accSign[1] = 1.0f; max21100Data.accSign[2] = 1.0f; max21100Data.gyoSign[0] = 1.0f; max21100Data.gyoSign[1] = 1.0f; max21100Data.gyoSign[2] = 1.0f; break; } utilFilterInit(&max21100Data.tempFilter, DIMU_OUTER_DT, DIMU_TEMP_TAU, IMU_ROOM_TEMP); max21100ReliablySetReg(0x22, 0x00); // BNK 00 // turn off I2C max21100ReliablySetReg(0x16, 0x01); // check device ID while (max21100GetReg(0x20) != 0xb2) ; // power up max21100ReliablySetReg(0x00, 0b01111111); // Normal Mode GYO+ACC // GYO scale @ 2KHz #if MAX21100_GYO_SCALE == 250 max21100ReliablySetReg(0x01, 0b00100011); #endif #if MAX21100_GYO_SCALE == 500 max21100ReliablySetReg(0x01, 0b00100010); #endif #if MAX21100_GYO_SCALE == 1000 max21100ReliablySetReg(0x01, 0b00100001); #endif #if MAX21100_GYO_SCALE == 2000 max21100ReliablySetReg(0x01, 0b00100000); #endif max21100ReliablySetReg(0x02, 0b00100000); // GYO 8KHz ODR // ACC scale #if MAX21100_ACC_SCALE == 2 max21100ReliablySetReg(0x04, 0b11000111); #endif #if MAX21100_ACC_SCALE == 4 max21100ReliablySetReg(0x04, 0b10000111); #endif #if MAX21100_ACC_SCALE == 8 max21100ReliablySetReg(0x04, 0b01000111); #endif #if MAX21100_ACC_SCALE == 16 max21100ReliablySetReg(0x04, 0b00000111); #endif max21100ReliablySetReg(0x05, 0b00110000); // ACC 2KHz ODR max21100SetReg(0x22, 0x01); // BNK 01 max21100ReliablySetReg(0x0b, 0b00100000); // IRQ config max21100ReliablySetReg(0x0c, 0b00000000); // IRQ config max21100SetReg(0x0d, 0b10000000); // IRQ config max21100Data.readReg = MAX21100_READ_BIT | 0x24; // start of sensor registers // External Interrupt line for data ready extRegisterCallback(DIMU_MAX21100_INT_PORT, DIMU_MAX21100_INT_PIN, EXTI_Trigger_Rising, 1, GPIO_PuPd_NOPULL, max21100IntHandler); spiChangeCallback(max21100Data.spi, max21100TransferComplete); }
void utilFilterInit3(utilFilter_t *f, float dt, float tau, float setpoint) { utilFilterInit(&f[0], dt, tau, setpoint); utilFilterInit(&f[1], dt, tau, setpoint); utilFilterInit(&f[2], dt, tau, setpoint); }
void mpu6000Init(void) { switch ((int)p[IMU_FLIP]) { case 1: mpu6000Data.accSign[0] = 1.0f; mpu6000Data.accSign[1] = -1.0f; mpu6000Data.accSign[2] = -1.0f; mpu6000Data.gyoSign[0] = 1.0f; mpu6000Data.gyoSign[1] = -1.0f; mpu6000Data.gyoSign[2] = -1.0f; break; case 2: mpu6000Data.accSign[0] = -1.0f; mpu6000Data.accSign[1] = 1.0f; mpu6000Data.accSign[2] = -1.0f; mpu6000Data.gyoSign[0] = -1.0f; mpu6000Data.gyoSign[1] = 1.0f; mpu6000Data.gyoSign[2] = -1.0f; break; case 0: default: mpu6000Data.accSign[0] = 1.0f; mpu6000Data.accSign[1] = 1.0f; mpu6000Data.accSign[2] = 1.0f; mpu6000Data.gyoSign[0] = 1.0f; mpu6000Data.gyoSign[1] = 1.0f; mpu6000Data.gyoSign[2] = 1.0f; break; } utilFilterInit(&mpu6000Data.tempFilter, DIMU_OUTER_DT, DIMU_TEMP_TAU, IMU_ROOM_TEMP); // reset mpu6000SetReg(107, 0b10000000); delay(100); // wake up w/ Z axis clock reg mpu6000ReliablySetReg(107, 0x03); // disable I2C interface mpu6000ReliablySetReg(106, 0b010000); // wait for a valid response while (mpu6000GetReg(117) != 0x68) delay(10); // GYO scale #if MPU6000_GYO_SCALE == 250 mpu6000ReliablySetReg(27, 0b00000); #endif #if MPU6000_GYO_SCALE == 500 mpu6000ReliablySetReg(27, 0b01000); #endif #if MPU6000_GYO_SCALE == 1000 mpu6000ReliablySetReg(27, 0b10000); #endif #if MPU6000_GYO_SCALE == 2000 mpu6000ReliablySetReg(27, 0b11000); #endif // ACC scale #if MPU6000_ACC_SCALE == 2 mpu6000ReliablySetReg(28, 0b00000); #endif #if MPU6000_ACC_SCALE == 4 mpu6000ReliablySetReg(28, 0b01000); #endif #if MPU6000_ACC_SCALE == 8 mpu6000ReliablySetReg(28, 0b10000); #endif #if MPU6000_ACC_SCALE == 16 mpu6000ReliablySetReg(28, 0b11000); #endif // Sample rate mpu6000ReliablySetReg(25, 0x00); // LPF mpu6000ReliablySetReg(26, 0x00); // Interrupt setup mpu6000ReliablySetReg(55, 0x01<<4); mpu6000ReliablySetReg(56, 0x01); // bump clock rate up to 21MHz spiChangeBaud(mpu6000Data.spi, MPU6000_SPI_RUN_BAUD); mpu6000Data.readReg = MPU6000_READ_BIT | 0x3b; // start of sensor registers spiChangeCallback(mpu6000Data.spi, mpu6000TransferComplete); // External Interrupt line for data ready extRegisterCallback(DIMU_MPU6000_INT_PORT, DIMU_MPU6000_INT_PIN, EXTI_Trigger_Rising, 1, GPIO_PuPd_NOPULL, mpu6000IntHandler); }
void radioInit(void) { uint16_t radioType = (uint16_t)p[RADIO_TYPE]; int i; AQ_NOTICE("Radio init\n"); memset((void *)&radioData, 0, sizeof(radioData)); radioData.mode = (radioType>>12) & 0x0f; for (i = 0; i < RADIO_NUM; i++) { radioInstance_t *r = &radioData.radioInstances[i]; USART_TypeDef *uart; // determine UART switch (i) { case 0: uart = RC1_UART; break; #ifdef RC2_UART case 1: uart = RC2_UART; break; #endif #ifdef RC3_UART case 2: uart = RC3_UART; break; #endif default: uart = 0; break; } r->radioType = (radioType>>(i*4)) & 0x0f; r->channels = &radioData.allChannels[RADIO_MAX_CHANNELS * i]; utilFilterInit(&r->qualityFilter, (1.0f / 50.0f), 0.75f, 0.0f); switch (r->radioType) { case RADIO_TYPE_SPEKTRUM11: case RADIO_TYPE_SPEKTRUM10: case RADIO_TYPE_DELTANG: if (uart) { spektrumInit(r, uart); radioRCSelect(i, 0); AQ_PRINTF("Spektrum on RC port %d\n", i); } break; case RADIO_TYPE_SBUS: if (uart) { futabaInit(r, uart); radioRCSelect(i, 1); AQ_PRINTF("Futaba on RC port %d\n", i); } break; case RADIO_TYPE_PPM: ppmInit(r); AQ_PRINTF("PPM on RC port %d\n", i); break; case RADIO_TYPE_SUMD: if (uart) { grhottInit(r, uart); radioRCSelect(i, 0); AQ_PRINTF("GrHott on RC port %d\n", i); } break; case RADIO_TYPE_MLINK: if (uart) { mlinkrxInit(r, uart); radioRCSelect(i, 0); AQ_PRINTF("Mlink on RC port %d\n", i); } break; case RADIO_TYPE_NONE: break; default: AQ_NOTICE("WARNING: Invalid radio type!\n"); break; } } switch (radioData.mode) { case RADIO_MODE_DIVERSITY: // select first available radio to start with for (i = 0; i < RADIO_NUM; i++) { if (radioData.radioInstances[i].radioType > RADIO_TYPE_NONE) { radioMakeCurrent(&radioData.radioInstances[i]); break; } } break; case RADIO_MODE_SPLIT: radioMakeCurrent(&radioData.radioInstances[0]); break; } // set mode default radioData.channels[(int)p[RADIO_FLAP_CH]] = -700; radioTaskStack = aqStackInit(RADIO_STACK_SIZE, "RADIO"); radioData.radioTask = CoCreateTask(radioTaskCode, (void *)0, RADIO_PRIORITY, &radioTaskStack[RADIO_STACK_SIZE-1], RADIO_STACK_SIZE); }