void initSensors()
{
/***** INIT INDIVIDUAL SENSORS *****/
initMPU6050(); //Gyroscope
initADXL345(); //Accellerometer
// ..osv
}
/*
void sendCoefThroughUART(void) {
signed int AC1, AC2, AC3, AC4, AC5, AC6, B1, B2, MB, MC, MD;
AC1 = coefficients(BMP085_CAL_AC1);
IntToStr(AC1, data_);
UART1_Write_Text(data_);
UART1_Write(' ');
AC2 = coefficients(BMP085_CAL_AC2);
IntToStr(AC2, data_);
UART1_Write_Text(data_);
UART1_Write(' ');
return;
}
*/
void main() {
genericInit();
I2C1_Init(100000); // Initiate I2C @ 100 kHz
initMPU6050();
//CallibrateMPU6050raw(); // Measures the offset values of both accelerometer and gyroscope
initHMC5883L();
/*
initTimer0();
startTimer0();
Delay_ms(1000); // Maximum allowable time
stopTimer0();
*/
Delay_ms(100); // Wait for UART module to stabilize
while(1) {
do {} while(UART1_Read() != 's'); // Wait until start signal is received
do {
readAccMPU6050();
readGyrMPU6050();
readTmpMPU6050();
readUTBMP085();
readUPBMP085();
readHMC5883Lraw();
sendThroughUARTtoMSVS();
//Delay_ms(1000);
} while(UART1_Read() != 'e'); // Do until end signal is received
}//while
return;
}
void initSensors()
{
//_delay_ms(300);
for (a=0; a<3; a++) {_delay_loop_2(65535);} // ca. 30 ms
initMPU6050();
initGyro();
initAcc();
//_delay_ms(300);
for (a=0; a<3; a++) {_delay_loop_2(65535);} // ca. 30 ms
}
void systemInit(void)
{
RCC_ClocksTypeDef rccClocks;
///////////////////////////////////
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB |
RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO |
RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8 |
RCC_APB2Periph_ADC1, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 |
RCC_APB1Periph_TIM5 | RCC_APB1Periph_TIM6 |
RCC_APB1Periph_I2C2, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
#ifdef _DTIMING
timingSetup();
#endif
///////////////////////////////////////////////////////////////////////////
checkFirstTime(false);
readEEPROM();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
pwmMotorDriverInit();
cliInit();
gpioInit();
adcInit();
LED2_ON;
delay(10000); // 10 seconds of 20 second delay for sensor stabilization
if (GetVCPConnectMode() != eVCPConnectReset)
{
cliPrintF("\r\nUSB startup delay...\r\n");
delay(3000);
if (GetVCPConnectMode() == eVCPConnectData)
{
cliPrintF("\r\nBGC32 firmware starting up, USB connected...\r\n");
}
}
else
{
cliPrintF("\r\nDelaying for usb/serial driver to settle\r\n");
delay(3000);
cliPrintF("\r\nBGC32 firmware starting up, serial active...\r\n");
}
#ifdef __VERSION__
cliPrintF("\ngcc version " __VERSION__ "\n");
#endif
cliPrintF("BGC32 Firmware V%s, Build Date " __DATE__ " "__TIME__" \n", __BGC32_VERSION);
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
cliPrintF("\nRunning on external HSE clock....\n");
}
else
{
cliPrintF("\nERROR: Running on internal HSI clock....\n");
}
RCC_GetClocksFreq(&rccClocks);
cliPrintF("\nADCCLK-> %2d MHz\n", rccClocks.ADCCLK_Frequency / 1000000);
cliPrintF( "HCLK-> %2d MHz\n", rccClocks.HCLK_Frequency / 1000000);
cliPrintF( "PCLK1-> %2d MHz\n", rccClocks.PCLK1_Frequency / 1000000);
cliPrintF( "PCLK2-> %2d MHz\n", rccClocks.PCLK2_Frequency / 1000000);
cliPrintF( "SYSCLK-> %2d MHz\n\n", rccClocks.SYSCLK_Frequency / 1000000);
delay(10000); // Remaining 10 seconds of 20 second delay for sensor stabilization - probably not long enough..
LED1_ON;
i2cInit(I2C2);
rcInit();
timingFunctionsInit();
BKPInit();
initFirstOrderFilter();
initPID();
initSinArray();
orientIMU();
initMPU6050();
// initMag();
}
//------------------------- initIMU() -------------------
// Initialize IMU, return true if error
unsigned char initIMU(void) {
unsigned char error = 0;
error += initMPU6050();
error += initHMC5883();
return error;
}