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);
}
