/*=====================================================================================================*/
void System_Init( void )
{
SystemInit();
LED_Config();
KEY_Config();
RS232_Config();
I2C_Config();
Motor_Config();
nRF24L01_Config();
PID_Init(&PID_Yaw);
PID_Init(&PID_Roll);
PID_Init(&PID_Pitch);
PID_Pitch.Kp = +3.5f;
PID_Pitch.Ki = +0.004f;
PID_Pitch.Kd = +4.0f;
PID_Roll.Kp = +3.5f;
PID_Roll.Ki = +0.004f;
PID_Roll.Kd = +4.0f;
PID_Yaw.Kp = +0.0f;
PID_Yaw.Ki = +0.0f;
PID_Yaw.Kd = +0.25f;
Delay_10ms(2);
}
/*=====================================================================================================*/
void Sensor_Config( void )
{
GPIO_InitTypeDef GPIO_InitStruct;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
/* INT PC3 */
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOC, &GPIO_InitStruct);
I2C_Config();
}
/*********************************************************************//**
* @brief Initial System Init using Port and Peripheral
* @param[in] None
* @return None
**********************************************************************/
void System_Init(void)
{
LPC_WDT->WDMOD &= ~WDT_WDMOD_WDEN; // Disable Watchdog
SystemInit(); // Initialize system and update core clock
Port_Init(); // Port Initialization
SYSTICK_Config(); // Systick Initialization
led_delay = 1000; // Heart Beat rate of 1Sec toggle
NVIC_SetPriority(SysTick_IRQn, 0); // Set SysTick as Highest Priority
UART_Config(LPC_UART0, 115200); // Uart0 Initialize at 9600 Baud Rate
SSP_Config (LPC_SSP1); // Initialize SPI
I2C_Config (LPC_I2C0); // Initialize I2C0
GLCD_Init(); // Initialize GLCD
GPIO_IntCmd(2,_BIT(7),1); // Enable GPIO Interrupt at P0.19 Falling Edge
NVIC_EnableIRQ(EINT3_IRQn); // NVIC Interrupt EINT3_IRQn for GPIO
NVIC_SetPriority(EINT3_IRQn, 4); // Set any lower Priority than SysTick
TSC2004_Cal_Init(&cmatrix);
}
int main(void)
{
//uint16_t kezdet, vege;
vSemaphoreCreateBinary(xADCSemaphore);
RCC_Config();
IO_Init();
UART_Config();
PWM_Config();
DMA_Config();
I2C_Config();
NVIC_Config();
DebugTimerInit();
xTaskCreate(prvInitTask,(signed char*)"INIT", configMINIMAL_STACK_SIZE,NULL,TASK_INIT_PRIORITY,NULL);
vTaskStartScheduler();
while (1)
{
}
}
int main( void )
{
u32 ReadID = 0;
SystemInit();
GPIO_Config();
I2C_Config();
RS232_Config();
Delay_1ms(10);
LED_B = 0;
while(ReadID != (MT9V022_ID<<8 | MT9V022_ID)) {
ReadID = MT9V022_Init();
LED_R = ~LED_R;
Delay_100ms(1);
}
while(1) {
LED_G = ~LED_G;
Delay_100ms(1);
}
}
/**
* @brief Main program
* @param None
* @retval None
*/
int main(void)
{
/*!< At this stage the microcontroller clock setting is already configured,
this is done through SystemInit() function which is called from startup
file (startup_stm32l1xx_xx.s) before to branch to application main.
To reconfigure the default setting of SystemInit() function, refer to
system_stm32l1xx.c file
*/
/* Initialize the LCD */
STM32L152D_LCD_Init();
/* Clear the LCD */
LCD_Clear(LCD_COLOR_WHITE);
/* I2C configuration ---------------------------------------------------------*/
I2C_Config();
/* Initialize LEDs mounted on STM32L152D-EVAL board */
STM_EVAL_LEDInit(LED1);
STM_EVAL_LEDInit(LED2);
STM_EVAL_LEDInit(LED3);
STM_EVAL_LEDInit(LED4);
/* SysTick configuration -----------------------------------------------------*/
SysTickConfig();
/* Enable AES AHB clock */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_AES, ENABLE);
/*************************************Master Code******************************/
#if defined (I2C_MASTER)
/* I2C De-initialize */
I2C_DeInit(I2Cx);
/* AES Encryption ------------------------------------------------------------*/
AES_ECB_Encrypt(EncryptionKey, TxBuffer, AES_TEXT_SIZE, CipherText);
/* Read the CipherText and check content correctness */
if (Buffercmp(ExpectedCipherText, CipherText, RXBUFFERSIZE) == PASSED)
{
/* Clear the LCD */
LCD_Clear(LCD_COLOR_WHITE);
/* Set the Back Color */
LCD_SetBackColor(LCD_COLOR_BLUE);
/* Set the Text Color */
LCD_SetTextColor(LCD_COLOR_GREEN);
LCD_DisplayStringLine(LCD_LINE_1, "Encryption : Success");
}
else
{
LCD_Clear(LCD_COLOR_WHITE);
/* Set the Back Color */
LCD_SetBackColor(LCD_COLOR_BLUE);
/* Set the Text Color */
LCD_SetTextColor(LCD_COLOR_GREEN);
LCD_DisplayStringLine(LCD_LINE_1, "Encryption : Failed");
}
/*!< I2C Struct Initialize */
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DUTYCYCLE;
I2C_InitStructure.I2C_OwnAddress1 = 0xA0;
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_ClockSpeed = I2C_SPEED;
#ifndef I2C_10BITS_ADDRESS
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
#else
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_10bit;
#endif /* I2C_10BITS_ADDRESS */
/*!< I2C Initialize */
I2C_Init(I2Cx, &I2C_InitStructure);
/* Enable Error Interrupt */
I2C_ITConfig(I2Cx, I2C_IT_ERR , ENABLE);
/* I2C ENABLE */
I2C_Cmd(I2Cx, ENABLE);
/* Master Transmitter---------------------------------------------------------*/
NumberOfByteToTransmit = TXBUFFERSIZE;
MasterMode = MASTER_MODE_TRANSMITTER;
Tx_Idx = 0x00;
/* Enable Error and Buffer Interrupts */
I2C_ITConfig(I2Cx, (I2C_IT_EVT | I2C_IT_BUF), ENABLE);
/* Generate the Start condition */
I2C_GenerateSTART(I2Cx, ENABLE);
/* Data transfer is performed in the I2C interrupt routine */
/* Wait until end of data transfer or time out */
TimeOut = USER_TIMEOUT;
while ((Tx_Idx < TXBUFFERSIZE)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
TimeOut = USER_TIMEOUT;
while ((I2C_GetFlagStatus(I2Cx, I2C_FLAG_BUSY))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
LCD_DisplayStringLine(LCD_LINE_2, " Send : Done ");
STM_EVAL_LEDOn(LED2);
STM_EVAL_LEDOn(LED3);
STM_EVAL_LEDOn(LED4);
#endif /* I2C_MASTER */
/**********************************Slave Code**********************************/
#if defined (I2C_SLAVE)
I2C_DeInit(I2Cx);
/* Initialize I2C peripheral */
/*!< I2C Init */
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DUTYCYCLE;
I2C_InitStructure.I2C_OwnAddress1 = SLAVE_ADDRESS;
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_ClockSpeed = I2C_SPEED;
#ifndef I2C_10BITS_ADDRESS
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
#else
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_10bit;
#endif /* I2C_10BITS_ADDRESS */
I2C_Init(I2Cx, &I2C_InitStructure);
/* Enable Error Interrupt */
I2C_ITConfig(I2Cx, (I2C_IT_ERR | I2C_IT_EVT | I2C_IT_BUF), ENABLE);
/* I2C ENABLE */
I2C_Cmd(I2Cx, ENABLE);
/* Slave Receiver-------------------------------------------------------------*/
Rx_Idx = 0x00;
Tx_Idx = 0x00;
/* Clear the RxBuffer */
Fill_Buffer(RxBuffer, RXBUFFERSIZE);
/* Wait until end of data transfer */
while (Rx_Idx < RXBUFFERSIZE)
{}
TimeOut = USER_TIMEOUT;
while ((I2C_GetFlagStatus(I2Cx, I2C_FLAG_BUSY))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
if (Buffercmp(ExpectedCipherText, RxBuffer, RXBUFFERSIZE) == PASSED)
{
/* Clear the LCD */
LCD_Clear(LCD_COLOR_WHITE);
/* Set the Back Color */
LCD_SetBackColor(LCD_COLOR_BLUE);
/* Set the Text Color */
LCD_SetTextColor(LCD_COLOR_GREEN);
LCD_DisplayStringLine(LCD_LINE_1, " Received : Done ");
STM_EVAL_LEDOn(LED2);
STM_EVAL_LEDOn(LED3);
STM_EVAL_LEDOn(LED4);
}
else
{
/* Clear the LCD */
LCD_Clear(LCD_COLOR_WHITE);
/* Set the Back Color */
LCD_SetBackColor(LCD_COLOR_BLUE);
/* Set the Text Color */
LCD_SetTextColor(LCD_COLOR_GREEN);
LCD_DisplayStringLine(LCD_LINE_1, " Receive : Failed ");
STM_EVAL_LEDOff(LED2);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
}
/* AES Decription ------------------------------------------------------------*/
AES_ECB_Decrypt(EncryptionKey, RxBuffer, AES_TEXT_SIZE, ComputedPlainText);
/* Read the ComputedPlainText and check content correctness */
if (Buffercmp(TxBuffer, ComputedPlainText, RXBUFFERSIZE) == PASSED)
{
/* LED2, LED3 and LED4 Toggle */
LCD_DisplayStringLine(LCD_LINE_2, "Decryption : Success");
}
else
{
/* ED2, LED3 and LED4 On */
LCD_DisplayStringLine(LCD_LINE_2, " Decryption : Failed");
}
/* -------------------------------------------------------------------------- */
#endif /* I2C_SLAVE */
while(1)
{}
}
int main(void)
{
uint32_t data_counter=0; //used as data timestamp
uint8_t deadly_flashes=0,system_state=0,repetition_counter=0;
int16_t sensor_data, sensor_raw_data[3]={}; //used for handling data passed back from sensors
int16_t sfe_sensor_ref_buff[2][3],sfe_sensor_ref_buff_old[2][3];//used to detect and fix I2C bus lockup
RTC_t RTC_time;
wave_stuffer Gyro_wav_stuffer={0,0},Accel_wav_stuffer={0,0};//Used to controlling wav file bit packing
SystemInit(); //Sets up the clk
setup_gpio(); //Initialised pins, and detects boot source
DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); //Watchdog stopped during JTAG halt
if(RCC->CSR&RCC_CSR_IWDGRSTF) { //Watchdog reset, turn off
RCC->CSR|=RCC_CSR_RMVF; //Reset the reset flags
shutdown();
}
SysTick_Configuration(); //Start up system timer at 100Hz for uSD card functionality
Watchdog_Config(WATCHDOG_TIMEOUT); //Set the watchdog
Watchdog_Reset(); //Reset watchdog as soon as possible incase it is still running at power on
rtc_init(); //Real time clock initialise - (keeps time unchanged if set)
Usarts_Init();
ISR_Config();
rprintfInit(__usart_send_char); //Printf over the bluetooth
if(USB_SOURCE==bootsource) {
Set_System(); //This actually just inits the storage layer
Set_USBClock();
USB_Interrupts_Config();
USB_Init();
uint32_t nojack=0x000FFFFF; //Countdown timer - a few hundered ms of 0v on jack detect forces a shutdown
while (bDeviceState != CONFIGURED) { //Wait for USB config - timeout causes shutdown
if(Millis>10000 || !nojack) //No USB cable - shutdown (Charger pin will be set to open drain, cant be disabled without usb)
shutdown();
if(GET_CHRG_STATE) //Jack detect resets the countdown
nojack=0x0FFFFF;
nojack--;
Watchdog_Reset(); //Reset watchdog here, if we are stalled here the Millis timeout should catch us
}
USB_Configured_LED();
EXTI_ONOFF_EN(); //Enable the off interrupt - allow some time for debouncing
while(1) { //If running off USB (mounted as mass storage), stay in this loop - dont turn on anything
if(Millis%1000>500) //1Hz on/off flashing
switch_leds_on(); //Flash the LED(s)
else
switch_leds_off();
Watchdog_Reset();
__WFI(); //Sleep until something arrives
}
}
else {
if(!GET_PWR_STATE) //Check here to make sure the power button is still pressed, if not, sleep
shutdown(); //This means a glitch on the supply line, or a power glitch results in sleep
EXTI_ONOFF_EN(); //Enable the off interrupt - allow some time for debouncing
ADC_Configuration(); //At present this is purely here to detect low battery
do {
battery_voltage=Battery_Voltage;//Have to flush adc for some reason
Delay(25000);
} while(fabs(Battery_Voltage-battery_voltage)>0.01 || !battery_voltage);
I2C_Config(); //Setup the I2C bus
Sensors=detect_sensors(0); //Search for connected sensors
if(battery_voltage<BATTERY_STARTUP_LIMIT)
deadly_flashes=1;
if(!(Sensors&(1<<FOREHEAD_ACCEL))) //Check for any missing sensors
deadly_flashes=2;
if(!(Sensors&(1<<(FOREHEAD_GYRO-1))))
deadly_flashes=3;
if(!(Sensors&(1<<(SFE_1_ACCEL-1))))
deadly_flashes=4;
if(!(Sensors&(1<<(SFE_1_MAGNO-1))))
deadly_flashes=5;
if(!(Sensors&(1<<(SFE_1_GYRO-1))))
deadly_flashes=6;
if(!(Sensors&(1<<(SFE_2_ACCEL-3))))
deadly_flashes=7;
if(!(Sensors&(1<<(SFE_2_MAGNO-3))))
deadly_flashes=8;
if(!(Sensors&(1<<(SFE_2_GYRO-3))))
deadly_flashes=9;
if((f_err_code = f_mount(0, &FATFS_Obj)))Usart_Send_Str((char*)"FatFs mount error\r\n");//This should only error if internal error
else if(!deadly_flashes){ //FATFS and the I2C initialised ok, try init the card, this also sets up the SPI1
if(!f_open(&FATFS_logfile,"time.txt",FA_OPEN_EXISTING | FA_READ | FA_WRITE)) {//Try and open a time file to get the system time
if(!f_stat((const TCHAR *)"time.txt",&FATFS_info)) {//Get file info
if(!FATFS_info.fsize) {//Empty file
RTC_time.year=(FATFS_info.fdate>>9)+1980;//populate the time struct (FAT start==1980, RTC.year==0)
RTC_time.month=(FATFS_info.fdate>>5)&0x000F;
RTC_time.mday=FATFS_info.fdate&0x001F;
RTC_time.hour=(FATFS_info.ftime>>11)&0x001F;
RTC_time.min=(FATFS_info.ftime>>5)&0x003F;
RTC_time.sec=(FATFS_info.ftime<<1)&0x003E;
rtc_settime(&RTC_time);
rprintfInit(__fat_print_char);//printf to the open file
printf("RTC set to %d/%d/%d %d:%d:%d\n",RTC_time.mday,RTC_time.month,RTC_time.year,\
RTC_time.hour,RTC_time.min,RTC_time.sec);
}
}
f_close(&FATFS_logfile);//Close the time.txt file
}
rtc_gettime(&RTC_time); //Get the RTC time and put a timestamp on the start of the file
rprintfInit(__str_print_char); //Print to the string
//timestamp name
printf("%d-%02d-%02dT%02d-%02d-%02d",RTC_time.year,RTC_time.month,RTC_time.mday,RTC_time.hour,RTC_time.min,RTC_time.sec);
rprintfInit(__usart_send_char); //Printf over the bluetooth
f_err_code = f_mkdir(print_string); //Try to make a directory where the logfiles will live
if(f_err_code) {
printf("FatFs drive error %d\r\n",f_err_code);
if(f_err_code==FR_DISK_ERR || f_err_code==FR_NOT_READY)
Usart_Send_Str((char*)"No uSD card inserted?\r\n");
repetition_counter=1;
}
else
f_err_code=f_chdir(print_string);//enter our new directory
if(f_err_code) {
if(!repetition_counter)
printf("FatFs drive error entering direcotry %d\r\n",f_err_code);
repetition_counter=1;
}
else {
strcat(print_string,".csv");
f_err_code=f_open(&FATFS_logfile,print_string,FA_CREATE_ALWAYS | FA_WRITE);//Try to open the main 100sps csv logfile
}
if(f_err_code) {
if(!repetition_counter)
printf("FatFs drive error creating logfile %d\r\n",f_err_code);
repetition_counter=1;
}
else {
print_string[strlen(print_string)-4]=0x00; //Wipe the .csv off the string
strcat(print_string,"_accel.wav");
f_err_code=f_open(&FATFS_wavfile_accel,print_string,FA_CREATE_ALWAYS | FA_WRITE);//Try to open the accel wav logfile
}
if(f_err_code) {
if(!repetition_counter)
printf("FatFs drive error creating accel wav file %d\r\n",f_err_code);
repetition_counter=1;
}
else {
print_string[strlen(print_string)-9]=0x00; //Wipe the accel.wav off the string
strcat(print_string,"gyro.wav");
f_err_code=f_open(&FATFS_wavfile_gyro,print_string,FA_CREATE_ALWAYS | FA_WRITE);//Try to open the gyro wav logfile
}
if(f_err_code) {
if(!repetition_counter)
printf("FatFs drive error creating gyro wav file %d\r\n",f_err_code);
}
else { //We have a mounted card
print_string[0]=0x00; //Wipe the string
f_err_code=f_lseek(&FATFS_logfile, PRE_SIZE);// Pre-allocate clusters
if (f_err_code || f_tell(&FATFS_logfile) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_logfile, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
else
rprintfInit(__str_print_char);//Printf to the logfile
}
if(f_err_code)
f_close(&FATFS_logfile);//Close the already opened file on error
else
file_opened=0x01;//So we know to close the file properly on shutdown - bit mask for the files
if(!f_err_code) {
f_err_code=f_lseek(&FATFS_wavfile_accel, PRE_SIZE);// Pre-allocate clusters
if (f_err_code || f_tell(&FATFS_wavfile_accel) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_wavfile_accel, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
}
if(f_err_code)
f_close(&FATFS_wavfile_accel);//Close the already opened file on error
else
file_opened|=0x02;//So we know to close the file properly on shutdown - bit mask for the files
}
if(!f_err_code) {
f_err_code=f_lseek(&FATFS_wavfile_gyro, PRE_SIZE);// Pre-allocate clusters
if (f_err_code || f_tell(&FATFS_wavfile_gyro) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_wavfile_gyro, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
}
if(f_err_code)
f_close(&FATFS_wavfile_gyro);//Close the already opened file on error
else
file_opened|=0x04;//So we know to close the file properly on shutdown - bit mask for the files
}
}
}
repetition_counter=0; //Reset this here
//We die, but flash out a number of flashes first
if(f_err_code || deadly_flashes) { //There was an init error
for(;deadly_flashes;deadly_flashes--) {
RED_LED_ON;
Delay(200000);
RED_LED_OFF;
Delay(200000);
Watchdog_Reset();
}
RED_LED_ON;
Delay(400000);
shutdown(); //Abort after a (further )single red flash
}
}
/**
* @brief Main program
* @param None
* @retval None
*/
int main(void)
{
/* At this stage the microcontroller clock setting is already configured,
this is done through SystemInit() function which is called from startup
file (startup_stm32f2xx.s) before to branch to application main.
To reconfigure the default setting of SystemInit() function, refer to
system_stm32f2xx.c file
*/
/* I2C configuration ---------------------------------------------------------*/
I2C_Config();
/* Initialize LEDs mounted on STM322xG-EVAL board */
STM_EVAL_LEDInit(LED1);
STM_EVAL_LEDInit(LED2);
STM_EVAL_LEDInit(LED3);
STM_EVAL_LEDInit(LED4);
/* SysTick configuration -----------------------------------------------------*/
SysTickConfig();
/* Clear the RxBuffer */
Fill_Buffer(RxBuffer, RXBUFFERSIZE);
/*************************************Master Code******************************/
#if defined (I2C_MASTER)
/* I2C Struct Initialize */
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DUTYCYCLE;
I2C_InitStructure.I2C_OwnAddress1 = 0xA0;
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_ClockSpeed = I2C_SPEED;
#ifndef I2C_10BITS_ADDRESS
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
#else
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_10bit;
#endif /* I2C_10BITS_ADDRESS */
/* I2C Initialize */
I2C_Init(I2Cx, &I2C_InitStructure);
/* Master Transmitter --------------------------------------------------------*/
/* Generate the Start condition */
I2C_GenerateSTART(I2Cx, ENABLE);
#ifdef I2C_10BITS_ADDRESS
/* Test on I2C1 EV5 and clear it */
TimeOut = USER_TIMEOUT;
while ((!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Send Header to I2Cx for write or time out */
I2C_SendData(I2Cx, HEADER_ADDRESS_Write);
/* Test on I2Cx EV9 and clear it */
TimeOut = USER_TIMEOUT;
while ((!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_ADDRESS10))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Send I2Cx slave Address for write */
I2C_Send7bitAddress(I2Cx, (uint8_t)SLAVE_ADDRESS, I2C_Direction_Transmitter);
#else /* I2C_7BITS_ADDRESS */
/* Test on I2Cx EV5 and clear it or time out */
TimeOut = USER_TIMEOUT;
while ((!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Send I2Cx slave Address for write */
I2C_Send7bitAddress(I2Cx, SLAVE_ADDRESS, I2C_Direction_Transmitter);
#endif /* I2C_10BITS_ADDRESS */
/* Test on I2Cx EV6 and clear it or time out */
TimeOut = USER_TIMEOUT;
while ((!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* I2Cx DMA Enable */
I2C_DMACmd(I2Cx, ENABLE);
/* Enable DMA TX Channel */
DMA_Cmd(I2Cx_DMA_STREAM_TX, ENABLE);
/* Wait until I2Cx_DMA_STREAM_TX enabled or time out */
TimeOut = USER_TIMEOUT;
while ((DMA_GetCmdStatus(I2Cx_DMA_STREAM_TX)!= ENABLE)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Wait until DMA Transfer Complete or time out */
TimeOut = USER_TIMEOUT;
while ((DMA_GetFlagStatus(I2Cx_DMA_STREAM_TX,I2Cx_TX_DMA_TCFLAG) == RESET)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* I2Cx DMA Disable */
I2C_DMACmd(I2Cx, DISABLE);
/* Wait until BTF Flag is set before generating STOP or time out */
TimeOut = USER_TIMEOUT;
while ((!I2C_GetFlagStatus(I2Cx,I2C_FLAG_BTF))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Send I2Cx STOP Condition */
I2C_GenerateSTOP(I2Cx, ENABLE);
/* Disable DMA TX Channel */
DMA_Cmd(I2Cx_DMA_STREAM_TX, DISABLE);
/* Wait until I2Cx_DMA_STREAM_TX disabled or time out */
TimeOut = USER_TIMEOUT;
while ((DMA_GetCmdStatus(I2Cx_DMA_STREAM_TX)!= DISABLE)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Clear any pending flag on Tx Stream */
DMA_ClearFlag(I2Cx_DMA_STREAM_TX, I2Cx_TX_DMA_TCFLAG | I2Cx_TX_DMA_FEIFLAG | I2Cx_TX_DMA_DMEIFLAG | \
I2Cx_TX_DMA_TEIFLAG | I2Cx_TX_DMA_HTIFLAG);
/* Master Receiver -----------------------------------------------------------*/
/* Enable DMA NACK automatic generation */
I2C_DMALastTransferCmd(I2Cx, ENABLE);
/* Send I2Cx START condition */
I2C_GenerateSTART(I2Cx, ENABLE);
#ifdef I2C_10BITS_ADDRESS
/* Test on EV5 and clear it or time out */
TimeOut = USER_TIMEOUT;
while ((!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Send Header to Slave for write */
I2C_SendData(I2Cx, HEADER_ADDRESS_Write);
/* Test on EV9 and clear it or time out */
TimeOut = USER_TIMEOUT;
while ((!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_ADDRESS10))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Send slave Address */
I2C_Send7bitAddress(I2Cx, (uint8_t)SLAVE_ADDRESS, I2C_Direction_Transmitter);
/* Test on I2Cx EV6 and clear it or time out*/
TimeOut = USER_TIMEOUT;
while ((!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Repeated Start */
I2C_GenerateSTART(I2Cx, ENABLE);
/* Test on EV5 and clear it or time out*/
TimeOut = USER_TIMEOUT;
while ((!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Send Header to Slave for Read */
I2C_SendData(I2Cx, HEADER_ADDRESS_Read);
#else /* I2C_7BITS_ADDRESS */
/* Test on I2Cx EV5 and clear it or time out*/
TimeOut = USER_TIMEOUT;
while ((!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Send I2Cx slave Address for write */
I2C_Send7bitAddress(I2Cx, SLAVE_ADDRESS, I2C_Direction_Receiver);
#endif /* I2C_10BITS_ADDRESS */
/* Test on I2Cx EV6 and clear it or time out */
TimeOut = USER_TIMEOUT;
while ((!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* I2Cx DMA Enable */
I2C_DMACmd(I2Cx, ENABLE);
/* Enable DMA RX Channel */
DMA_Cmd(I2Cx_DMA_STREAM_RX, ENABLE);
/* Wait until I2Cx_DMA_STREAM_RX enabled or time out */
TimeOut = USER_TIMEOUT;
while ((DMA_GetCmdStatus(I2Cx_DMA_STREAM_RX)!= ENABLE)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Transfer complete or time out */
TimeOut = USER_TIMEOUT;
while ((DMA_GetFlagStatus(I2Cx_DMA_STREAM_RX,I2Cx_RX_DMA_TCFLAG)==RESET)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Send I2Cx STOP Condition */
I2C_GenerateSTOP(I2Cx, ENABLE);
/* Disable DMA RX Channel */
DMA_Cmd(I2Cx_DMA_STREAM_RX, DISABLE);
/* Wait until I2Cx_DMA_STREAM_RX disabled or time out */
TimeOut = USER_TIMEOUT;
while ((DMA_GetCmdStatus(I2Cx_DMA_STREAM_RX)!= DISABLE)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Disable I2C DMA request */
I2C_DMACmd(I2Cx,DISABLE);
/* Clear any pending flag on Rx Stream */
DMA_ClearFlag(I2Cx_DMA_STREAM_RX, I2Cx_RX_DMA_TCFLAG | I2Cx_RX_DMA_FEIFLAG | I2Cx_RX_DMA_DMEIFLAG | \
I2Cx_RX_DMA_TEIFLAG | I2Cx_RX_DMA_HTIFLAG);
if (Buffercmp(TxBuffer, RxBuffer, RXBUFFERSIZE) == PASSED)
{
/* LED2, LED3 and LED4 Toggle */
STM_EVAL_LEDOn(LED2);
STM_EVAL_LEDOn(LED3);
STM_EVAL_LEDOn(LED4);
}
else
{ /* ED2, LED3 and LED4 On */
STM_EVAL_LEDOff(LED2);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
}
#endif /* I2C_MASTER */
/**********************************Slave Code**********************************/
#if defined (I2C_SLAVE)
/* Initialize I2C peripheral */
/* I2C Init */
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DUTYCYCLE;
I2C_InitStructure.I2C_OwnAddress1 = SLAVE_ADDRESS;
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_ClockSpeed = I2C_SPEED;
#ifndef I2C_10BITS_ADDRESS
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
#else
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_10bit;
#endif /* I2C_10BITS_ADDRESS */
I2C_Init(I2Cx, &I2C_InitStructure);
/* Slave Receiver ------------------------------------------------------------*/
/* Test on I2C EV1 and clear it */
while (!I2C_CheckEvent(I2Cx, I2C_EVENT_SLAVE_RECEIVER_ADDRESS_MATCHED))
{}
/* I2Cx DMA Enable */
I2C_DMACmd(I2Cx, ENABLE);
/* Enable DMA RX Channel */
DMA_Cmd(I2Cx_DMA_STREAM_RX, ENABLE);
/* Wait until I2Cx_DMA_STREAM_RX enabled or time out */
TimeOut = USER_TIMEOUT;
while ((DMA_GetCmdStatus(I2Cx_DMA_STREAM_RX)!= ENABLE)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Transfer complete or time out */
TimeOut = USER_TIMEOUT;
while ((DMA_GetFlagStatus(I2Cx_DMA_STREAM_RX,I2Cx_RX_DMA_TCFLAG)==RESET)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Test on I2Cx EV4 and clear it or time out */
TimeOut = USER_TIMEOUT;
while ((!I2C_CheckEvent(I2Cx, I2C_EVENT_SLAVE_STOP_DETECTED))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Disable DMA RX Channel */
DMA_Cmd(I2Cx_DMA_STREAM_RX, DISABLE);
/* Wait until I2Cx_DMA_STREAM_RX disabled or time out */
TimeOut = USER_TIMEOUT;
while ((DMA_GetCmdStatus(I2Cx_DMA_STREAM_RX)!= DISABLE)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Disable I2C DMA request */
I2C_DMACmd(I2Cx,DISABLE);
/* Clear any pending flag on Rx Stream */
DMA_ClearFlag(I2Cx_DMA_STREAM_RX, I2Cx_RX_DMA_TCFLAG | I2Cx_RX_DMA_FEIFLAG | I2Cx_RX_DMA_DMEIFLAG | \
I2Cx_RX_DMA_TEIFLAG | I2Cx_RX_DMA_HTIFLAG);
/* Slave Transmitter ---------------------------------------------------------*/
/* Test on I2C EV1 and clear it or time out*/
while (!I2C_CheckEvent(I2Cx, I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED))
{}
/* I2Cx DMA Enable */
I2C_DMACmd(I2Cx, ENABLE);
/* Enable DMA RX Channel */
DMA_Cmd(I2Cx_DMA_STREAM_TX, ENABLE);
/* Wait until I2Cx_DMA_STREAM_TX enabled or time out */
TimeOut = USER_TIMEOUT;
while ((DMA_GetCmdStatus(I2Cx_DMA_STREAM_TX)!= ENABLE)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Transfer complete or time out */
TimeOut = USER_TIMEOUT;
while ((DMA_GetFlagStatus(I2Cx_DMA_STREAM_TX,I2Cx_TX_DMA_TCFLAG)==RESET)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Test on I2Cx EV3-2 and clear it or time out */
TimeOut = USER_TIMEOUT;
while ((!I2C_CheckEvent(I2Cx, I2C_EVENT_SLAVE_ACK_FAILURE))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Disable DMA TX Channel */
DMA_Cmd(I2Cx_DMA_STREAM_TX, DISABLE);
/* Wait until I2Cx_DMA_STREAM_TX disabled or time out */
TimeOut = USER_TIMEOUT;
while ((DMA_GetCmdStatus(I2Cx_DMA_STREAM_TX)!= DISABLE)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
/* Disable I2C DMA request */
I2C_DMACmd(I2Cx,DISABLE);
/* Clear any pending flag on Tx Stream */
DMA_ClearFlag(I2Cx_DMA_STREAM_TX, I2Cx_TX_DMA_TCFLAG | I2Cx_TX_DMA_FEIFLAG | I2Cx_TX_DMA_DMEIFLAG | \
I2Cx_TX_DMA_TEIFLAG | I2Cx_TX_DMA_HTIFLAG);
if (Buffercmp(TxBuffer, RxBuffer, RXBUFFERSIZE) == PASSED)
{
/* LED2, LED3 and LED4 are On */
STM_EVAL_LEDOn(LED2);
STM_EVAL_LEDOn(LED3);
STM_EVAL_LEDOn(LED4);
}
else
{ /* LED2, LED3 and LED4 are Off */
STM_EVAL_LEDOff(LED2);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
}
#endif /* I2C_SLAVE */
while(1)
{}
}
/*******************************************************************************
* Function Name : CODEC_ReadRegister
* Description : Reads a register of the audio Codec through I2C.
* Input : - RegisterAddr: The target register address (between 00x and 0x24)
* Output : None
* Return : The value of the read register
*******************************************************************************/
uint32_t CODEC_ReadRegister(uint32_t RegisterAddr)
{
uint32_t tmp = 0;
/* Disable the I2C1 peripheral */
I2C_Cmd(I2C1, DISABLE);
/* Reset all I2C2 registers */
I2C_SoftwareResetCmd(I2C1, ENABLE);
I2C_SoftwareResetCmd(I2C1, DISABLE);
/* Configure the I2C peripheral */
I2C_Config();
/* Enable the I2C peripheral */
I2C_GenerateSTART(I2C1, ENABLE);
/* Test on EV5 and clear it */
while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT))
{}
/* Disable Acknowledgement */
I2C_AcknowledgeConfig(I2C1, DISABLE);
/* Transmit the slave address and enable writing operation */
I2C_Send7bitAddress(I2C1, CODEC_ADDRESS, I2C_Direction_Transmitter);
/* Test on EV6 and clear it */
while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
{}
/* Transmit the first address for r/w operations */
I2C_SendData(I2C1, RegisterAddr);
/* Test on EV8 and clear it */
while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED))
{}
/* Regenerate a start condition */
I2C_GenerateSTART(I2C1, ENABLE);
/* Test on EV5 and clear it */
while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT))
{}
/* Transmit the slave address and enable writing operation */
I2C_Send7bitAddress(I2C1, CODEC_ADDRESS, I2C_Direction_Receiver);
/* Test on EV6 and clear it */
while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED))
{}
/* Test on EV7 and clear it */
while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED))
{}
/* End the configuration sequence */
I2C_GenerateSTOP(I2C1, ENABLE);
/* Load the register value */
tmp = I2C_ReceiveData(I2C1);
/* Disable Acknowledgement */
I2C_AcknowledgeConfig(I2C1, ENABLE);
/* Return the read value */
return tmp;
}
/*******************************************************************************
* Function Name : CODEC_WriteRegister
* Description : Writes a value in a register of the audio Codec through I2C.
* Input : - RegisterAddr: The target register address (between 00x and 0x24)
* : - RegisterValue: The target register value to be written
* : - Verify: 0-> Don't verify the written data, 1-> Verify the written data
* Output : None
* Return : - 0 -> Correct write operation
* : - !0 -> Incorrect write operation
*******************************************************************************/
uint32_t CODEC_WriteRegister(uint32_t RegisterAddr, uint32_t RegisterValue)
{
uint32_t read_verif = 0;
/* Reset all I2C2 registers */
I2C_SoftwareResetCmd(I2C1, ENABLE);
I2C_SoftwareResetCmd(I2C1, DISABLE);
/* Enable the I2C1 peripheral */
I2C_Cmd(I2C1, ENABLE);
/* Configure the I2C peripheral */
I2C_Config();
/* Begin the config sequence */
I2C_GenerateSTART(I2C1, ENABLE);
/* Test on EV5 and clear it */
while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT))
{}
/* Transmit the slave address and enable writing operation */
I2C_Send7bitAddress(I2C1, CODEC_ADDRESS, I2C_Direction_Transmitter);
/* Test on EV6 and clear it */
while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
{}
/* Transmit the first address for r/w operations */
I2C_SendData(I2C1, RegisterAddr);
/* Test on EV8 and clear it */
while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED))
{}
/* Prepare the register value to be sent */
I2C_SendData(I2C1, RegisterValue);
/* Test on EV8 and clear it */
while (!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED))
{}
/* End the configuration sequence */
I2C_GenerateSTOP(I2C1, ENABLE);
/* Verify (if needed) that the loaded data is correct */
#ifdef VERIFY_WRITTENDATA
/* Read the just written register*/
read_verif = CODEC_ReadRegister(RegisterAddr);
/* Load the register and verify its value */
if (read_verif != RegisterValue)
{
/* Control data wrongly transferred */
read_verif = 1;
}
else
{
/* Control data correctly transferred */
read_verif = 0;
}
#endif
/* Return the verifying value: 0 (Passed) or 1 (Failed) */
return read_verif;
}
int main(void)
{
uint32_t ppg; //PPG channel
uint32_t data_counter=0; //used as data timestamp
uint8_t system_state=0; //used to track button press functionality
float sensor_data; //used for handling data passed back from sensors
RTC_t RTC_time;
_REENT_INIT_PTR(&my_reent);
_impure_ptr = &my_reent;
SystemInit(); //Sets up the clk
setup_gpio(); //Initialised pins, and detects boot source
DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); //Watchdog stopped during JTAG halt
if(RCC->CSR&RCC_CSR_IWDGRSTF) { //Watchdog reset, turn off
RCC->CSR|=RCC_CSR_RMVF; //Reset the reset flags
shutdown();
}
SysTick_Configuration(); //Start up system timer at 100Hz for uSD card functionality
Watchdog_Config(WATCHDOG_TIMEOUT); //Set the watchdog
Watchdog_Reset(); //Reset watchdog as soon as possible incase it is still running at power on
rtc_init(); //Real time clock initialise - (keeps time unchanged if set)
Usarts_Init();
ISR_Config();
rprintfInit(__usart_send_char); //Printf over the bluetooth
if(USB_SOURCE==bootsource) {
Set_System(); //This actually just inits the storage layer
Set_USBClock();
USB_Interrupts_Config();
USB_Init();
uint32_t nojack=0x000FFFFF; //Countdown timer - a few hundered ms of 0v on jack detect forces a shutdown
while (bDeviceState != CONFIGURED) { //Wait for USB config - timeout causes shutdown
if(Millis>10000 || !nojack) //No USB cable - shutdown (Charger pin will be set to open drain, cant be disabled without usb)
shutdown();
if(GET_VBUS_STATE) //Jack detect resets the countdown
nojack=0x0FFFFF;
nojack--;
Watchdog_Reset(); //Reset watchdog here, if we are stalled here the Millis timeout should catch us
}
USB_Configured_LED();
EXTI_ONOFF_EN(); //Enable the off interrupt - allow some time for debouncing
while(1) { //If running off USB (mounted as mass storage), stay in this loop - dont turn on anything
if(Millis%1000>500) //1Hz on/off flashing
switch_leds_on(); //Flash the LED(s)
else
switch_leds_off();
Watchdog_Reset();
}
}
if(!GET_PWR_STATE) //Check here to make sure the power button is still pressed, if not, sleep
shutdown(); //This means a glitch on the supply line, or a power glitch results in sleep
for(uint8_t n=0;n<PPG_CHANNELS;n++)
init_buffer(&(Buff[n]),PPG_BUFFER_SIZE);//Enough for ~0.25S of data
setup_pwm(); //Enable the PWM outputs on all three channels
Delay(100000); //Sensor+inst amplifier takes about 100ms to stabilise after power on
ADC_Configuration(); //We leave this a bit later to allow stabilisation
calibrate_sensor(); //Calibrate the offset on the diff pressure sensor
EXTI_ONOFF_EN(); //Enable the off interrupt - allow some time for debouncing
I2C_Config(); //Setup the I2C bus
uint8_t sensors_=detect_sensors(); //Search for connected sensors
sensor_data=GET_BATTERY_VOLTAGE; //Have to flush adc for some reason
Delay(10000);
if(!(sensors_&~(1<<PRESSURE_HOSE))||GET_BATTERY_VOLTAGE<BATTERY_STARTUP_LIMIT) {//We will have to turn off
Watchdog_Reset(); //LED flashing takes a while
if(abs(Reported_Pressure)>PRESSURE_MARGIN)
Set_Motor(-1); //If the is air backpressure, dump to rapidly drop to zero pressure before turnoff
if(file_opened)
f_close(&FATFS_logfile); //be sure to terminate file neatly
red_flash();
Delay(400000);
red_flash(); //Two flashes means battery abort -----------------ABORT 2
if(sensors_&~(1<<PRESSURE_HOSE))
shutdown();
Delay(400000);
red_flash(); //Three flashes means no sensors abort ------------ABORT 3
shutdown();
}
if((f_err_code = f_mount(0, &FATFS_Obj)))Usart_Send_Str((char*)"FatFs mount error\r\n");//This should only error if internal error
else { //FATFS initialised ok, try init the card, this also sets up the SPI1
if(!f_open(&FATFS_logfile,"time.txt",FA_OPEN_EXISTING | FA_READ | FA_WRITE)) {//Try and open a time file to get the system time
if(!f_stat((const TCHAR *)"time.txt",&FATFS_info)) {//Get file info
if(!FATFS_info.fsize) { //Empty file
RTC_time.year=(FATFS_info.fdate>>9)+1980;//populate the time struct (FAT start==1980, RTC.year==0)
RTC_time.month=(FATFS_info.fdate>>5)&0x000F;
RTC_time.mday=FATFS_info.fdate&0x001F;
RTC_time.hour=(FATFS_info.ftime>>11)&0x001F;
RTC_time.min=(FATFS_info.ftime>>5)&0x003F;
RTC_time.sec=(FATFS_info.ftime<<1)&0x003E;
rtc_settime(&RTC_time);
rprintfInit(__fat_print_char);//printf to the open file
printf("RTC set to %d/%d/%d %d:%d:%d\n",RTC_time.mday,RTC_time.month,RTC_time.year,\
RTC_time.hour,RTC_time.min,RTC_time.sec);
}
}
f_close(&FATFS_logfile); //Close the time.txt file
}
#ifndef SINGLE_LOGFILE
rtc_gettime(&RTC_time); //Get the RTC time and put a timestamp on the start of the file
rprintfInit(__str_print_char); //Print to the string
printf("%d-%d-%dT%d-%d-%d.txt",RTC_time.year,RTC_time.month,RTC_time.mday,RTC_time.hour,RTC_time.min,RTC_time.sec);//Timestamp name
rprintfInit(__usart_send_char); //Printf over the bluetooth
#endif
if((f_err_code=f_open(&FATFS_logfile,LOGFILE_NAME,FA_CREATE_ALWAYS | FA_WRITE))) {//Present
printf("FatFs drive error %d\r\n",f_err_code);
if(f_err_code==FR_DISK_ERR || f_err_code==FR_NOT_READY)
Usart_Send_Str((char*)"No uSD card inserted?\r\n");
}
else { //We have a mounted card
f_err_code=f_lseek(&FATFS_logfile, PRE_SIZE);// Pre-allocate clusters
if (f_err_code || f_tell(&FATFS_logfile) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_logfile, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
else
rprintfInit(__str_print_char);//Printf to the logfile
}
if(f_err_code)
f_close(&FATFS_logfile);//Close the already opened file on error
else
file_opened=1; //So we know to close the file properly on shutdown
}
}
/* 硬件初始化 */
void sensors_hw_init(void)
{
I2C_Config();
}
/**
* @brief Main program
* @param None
* @retval None
*/
int main(void)
{
/*At this stage the microcontroller clock setting is already configured,
this is done through SystemInit() function which is called from startup
file (startup_stm32f2xx.s) before to branch to application main.
To reconfigure the default setting of SystemInit() function, refer to
system_stm32f2xx.c file
*/
/* I2C configuration ---------------------------------------------------------*/
I2C_Config();
/* Initialize LEDs mounted on STM322xG-EVAL board */
STM_EVAL_LEDInit(LED1);
STM_EVAL_LEDInit(LED2);
STM_EVAL_LEDInit(LED3);
STM_EVAL_LEDInit(LED4);
#if defined (I2C_MASTER)
/* Initialize push-buttons mounted on STM322xG-EVAL board */
TimeOut = USER_TIMEOUT;
while ((IOE_Config() != IOE_OK) && (TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
#endif /* I2C_MASTER */
/* SysTick configuration -----------------------------------------------------*/
SysTickConfig();
/*************************************Master Code******************************/
#if defined (I2C_MASTER)
/* I2C De-initialize */
I2C_DeInit(I2Cx);
/*!< I2C Struct Initialize */
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DUTYCYCLE;
I2C_InitStructure.I2C_OwnAddress1 = 0xA0;
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_ClockSpeed = I2C_SPEED;
#ifndef I2C_10BITS_ADDRESS
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
#else
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_10bit;
#endif /* I2C_10BITS_ADDRESS */
/*!< I2C Initialize */
I2C_Init(I2Cx, &I2C_InitStructure);
/* Enable Error Interrupt */
I2C_ITConfig(I2Cx, I2C_IT_ERR , ENABLE);
/* I2C ENABLE */
I2C_Cmd(I2Cx, ENABLE);
while (1)
{
CmdTransmitted = 0x00;
NumberOfByte = 0x00;
Tx_Idx = 0x00;
/* Clear PressedButton by reading joystick */
PressedButton = IOE_JoyStickGetState();
/* Waiting joystick pressed */
while (PressedButton == JOY_NONE)
{
PressedButton = IOE_JoyStickGetState();
}
/* I2C in Master Transmitter Mode ----------------------------------------*/
switch (PressedButton)
{
/* JOY_RIGHT button pressed */
case JOY_RIGHT:
NumberOfByte = CMD_RIGHT_SIZE;
CmdTransmitted = CMD_RIGHT;
break;
/* JOY_LEFT button pressed */
case JOY_LEFT:
NumberOfByte = CMD_LEFT_SIZE;
CmdTransmitted = CMD_LEFT;
break;
/* JOY_UP button pressed */
case JOY_UP:
NumberOfByte = CMD_UP_SIZE;
CmdTransmitted = CMD_UP;
break;
/* JOY_DOWN button pressed */
case JOY_DOWN:
NumberOfByte = CMD_DOWN_SIZE;
CmdTransmitted = CMD_DOWN;
break;
/* JOY_SEL button pressed */
case JOY_SEL:
NumberOfByte = CMD_SEL_SIZE;
CmdTransmitted = CMD_SEL;
break;
default:
break;
}
if (CmdTransmitted != 0x00)
{
/* Enable Error and Buffer Interrupts */
I2C_ITConfig(I2Cx, (I2C_IT_EVT | I2C_IT_BUF), ENABLE);
/* Generate the Start condition */
I2C_GenerateSTART(I2Cx, ENABLE);
/* Data transfer is performed in the I2C interrupt routine */
/* Wait until end of data transfer or time out */
TimeOut = USER_TIMEOUT;
while ((Tx_Idx < GetVar_NbrOfDataToTransfer())&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
TimeOut = USER_TIMEOUT;
while ((I2C_GetFlagStatus(I2Cx, I2C_FLAG_BUSY))&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
}
}
#endif /* I2C_MASTER */
/**********************************Slave Code**********************************/
#if defined (I2C_SLAVE)
I2C_DeInit(I2Cx);
/* Initialize I2C peripheral */
/*!< I2C Init */
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DUTYCYCLE;
I2C_InitStructure.I2C_OwnAddress1 = SLAVE_ADDRESS;
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_ClockSpeed = I2C_SPEED;
#ifndef I2C_10BITS_ADDRESS
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
#else
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_10bit;
#endif /* I2C_10BITS_ADDRESS */
I2C_Init(I2Cx, &I2C_InitStructure);
/* Enable Error Interrupt */
I2C_ITConfig(I2Cx, (I2C_IT_ERR | I2C_IT_EVT | I2C_IT_BUF), ENABLE);
/* I2C ENABLE */
I2C_Cmd(I2Cx, ENABLE);
/* Infinite Loop */
while (1)
{
CmdReceived = 0x00;
NumberOfByte = 0x00;
/* Clear the RxBuffer */
Fill_Buffer(RxBuffer, RXBUFFERSIZE);
while (CmdReceived == 0x00)
{}
/* Wait until end of data transfer */
while (Rx_Idx < GetVar_NbrOfDataToReceive())
{}
/* I2C in Slave Receiver Mode --------------------------------------------*/
if (CmdReceived != 0x00)
{
switch (Rx_Idx)
{
/* Right button pressed */
case CMD_RIGHT_SIZE:
if (Buffercmp(TxBuffer, RxBuffer, CMD_RIGHT_SIZE) == PASSED)
{
/* Turn ON LED2 and LED3 */
STM_EVAL_LEDOn(LED2);
STM_EVAL_LEDOn(LED3);
/* Turn all other LEDs off */
STM_EVAL_LEDOff(LED4);
}
break;
/* Left button pressed*/
case CMD_LEFT_SIZE:
if (Buffercmp(TxBuffer, RxBuffer, CMD_LEFT_SIZE) == PASSED)
{
/* Turn ON LED4 */
STM_EVAL_LEDOn(LED4);
/* Turn all other LEDs off */
STM_EVAL_LEDOff(LED2);
STM_EVAL_LEDOff(LED3);
}
break;
/* Up button pressed */
case CMD_UP_SIZE:
if (Buffercmp(TxBuffer, RxBuffer, CMD_UP_SIZE) == PASSED)
{
/* Turn ON LED2 */
STM_EVAL_LEDOn(LED2);
/* Turn all other LEDs off */
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
}
break;
/* Down button pressed */
case CMD_DOWN_SIZE:
if (Buffercmp(TxBuffer, RxBuffer, CMD_DOWN_SIZE) == PASSED)
{
/* Turn ON LED3 */
STM_EVAL_LEDOn(LED3);
/* Turn all other LEDs off */
STM_EVAL_LEDOff(LED2);
STM_EVAL_LEDOff(LED4);
}
break;
/* Sel button pressed */
case CMD_SEL_SIZE:
if (Buffercmp(TxBuffer, RxBuffer, CMD_SEL_SIZE) == PASSED)
{
/* Turn ON all LEDs */
STM_EVAL_LEDOn(LED2);
STM_EVAL_LEDOn(LED3);
STM_EVAL_LEDOn(LED4);
}
break;
default:
break;
}
}
}
#endif /* I2C_SLAVE */
}
int main( void )
{
uart_config_t uart0;
gpio_config_t key1;
i2c_config_t i2c0;
i2c_command_t command;
spi_config_t spi0;
spi_command_t spi_cmd;
adc_config_t adc5;
dac_config_t dac0;
error_code_t error;
uart0.baudrate = B115200;
uart0.block_type = NON_BLOCKING;
uart0.buffer = 0;
uart0.irqhandler = 0;
uart0.parity = UART_PARITY_NONE;
uart0.uart_port = COM0;
uart0.wordlen = UART_WORDLEN8;
uart0.stopbit = STOP_BIT_1_BIT;
key1.Direction = INPUT;
key1.Initial_Value = LO;
key1.Interrupt_Type = INTERRUPT_ENABLED_BOTH;
key1.Pin = PIN11;
key1.Port= PORT2;
key1.Pin_Mode = PULLUP_PULLDOWN_DISABLED;
key1.Pin_Mode_OD = PIN_MODE_OPEN_DRAIN_NORMAL;
key1.Pin_Typedef = 0;
key1.irqhandler = 0;
i2c0.i2c_port = I2C0;
i2c0.i2c_mode = MASTER;
i2c0.datarate = STANDARD;
i2c0.buffer = 0;
i2c0.irqhandler = 0;
spi0.bits = SPI_8_BITS;
spi0.buffer = 0;
spi0.clk_phase = SPI_PHASE_INPHASE;
spi0.clk_polarity = SPI_CLK_RISING;
spi0.freq = 5e6;
spi0.irqhandler = 0;
spi0.lsbf = MSB_FIRST;
spi0.mode = SPI_MASTER;
spi0.port = SPI0;
spi0.dummyData = 0x00;
adc5.channel = ADC_CHANNEL5;
adc5.rate = 200e3;
adc5.trigger_mode = BURST;
adc5.irqhandler = 0;
dac0.sampling_rate = 32e3;
if ( (error = GPIO_Config(&key1)) != NO_ERROR ) {
while (1);
}
signal_level_t trigger_mode;
GPIO_GetLevel(&key1, &trigger_mode);
if (trigger_mode == HI) {
adc5.trigger_mode = BURST;
} else {
adc5.trigger_mode = MANUAL;
}
if ( (error = UART_Config(&uart0)) != NO_ERROR) {
while (1);
}
SetDebug_Port(COM0);
if ( (error = I2C_Config(&i2c0)) != NO_ERROR ) {
while (1);
}
if ( (error = SPI_Config(&spi0)) != NO_ERROR ) {
while (1);
}
if ( (error = ADC_Config(&adc5)) != NO_ERROR ) {
while (1);
}
if ( (error = DAC_Config(&dac0)) != NO_ERROR ) {
while (1);
}
uc_printf ("HardWare Initialized\n\r");
/* READ ID USING READ-ID */
//Write Register.
spi_cmd.writeReg = 0x90;
spi_cmd.writeRegValid = 1;
uint16_t write_data[] = {0x00, 0x00, 0x00 };
spi_cmd.writeBuffer = write_data;
spi_cmd.writeDataSize = sizeof(write_data);
//Read Register.
uint16_t read_data[4];
spi_cmd.readReg = 0x00;
spi_cmd.readRegValid = 0;
spi_cmd.readBuffer = read_data;
spi_cmd.readDataSize = sizeof(read_data);
spi_cmd.operation = SPI_WRITE | SPI_READ; //back to back.
error = SPI_Write(SPI0, &spi_cmd);
if (error != NO_ERROR) {
while (1);
}
uc_printf ("SPI Flash Device ID(READ-ID) %d %d %d %d\n\r", read_data[0], read_data[1], read_data[2], read_data[3]);
/* READ ID USING JEDEC READ */
uint16_t read_data_jedec[4];
spi_cmd.readReg = 0x9F;
spi_cmd.readRegValid = 1;
spi_cmd.readBuffer = read_data_jedec;
spi_cmd.readDataSize = sizeof(read_data_jedec);
error = SPI_Read(SPI0, &spi_cmd);
if (error != NO_ERROR) {
while (1);
}
uc_printf ("SPI Flash Device ID(Jedec) %d %d %d %d\n\r", read_data_jedec[0], read_data_jedec[1], read_data_jedec[2], read_data_jedec[3]);
int size = 18;
char fw_version[18] = "PowerAvrVersion5.5";
char fw_version_read[18] = { 0 };
command.address = 0x50;
command.data = fw_version;
command.operation = WRITE;
command.reg = 0x00;
command.size = 1; //11;
int i, j, k;
for (j=0; j<1; j++) {
for (i=0; i<size;i++) {
//UART_PutChars(COM0,"Writing", 9);
command.reg = 0x00 + i;
command.size = 1;
command.data = &(fw_version[i]);
error = I2C_Write(I2C0, &command);
if (error != NO_ERROR) {
break;
}
//This is to cause a delay after write. This should be replaced with the wait in the I2C_Write Operation.
for (k=0; k<16; k++) {
uc_printf(".");
uc_printf("\b");
}
}
if (error != NO_ERROR) {
while (1);
}
}
uc_printf("I2C Write Done\n\r");
command.operation = READ;
command.data = fw_version_read;
command.size = 1 ;
for (j=0; j<1; j++) {
for (i=0; i<size;i++) {
fw_version_read[i] = 'X';
}
for (i=0; i<size;i++) {
command.reg = 0x00 + i;
command.size = 1;
command.data = &(fw_version_read[i]);
error = I2C_Read(I2C0, &command);
if (error != NO_ERROR) {
break;
}
}
if (error != NO_ERROR) {
while (1);
}
}
uc_printf("I2C Read Done\n\r");
for (i=0; i<size; i++) {
uc_printf("%c",fw_version_read[i]);
}
uc_printf("\n\r");
pin_interrupt_type_t key1_status;
float vol=0;
int timeout = 0;
while (1) {
error = GPIO_GetIRQ(&key1, &key1_status);
if (error != NO_ERROR) {
while (1);
}
if (key1_status == INTERRUPT_ENABLED_FALLING) {
uc_printf ("Falling Edge\n\r");
error = GPIO_ClrIRQ(&key1);
if (error != NO_ERROR) {
while (1);
}
if ( (error = ADC_Read(&adc5)) != NO_ERROR ) {
while (1);
}
} else if (key1_status == INTERRUPT_ENABLED_RISING) {
uc_printf ("Rising Edge\n\r");
error = GPIO_ClrIRQ(&key1);
if (error != NO_ERROR) {
while (1);
}
}
//#if (DISPLAY_ADC == 1)
//if (adc5.trigger_mode == BURST) {
if (timeout == 20000) {
ADC_Read(&adc5);
timeout = 0;
} else {
timeout++;
}
//}
if (adc5.done) {
vol =0 ;
int i=0;
int result = adc5.result;
for (i=0; i<4096; i+=256) {
vol++;
if (result >= i && result <= (i + 256) ) {
break;
}
}
/*
for (i=0; i<adc5.result/33; i++) {
uc_printf("*");
}
uc_printf("\r");
*/
//uc_printf ("%d\n\r", adc5.result);
adc5.done = 0;
}
//#endif
//#if (PLAY_PIANO == 1)
if (vol == 0) {
vol = 1;
} else if (vol > 1) {
//vol = vol ; //1 + (vol - 1)*0.2;
}
Play_Piano(&dac0, vol );
//#endif
}
#if (UART_TEST == 1)
char rx_data;
char tx_data[] = "This is x nnnnn\n\r";
while (1) {
//if ( UART_GetChar(COM0, &rx_data) != FIFO_EMPTY ) {
tx_data[8] = 'a';
//UART_PutChar(COM0, rx_data);
UART_PutChars(COM0, tx_data, 17 );
int i;
tx_data[8] = 'b';
for (i=0; i<17; i++) {
UART_PutChar(COM0, tx_data[i]);
}
//}
}
#endif
return 0;
}
/**
* @brief Main program
* @param None
* @retval None
*/
int main(void)
{
/*!< At this stage the microcontroller clock setting is already configured,
this is done through SystemInit() function which is called from startup
file (startup_stm32l1xx_xx.s) before to branch to application main.
To reconfigure the default setting of SystemInit() function, refer to
system_stm32l1xx.c file
*/
/* I2C configuration ------------------------------------------------------*/
I2C_Config();
/* Initialize LEDs mounted on STM32L152-EVAL board */
STM_EVAL_LEDInit(LED1);
STM_EVAL_LEDInit(LED2);
STM_EVAL_LEDInit(LED3);
STM_EVAL_LEDInit(LED4);
/* SysTick configuration ---------------------------------------------------*/
SysTickConfig();
/*************************************Master Code******************************/
#if defined (I2C_MASTER)
/* I2C De-initialize */
I2C_DeInit(I2Cx);
/*!< I2C Struct Initialize */
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DUTYCYCLE;
I2C_InitStructure.I2C_OwnAddress1 = 0xA0;
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_ClockSpeed = I2C_SPEED;
#ifndef I2C_10BITS_ADDRESS
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
#else
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_10bit;
#endif /* I2C_10BITS_ADDRESS */
/*!< I2C Initialize */
I2C_Init(I2Cx, &I2C_InitStructure);
/* Enable Error Interrupt */
I2C_ITConfig(I2Cx, I2C_IT_ERR , ENABLE);
/* I2C ENABLE */
I2C_Cmd(I2Cx, ENABLE);
while (1)
{
NumberOfByteToReceive = RXBUFFERSIZE;
Rx_Idx = 0x00;
/* Clear the RxBuffer */
Fill_Buffer(RxBuffer, RXBUFFERSIZE);
/* Enable Event Interrupts */
I2C_ITConfig(I2Cx, I2C_IT_EVT , ENABLE);
/* Enable Acknowledge */
I2C_AcknowledgeConfig(I2Cx, ENABLE);
/* Generate the Start condition */
I2C_GenerateSTART(I2Cx, ENABLE);
/* Data transfer is performed in the I2C interrupt routine */
/* Wait until end of data transfer or time out*/
TimeOut = USER_TIMEOUT;
while ((Rx_Idx < RXBUFFERSIZE)&&(TimeOut != 0x00))
{}
if(TimeOut == 0)
{
TimeOut_UserCallback();
}
if (Buffercmp(TxBuffer, RxBuffer, RXBUFFERSIZE) == PASSED)
{
/* LED2, LED3 and LED4 Toggle */
STM_EVAL_LEDToggle(LED2);
STM_EVAL_LEDToggle(LED3);
STM_EVAL_LEDToggle(LED4);
}
else
{
/* ED2, LED3 and LED4 On */
STM_EVAL_LEDOff(LED2);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
}
delay(10);
}
#endif /* I2C_MASTER */
/**********************************Slave Code**********************************/
#if defined (I2C_SLAVE)
/*I2Cx DeInitialize*/
I2C_DeInit(I2Cx);
/*!< I2C Struct Initialize */
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DUTYCYCLE;
I2C_InitStructure.I2C_OwnAddress1 = SLAVE_ADDRESS;
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_ClockSpeed = I2C_SPEED;
#ifndef I2C_10BITS_ADDRESS
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
#else
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_10bit;
#endif /* I2C_10BITS_ADDRESS */
I2C_Init(I2Cx, &I2C_InitStructure);
/* Enable Error Interrupt */
I2C_ITConfig(I2Cx, I2C_IT_ERR , ENABLE);
/* I2C ENABLE */
I2C_Cmd(I2Cx, ENABLE);
/* Infinite Loop */
while (1)
{
Tx_Idx = 0x00;
/* Enable I2C event interrupt */
I2C_ITConfig(I2Cx, I2C_IT_EVT, ENABLE);
/* Wait until end of data transfer */
while (Tx_Idx < TXBUFFERSIZE)
{
}
/* LED2, LED3 and LED4 Toggle */
STM_EVAL_LEDToggle(LED2);
STM_EVAL_LEDToggle(LED3);
STM_EVAL_LEDToggle(LED4);
}
#endif /* I2C_SLAVE */
}
int main(void)
{
Delay_ms(100);
Periph_clock_enable();
GPIO_Config();
Usart4Init();
I2C_Config();
ADC_Config();
MPU6050_Init();
Timer1_Config();
Timer8_Config();
Timer2_Config();
Timer5_Config();
Timer4_Config();
Timer3_Config();//RC control timer
NVIC_Configuration();
EXTI_Config();
TIM_Cmd(TIM5, ENABLE);
TIM_CtrlPWMOutputs(TIM5, ENABLE);
for (i = 1 ; i < 1 ; i++) ; //small delay before starting Timer4
TIM_Cmd(TIM4, ENABLE);
TIM_CtrlPWMOutputs(TIM4, ENABLE);
Delay_ms(100);
for (i = 0; i < configDataSize; i++) //reads configuration from eeprom
{
ReadFromEEPROM(i);
configData[i] = EepromData;
Delay_ms(5);
}
I2C_AcknowledgeConfig(I2C2, ENABLE);
Delay_ms(100);
while (1)
{
LEDon;
DEBUG_LEDon;
while (ConfigMode == 1)
{
TimerOff(); //Configuration loop
}
MPU6050_ACC_get();//Getting Accelerometer data
acc_roll_angle = -(atan2(accADC_x, accADC_z)) + (configData[11] - 50.00) * 0.0035; //Calculating pitch ACC angle+callibration
acc_pitch_angle = +(atan2(accADC_y, accADC_z)); //Calculating roll ACC angle
MPU6050_Gyro_get();//Getting Gyroscope data
acc_roll_angle_vid = ((acc_roll_angle_vid * 99.00) + acc_roll_angle) / 100.00; //Averaging pitch ACC values
acc_pitch_angle_vid = ((acc_pitch_angle_vid * 99.00) + acc_pitch_angle) / 100.00; //Averaging roll ACC values
sinus = sinusas[(int)(rc4)]; //Calculating sinus
cosinus = sinusas[90 - (int)(rc4)]; //Calculating cosinus
ROLL = -gyroADC_z * sinus + gyroADC_y * cosinus;
roll_angle = (roll_angle + ROLL * dt) + 0.0002 * (acc_roll_angle_vid - roll_angle); //Roll Horizon
//ROLL=-gyroADC_z*sinus+gyroADC_y*cosinus;
yaw_angle = (yaw_angle + gyroADC_z * dt); //Yaw
pitch_angle_true = ((pitch_angle_true + gyroADC_x * dt) + 0.0002 * (acc_pitch_angle_vid - pitch_angle_true)); //Pitch Horizon
ADC1Ch1_vid = ((ADC1Ch1_vid * 99.00) + (readADC1(1) / 4000.00)) / 100.00; //Averaging ADC values
ADC1Ch1_vid = 0.00;
rc4_avg = ((rc4_avg * 499.00) + (rc4)) / 500.00; //Averaging RC4 values
pitch_angle = pitch_angle_true - rc4_avg / 57.3; //Adding angle
pitch_angle_correction = pitch_angle * 150.0;
if (pitch_angle_correction > 2.0)
{
pitch_angle_correction = 2.0;
}
if (pitch_angle_correction < -2.0)
{
pitch_angle_correction = -2.0;
}
pitch_setpoint = pitch_setpoint + pitch_angle_correction; //Pitch return to zero after collision
roll_angle_correction = roll_angle * 200.0;
if (roll_angle_correction > 2.0)
{
roll_angle_correction = 2.0;
}
if (roll_angle_correction < -2.0)
{
roll_angle_correction = -2.0;
}
roll_setpoint = roll_setpoint + roll_angle_correction; //Roll return to zero after collision
ADC1Ch13_vid = ((ADC1Ch13_vid * 99.00) + ((readADC1(13) - 2000) / 4000.00)) / 100.00; //Averaging ADC values
if (configData[10] == '0')
{
yaw_angle = (yaw_angle + gyroADC_z * dt) + 0.01 * (ADC1Ch13_vid - yaw_angle); //Yaw AutoPan
}
if (configData[10] == '1')
{
yaw_angle = (yaw_angle + gyroADC_z * dt); //Yaw RCPan
}
yaw_angle_correction = yaw_angle * 50.0;
if (yaw_angle_correction > 1.0)
{
yaw_angle_correction = 1.0;
}
if (yaw_angle_correction < -1.0)
{
yaw_angle_correction = -1.0;
}
yaw_setpoint = yaw_setpoint + yaw_angle_correction; //Yaw return to zero after collision
pitch_PID();//Pitch axis pid
roll_PID(); //Roll axis pid
yaw_PID(); //Yaw axis pid
printcounter++; //Print data to UART
if (printcounter >= 100)
{
//sprintf (buff, " %d %d %c Labas\n\r", ACCread[0], ACCread[1], ACCread[2]);
//sprintf (buff, " %x %x %x %x %x %x Labas\n\r", ACCread[0], ACCread[1], ACCread[2], ACCread[3], ACCread[4], ACCread[5]);
//sprintf (buff, "Labas %d %d\n\r", ACCread[0], ACCread[1]);
//sprintf (buff, "%3.1f %f\n\r", ADC1Ch1_vid*57.3, sinus);
//sprintf (buff, "Labas %f %f %f \n\r", accADC_x, accADC_y, accADC_z);
//sprintf (buff, "%3.1f %3.1f \n\r", acc_roll_angle_vid*57.3, acc_pitch_angle_vid *57.3);
//sprintf (buff, "%3.1f %3.1f \n\r", pitch_angle*57.3, roll_angle*57.3);
//sprintf (buff, "%d\n\r", rc4);
//USART_PutString(buff);
printcounter = 0;
}
stop = 0;
LEDoff;
watchcounter = 0;
while (stop == 0) {} //Closed loop waits for interrupt
}
}
