// Now create any global variables such as motors, servos, sensors etc
// This routine is called once only and allows you to do set up the hardware
// Dont use any 'clock' functions here - use 'delay' functions instead
void appInitHardware(void){
a2dSetPrescaler(ADC_PRESCALE_DIV128);
cyrf6936_Initialise_hard();
// Initialise SPI bus as master. (RF modules connected to hardware SPI)
spiBusInit(&bus, TRUE);
spiDeviceSelect(&cyrf_0, TRUE);
spiDeviceSelect(&cyrf_0, FALSE);
spiDeviceSelect(&cyrf_1, TRUE);
spiDeviceSelect(&cyrf_1, FALSE);
// set I/O pins for RF module(s).
pin_make_input(D4, FALSE); // set PACTL pin to input. (module on connector J1)
pin_make_input(D5, FALSE); // set PACTLn pin to input. (module on connector J1)
pin_make_input(D6, FALSE); // set PACTL pin to input. (module on connector J2)
pin_make_input(D7, FALSE); // set PACTLn pin to input. (module on connector J2)
#ifdef RF_MODULE_ARTAFLEX
//
pin_make_output(D4, FALSE); // set RXPA pin to output. (module on connector J1)
pin_make_output(D5, FALSE); // set TXPA pin to output. (module on connector J1)
pin_make_output(D6, FALSE); // set RXPA pin to output. (module on connector J2)
pin_make_output(D7, FALSE); // set TXPA pin to output. (module on connector J2)
#endif
pin_make_input(E7, TRUE); // set UNIGEN RF module IRQ pin to input. (module on connector J1)
pin_make_input(E6, TRUE); // set UNIGEN RF module IRQ pin to input. (module on connector J2)
pin_make_output(G3, FALSE); // set UNIGEN RF module RST pin to output. (both modules)
//pin_low(G3); // don't reset yet.
// set I/O pins for status LEDs
pin_make_output(C0, TRUE); // set LED pin for output
pin_make_output(C1, TRUE); // set LED pin for output
pin_make_output(C2, FALSE); // set LED pin for output
//pin_high(C0); // LED off
//pin_high(C1); // LED off
//pin_low(C2); // LED on
// Set UART1 to 19200 baud
uartInit(UART1, 38400);
// Tell rprintf to output to UART1
rprintfInit(&uart1SendByte);
// Initialise the servo controller using Hardware PWM
servoPWMInit(&bank1);
// Initialise WatchDog Timer
wdt_enable( WDTO_500MS );
}
void appInitHardware(void){
// Initialise the servo controller
// Initialise the SPI bus
spiBusInit(&spiBus,true);
//servosInit(&bank1, TIMER1);
// Set UART0 to 9600 baud
uartInit(UART0, 9600);
// Tell rprintf to output to UART0
rprintfInit(&uart0SendByte);
}
bool detectGyro(uint16_t gyroLpf)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_DEFAULT:
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
#ifdef GYRO_MPU6050_ALIGN
gyroHardware = GYRO_MPU6050;
gyroAlign = GYRO_MPU6050_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro, gyroLpf)) {
#ifdef GYRO_L3G4200D_ALIGN
gyroHardware = GYRO_L3G4200D;
gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro, gyroLpf)) {
#ifdef GYRO_MPU3050_ALIGN
gyroHardware = GYRO_MPU3050;
gyroAlign = GYRO_MPU3050_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro, gyroLpf)) {
#ifdef GYRO_L3GD20_ALIGN
gyroHardware = GYRO_L3GD20;
gyroAlign = GYRO_L3GD20_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_SPI_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6000_ALIGN
gyroHardware = GYRO_SPI_MPU6000;
gyroAlign = GYRO_SPI_MPU6000_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_SPI_MPU6500:
#ifdef USE_GYRO_SPI_MPU6500
#ifdef USE_HARDWARE_REVISION_DETECTION
spiBusInit();
#endif
#ifdef NAZE
if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
break;
}
#else
if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
break;
}
#endif
#endif
; // fallthrough
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro, gyroLpf)) {
gyroHardware = GYRO_FAKE;
break;
}
#endif
; // fallthrough
case GYRO_NONE:
gyroHardware = GYRO_NONE;
}
if (gyroHardware == GYRO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
