int main(void)
{
Periph_clock_enable();
GPIO_Config();
LEDon;
Delay_ms(10); //short blink
LEDoff;
Delay_ms(50);
Usart4Init();
ADC_Config();
MPU6050_Init();
Timer2_Config();
Timer3_Config();//RC control timer
NVIC_Configuration();
EXTI_Config();
//engineInit(); ///????to initialize all variables;
configLoad();
MPU6050_Gyro_calibration();
while(1)
{
engineProcess();
while(stop==0) {}//Closed loop waits for interrupt
}
}
/**********主程序*******/
int main()
{
#ifdef DEBUG
debug();
#endif
/* System Clocks Configuration */
SystemInit();
/* Enable the FSMC Clock */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_FSMC, ENABLE);
/* Configure FSMC */
FSMC_LCD_Init();
/* Init for LCD */
LCD_Setup();
EXTI_Config();
SPI_Flash_Init();
NVIC_Configuration();
/*变量初始化*/
GlobalStateFlag=0;
MenuFlag=1;
Pic_Addr=gImage_picture1;
HomePage_Disp();
/* Infinite loop */
while (1)
{
}
}
void NRF24L01_Init(void)
{
SPI2_Init(); //初始化SPI
NRF24L01_CE_L(); //使能24L01
NRF24L01_CSN_H(); //SPI片选取消
EXTI_Config();//配置中断
//清除接收、发送缓冲区、标志
NRF24L01_Write_Reg(NRF24L01_FLUSH_RX,0xff);//清除RX FIFO寄存器
NRF24L01_Write_Reg(NRF24L01_FLUSH_TX,0xff);//清除TX FIFO寄存器
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+NRF24L01_STATUS,0xff); //清除TX_DS或MAX_RT中断标志
}
/*********************************************************************//**
* @brief c_entry: Main program body
* @param[in] None
* @return None
**********************************************************************/
void c_entry (void)
{
EXTI_InitTypeDef EXTICfg;
/* Initialize debug via UART0
* – 115200bps
* – 8 data bit
* – No parity
* – 1 stop bit
* – No flow control
*/
debug_frmwrk_init();
// print welcome screen
print_menu();
/* Initialize EXT pin and registers
* P2.10 as /EINT0
*/
PINSEL_ConfigPin(2,10,1);
EXTI_Init();
EXTICfg.EXTI_Line = EXTI_EINT0;
/* edge sensitive */
EXTICfg.EXTI_Mode = EXTI_MODE_EDGE_SENSITIVE;
EXTICfg.EXTI_polarity = EXTI_POLARITY_LOW_ACTIVE_OR_FALLING_EDGE;
EXTI_ClearEXTIFlag(EXTI_EINT0);
EXTI_Config(&EXTICfg);
NVIC_SetPriorityGrouping(4);
NVIC_SetPriority(EINT0_IRQn, 0);
NVIC_EnableIRQ(EINT0_IRQn);
_DBG_("Press '1' to enter system in deep sleep mode.\n\r"\
"If you want to wake-up the system, press INT/WAKE-UP button.");
while(_DG !='1')
{
_DBG_("I'm waiting...\n\r");
}
_DBG_("I'm sleeping...");
// Enter target deep sleep mode
CLKPWR_DeepSleep();
SystemInit();
debug_frmwrk_init();
// MCU will be here after waking up
_DBG_("\n\r-------- I'm wake up! -------- ");
while (1);
}
/*
---------------------------------------------------------------------------
* 函数名称：void Application(void)
* 函数功能：读取图像数据，进行图像处理根据处理结果控制舵机方向与车速
* 输入形参：无
* 返回值：无
---------------------------------------------------------------------------
*/
void Application(void)
{
Change_Speed(540);
while(1)
{
/*如果图像数据可读*/
if(OV_State==Read)
{
/*屏蔽中断*/
EXTI_Config(DIS);
/*读取图像*/
Read_Pic();
/*图像处理*/
Image_Process();
/*舵机、电调控制*/
Steer_control();
SpeedContiol(2500);
/*图像处理完毕状态转换*/
OV_State=Wait_Vsync1;
/*打开中断*/
EXTI_Config(EN);
}
}
}
/*********************************************************************//**
* @brief EINT0 Interrupt Handler
* @param[in] None
*
* @return None
**********************************************************************/
void EINT_BSP_Init(EINT_init_t *eint)
{
//EXTI_InitTypeDef exti_cfg;
if(eint == NULL)
return;
PINSEL_ConfigPin (eint->port, eint->pin, eint->func_num);
PINSEL_SetPinMode(eint->port,eint->pin,IOCON_MODE_PLAIN);
GPIO_SetDir(eint->port,1<<eint->pin,0);
// exti_cfg.EXTI_Line = EXTI_EINT0;
// exti_cfg.EXTI_Mode = EXTI_MODE_EDGE_SENSITIVE;
// exti_cfg.EXTI_polarity = EXTI_POLARITY_HIGH_ACTIVE_OR_RISING_EDGE;
if(eint->exti_cfg == NULL)
return;
EXTI_Config(eint->exti_cfg);
if(eint->irq_msg == NULL)
return;
if(eint->eint_num == 0)
{
//EINT0_msg_p = eint->irq_msg;
memcpy(&EINT0_msg_p,eint->irq_msg,sizeof(IRQ_msg_t));
NVIC_EnableIRQ(EINT0_IRQn);
}
else if(eint->eint_num == 1)
{
memcpy(&EINT1_msg_p,eint->irq_msg,sizeof(IRQ_msg_t));
NVIC_EnableIRQ(EINT1_IRQn);
}
else if(eint->eint_num == 2)
{
memcpy(&EINT2_msg_p,eint->irq_msg,sizeof(IRQ_msg_t));
NVIC_EnableIRQ(EINT2_IRQn);
}
else if(eint->eint_num == 3)
{
memcpy(&EINT3_msg_p,eint->irq_msg,sizeof(IRQ_msg_t));
NVIC_EnableIRQ(EINT3_IRQn);
}
}
//==============================================================================
// Sleep 設定 (EUP)
//==============================================================================
void F_Sleep_Init(void)
{
F_AllLCDRam(0);
F_Display();
F_1625CS_1(0);
F_1625WR(0);
F_1625DATA(0);
F_BackLight(0);
F_IncControlUp(0);
F_IncControlDown(0);
F_EUP_EEPROM_GPIO();
// EUP
//M_ERP_SetHigh;
M_ERP_SetLow;
EXTI_Config();
F_ERP_KBSet();
/* Request to enter STOP mode with regulator in low power mode */
PWR_EnterSTOPMode(PWR_Regulator_LowPower, PWR_STOPEntry_WFI);
EXTI_DeInit();
IWDG_Config(0);
}
/*********************************************************************//**
* @brief c_entry: Main program body
* @param[in] None
* @return None
**********************************************************************/
void c_entry (void)
{
EXTI_InitTypeDef EXTICfg;
GPIO_Init();
/* Initialize debug via UART0
* – 115200bps
* – 8 data bit
* – No parity
* – 1 stop bit
* – No flow control
*/
debug_frmwrk_init();
PINSEL_ConfigPin(2, 10,1);
ADC_Init(LPC_ADC, 100);
ADC_IntConfig(LPC_ADC, BRD_ADC_PREPARED_INTR, ENABLE);
ADC_ChannelCmd(LPC_ADC, BRD_ADC_PREPARED_CHANNEL, ENABLE);
GPIO_SetDir(BRD_LED_1_CONNECTED_PORT, BRD_LED_1_CONNECTED_MASK, GPIO_DIRECTION_OUTPUT);
GPIO_SetDir(BRD_LED_2_CONNECTED_PORT, BRD_LED_2_CONNECTED_MASK, GPIO_DIRECTION_OUTPUT);
/* Initialize External 0 interrupt */
EXTI_Init();
EXTICfg.EXTI_Line = EXTI_EINT0;
/* edge sensitive */
EXTICfg.EXTI_Mode = EXTI_MODE_EDGE_SENSITIVE;
EXTICfg.EXTI_polarity = EXTI_POLARITY_LOW_ACTIVE_OR_FALLING_EDGE;
EXTI_Config(&EXTICfg);
#if (INT_MODE == 0) //same group, different sub-levels (Tail-chaining example)
//sets group priorities: 8 - subpriorities: 3
NVIC_SetPriorityGrouping(4);
//000:10 (bit 7:3) assign eint0 to group 0, sub-priority 2 within group 0
NVIC_SetPriority(EINT0_IRQn, 2);
NVIC_SetPriority(ADC_IRQn, 0x01);
#else //different group - (Late-arriving example) ==================================================
//sets group priorities: 8 - subpriorities: 3
NVIC_SetPriorityGrouping(4);
//000:00 (bit 7:3) assign eint0 to group 0, sub-priority 0 within group 0
NVIC_SetPriority(EINT0_IRQn, 0);
NVIC_SetPriority(ADC_IRQn, 0x04);
#endif
NVIC_EnableIRQ(EINT0_IRQn);
/* Enable ADC in NVIC */
NVIC_EnableIRQ(ADC_IRQn);
while(1)
{
// Start conversion
ADC_StartCmd(LPC_ADC, ADC_START_NOW);
/* Enable ADC in NVIC */
NVIC_EnableIRQ(ADC_IRQn);
}
}
static void SetupHardware( void )
{
u8 buf[32];
#if 0//debug by karlno
u8 *ptr1,*ptr2,*ptr3,*ptr4;
#endif
GPIO_Config();//针对gpio的一些配置
NVIC_Config();//针对中断向量的一些配置
EXTI_Config();//外部中断配置
#if 0//debug by karlno
Debug("#### 123\n\r");
ptr1=Q_Mallco(200);
ptr2=Q_Mallco(68);
Q_Free(ptr1);
ptr3=Q_Mallco(68);
ptr3=Q_Mallco(68);
while(1);
#endif
Debug("\n\n\n\r************************************\n\r");
Debug("* Q-SYS %s *\n\r",QSYS_VERSION);
Debug("* Start up our dreams! *\n\r");
#if OS_USE_FREERTOS
Debug("* Base on FreeRTOS *\n\r");
#elif OS_USE_UCOS
Debug("* Base on uC/OS *\n\r");
#endif
Debug("* Hardware PID : %d %d *\n\r",QXW_PRODUCT_ID,QXW_LCM_ID);
Debug("* Hardware ID : %X *\n\r",GetHwID());
Debug("* Www.Q-ShareWe.Com *\n\r");
Debug("************************************\n\r\n\r");
M25P16_Init();//spi flash
M25P16_Read_Id(buf);
Debug("SPI Flash ID:\n\r");
DisplayBuf(buf,20,8);
Debug("\n\r");
SPI_Touch_Init();//触摸屏
VsInit();//Vs1003的配置
//sd卡文件系统初始化
if(disk_initialize(0))
{
Debug("Disk Initialize error!\n\r");
}
else
{
Debug("Disk Initialize OK!\n\r");
#if 0//debug
{
FIL fsrc; // file objects
FRESULT res; // FatFs function common result code
UINT br;
u8 *p=Q_Mallco(1024);
u32 time=QW_GetNowTimeMs();
res = f_mount(0,&FS); // 初始化分区结构，它不初始化SD卡
if(res == FR_OK)
{
Debug("\n\nATA mounrt OK, fs_type = %d\n\r",FS.fs_type);
}
else
{
Debug("\nATA mounrt Error!!!\n%d\n\r",res);
}
res = f_open(&fsrc,"1.rar",FA_READ) ;
Debug("Open %d\n\r",res);
while(1)
{
res = f_read(&fsrc,p,fsrc.fsize,&br); // 读文件数据，长度file.fsize，
if(br==0)break;
}
f_close(&fsrc);
Debug("Time Gap:%d\n\r",QW_GetNowTimeMs()-time);
while(1);
}
#endif
FS_Init();
Debug("File system mount OK!\n\r");
Q_DB_SetStatus(Status_FsInitFinish,TRUE,NULL,0);
}
Adc_Init();
Tim2_Init();//用户定时器
Tim3_Init();//背光pwm初始化
Tim4_Init();//用户定时器
Tim5_Init();//用户定时器
USB_SetHw();
USB_Init();
}
OpenOBC::OpenOBC() : displayMode(reinterpret_cast<volatile DisplayMode_Type&>(LPC_RTC->GPREG0)), averageLitresPer100km(reinterpret_cast<volatile float&>(LPC_RTC->GPREG1)), averageFuelConsumptionSeconds(reinterpret_cast<volatile uint32_t&>(LPC_RTC->GPREG2))
{
wdt.start(10);
GPIO_IntDisable(0, 0xffffffff, 0);
GPIO_IntDisable(0, 0xffffffff, 1);
GPIO_IntDisable(2, 0xffffffff, 0);
GPIO_IntDisable(2, 0xffffffff, 1);
//enable bus, usage, and memmanage faults
SCB->SHCSR |= (1<<18) | (1<<17) | (1<<16);
idle = new IO(2, 0, true);
isr = new IO(2, 1, true);
obcS = this;
doHardwareTest = false;
callback = new Callback();
// callback->addCallback(this, &OpenOBC::buttonMemo, 10000);
__enable_irq();
SysTick_Config(SystemCoreClock/1000 - 1); //interrupt period 1ms
debug = new Debug(DEBUG_TX_PORTNUM, DEBUG_TX_PINNUM, DEBUG_RX_PORTNUM, DEBUG_RX_PINNUM, DEBUG_BAUD, &interruptManager); debugS = debug;
// printf("-"); fflush(stdout); delay(250); printf("-"); fflush(stdout); delay(250); printf("-"); fflush(stdout); delay(250); printf(">\r\n"); delay(250);
printf("--->\r\n");
printf("clock speed: %i MHz\r\n", SystemCoreClock / 1000000);
printf("stack: 0x%lx heap: 0x%lx free: %li\r\n", get_stack_top(), get_heap_end(), get_mem_free());
vrefEn = new IO(VREF_EN_PORT, VREF_EN_PIN, true, false);
//spi coniguration
spi0 = new SPI(SPI0_MOSI_PORT, SPI0_MOSI_PIN, SPI0_MISO_PORT, SPI0_MISO_PIN, SPI0_SCK_PORT, SPI0_SCK_PIN);
spi1 = new SPI(SPI1_MOSI_PORT, SPI1_MOSI_PIN, SPI1_MISO_PORT, SPI1_MISO_PIN, SPI1_SCK_PORT, SPI1_SCK_PIN);
//i2c configuration
i2c0 = new I2C(I2C0_SDA_PORT, I2C0_SDA_PIN, I2C0_SCL_PORT, I2C0_SCL_PIN);
i2c1 = new I2C(I2C1_SDA_PORT, I2C1_SDA_PIN, I2C1_SCL_PORT, I2C1_SCL_PIN);
//i/o expander configuration
Input* io0Interrupt = new Input(PCA95XX_INTERRUPT_PORT, PCA95XX_INTERRUPT_PIN);
io = new PCA95xx(*i2c0, PCA95XX_ADDRESS, *io0Interrupt, 400000);
codeLed = new PCA95xxPin(*io, CODE_LED_PORT, CODE_LED_PIN, true);
limitLed = new PCA95xxPin(*io, LIMIT_LED_PORT, LIMIT_LED_PIN, true);
timerLed = new PCA95xxPin(*io, TIMER_LED_PORT, TIMER_LED_PIN, true);
ccmLight = new PCA95xxPin(*io, CCM_LIGHT_PORT, CCM_LIGHT_PIN, true);
chime0 = new PCA95xxPin(*io, CHIME0_PORT, CHIME0_PIN, true);
chime1 = new PCA95xxPin(*io, CHIME1_PORT, CHIME1_PIN, true);
ventilation = new PCA95xxPin(*io, VENTILATION_PORT, VENTILATION_PIN, true);
antitheftHorn = new PCA95xxPin(*io, ANTITHEFT_HORN_PORT, ANTITHEFT_HORN_PIN, true);
ews = new PCA95xxPin(*io, EWS_PORT, EWS_PIN, true);
out0 = new PCA95xxPin(*io, OUT0_PORT, OUT0_PIN, true);
out1 = new PCA95xxPin(*io, OUT1_PORT, OUT1_PIN, true);
out2 = new PCA95xxPin(*io, OUT2_PORT, OUT2_PIN, true);
out3 = new PCA95xxPin(*io, OUT3_PORT, OUT3_PIN, true);
io0 = new PCA95xxPin(*io, IO0_PORT, IO0_PIN, false);
io1 = new PCA95xxPin(*io, IO1_PORT, IO1_PIN, false);
//lcd configuration
lcdBiasEn = new PCA95xxPin(*io, LCD_BIAS_EN_PORT, LCD_BIAS_EN_PIN, true);
lcdReset = new IO(LCD_RESET_PORT, LCD_RESET_PIN, false);
// IO lcdBias(LCD_BIASCLOCK_PORT, LCD_BIASCLOCK_PIN, true);
lcdBiasClock = new PWM(LCD_BIASCLOCK_PORT, LCD_BIASCLOCK_PIN, .99);
IO* lcdSel = new IO(LCD_SELECT_PORT, LCD_SELECT_PIN, true);
IO* lcdRefresh = new IO(LCD_REFRESH_PORT, LCD_REFRESH_PIN, false);
IO* lcdUnk0 = new IO(LCD_UNK0_PORT, LCD_UNK0_PIN, true);
IO* lcdUnk1 = new IO(LCD_UNK1_PORT, LCD_UNK1_PIN, false);
lcd = new ObcLcd(*spi1, *lcdSel, *lcdRefresh, *lcdUnk0, *lcdUnk1);
*lcdReset = true;
*lcdBiasEn = true;
//backlight configuration
lcdLight = new IO(LCD_BACKLIGHT_PORT, LCD_BACKLIGHT_PIN, true);
clockLight = new IO(CLOCK_BACKLIGHT_PORT, CLOCK_BACKLIGHT_PIN, true);
auxLight = new IO(AUX_BACKLIGHT_PORT, AUX_BACKLIGHT_PIN, true);
keypadLight = new IO(KEYPAD_BACKLIGHT_PORT, KEYPAD_BACKLIGHT_PIN, true);
// lcdBacklight = new PWM(LCD_BACKLIGHT_PORT, LCD_BACKLIGHT_PIN, .2);
// clockBacklight = new PWM(CLOCK_BACKLIGHT_PORT, CLOCK_BACKLIGHT_PIN);
printf("openOBC firmware version: %s\r\n", GIT_VERSION);
//config file configuration
IO* sdcardCs = new IO(SDCARD_CS_PORT, SDCARD_CS_PIN);
sd = new SDFS(*spi1, *sdcardCs);
sd->mount("sd");
config = new ConfigFile("/sd/openobc.cfg");
//parse the config file; create one with default parameters if it doesn't exist
if(config->readConfig() < 0)
{
int32_t result = config->writeConfig();
if(result >= 0)
{
DEBUG("created new config file: %s\r\n", config->getFilename().c_str());
}
else
{
DEBUG("couldn't open file for writing: %s\r\n", config->getFilename().c_str());
}
}
ObcBootupText::mode mode;
std::string bootupTextMode = config->getValueByNameWithDefault("BootupTextMode", "GitHash");
std::string bootupText;
std::string customBootupText = config->getValueByNameWithDefault("CustomBootupText", "openOBC");
uint bootupDelay = atoi(config->getValueByNameWithDefault("BootupDelay", "800").c_str());
if(bootupTextMode == "GitHash")
{
mode = ObcBootupText::GitHash;
bootupText = "openOBC " GIT_VERSION;
lcd->printfClock("%s", GIT_TAG);
}
else if(bootupTextMode == "Custom")
{
mode = ObcBootupText::Custom;
bootupText = customBootupText;
}
else if(bootupTextMode == "None")
{
mode = ObcBootupText::None;
bootupText = "";
}
lcd->printf("%s", bootupText.c_str());
ccmLight->on();
codeLed->on();
limitLed->on();
timerLed->on();
delay(bootupDelay);
if(config->getValueByNameWithDefault("LogConsoleToFile", "no") == "yes")
freopen("/sd/stdout.log", "a", stdout);
//default config file parameters
if(config->getValueByName("DefaultDisplayMode") == "")
config->setValueByName("DefaultDisplayMode", "DISPLAY_LAST_DISPLAYMODE");
if(config->getValueByName("BatteryVoltageCalibration") == "")
config->setValueByName("BatteryVoltageCalibration", "1.0000");
//default display mode configuration
std::string defaultDisplayModeString = config->getValueByName("DefaultDisplayMode");
if(defaultDisplayModeString == "DISPLAY_LAST_DISPLAYMODE")
displayMode = displayMode;
else if(defaultDisplayModeString == "DISPLAY_CHECK")
displayMode = DISPLAY_CHECK;
else if(defaultDisplayModeString == "DISPLAY_CONSUM1")
displayMode = DISPLAY_CONSUM1;
else if(defaultDisplayModeString == "DISPLAY_CONSUM2")
displayMode = DISPLAY_CONSUM2;
else if(defaultDisplayModeString == "DISPLAY_CONSUM3")
displayMode = DISPLAY_CONSUM3;
else if(defaultDisplayModeString == "DISPLAY_CONSUM4")
displayMode = DISPLAY_CONSUM4;
else if(defaultDisplayModeString == "DISPLAY_FREEMEM")
displayMode = DISPLAY_FREEMEM;
else if(defaultDisplayModeString == "DISPLAY_RANGE1")
displayMode = DISPLAY_RANGE1;
else if(defaultDisplayModeString == "DISPLAY_RANGE2")
displayMode = DISPLAY_RANGE2;
else if(defaultDisplayModeString == "DISPLAY_ACCELEROMETER")
displayMode = DISPLAY_ACCELEROMETER;
else if(defaultDisplayModeString == "DISPLAY_OPENOBC")
displayMode = DISPLAY_OPENOBC;
else if(defaultDisplayModeString == "DISPLAY_SPEED")
displayMode = DISPLAY_SPEED;
else if(defaultDisplayModeString == "DISPLAY_TEMP")
displayMode = DISPLAY_TEMP;
else if(defaultDisplayModeString == "DISPLAY_VOLTAGE")
displayMode = DISPLAY_VOLTAGE;
else
displayMode = DISPLAY_OPENOBC;
clockDisplayMode = CLOCKDISPLAY_CLOCK;
batteryVoltageCalibration = atof(config->getValueByName("BatteryVoltageCalibration").c_str());
//rtc configuration
rtc = new RTC(); rtcS = rtc;
RTC_TIME_Type settime;
settime.YEAR = 2012;
settime.MONTH = 9;
settime.DOM = 20;
settime.HOUR = 20;
settime.MIN = 29;
settime.SEC = 0;
// rtc->setTime(&settime);
//accelerometer configuration
Input* accelInterrupt = new Input(ACCEL_INTERRUPT_PORT, ACCEL_INTERRUPT_PIN);
accel = new MMA845x(*i2c0, ACCEL_ADDRESS, *accelInterrupt, interruptManager);
//ccm configuration
Input* ccmData = new Input(CCM_DATA_PORT, CCM_DATA_PIN);
IO* ccmClock = new IO(CCM_CLOCK_PORT, CCM_CLOCK_PIN);
IO* ccmLatch = new IO(CCM_LATCH_PORT, CCM_LATCH_PIN);
uint8_t ccmDisableMask = strtoul(config->getValueByNameWithDefault("ObcCheckDisableMask", "0x%02x", DEFAULT_CCM_DISABLE_MASK).c_str(), NULL, 0);
uint8_t ccmInvertMask = strtoul(config->getValueByNameWithDefault("ObcCheckInvertMask", "0x%02x", DEFAULT_CCM_INVERT_MASK).c_str(), NULL, 0);
ccm = new CheckControlModule(*ccmData, *ccmClock, *ccmLatch, ccmDisableMask, ccmInvertMask);
//fuel level configuration
Input* fuelLevelInput = new Input(FUEL_LEVEL_PORT, FUEL_LEVEL_PIN);
fuelLevel = new FuelLevel(*fuelLevelInput, interruptManager);
//stalk button configuration
stalkButton = new Input(STALK_BUTTON_PORT, STALK_BUTTON_PIN, false);
//brake switch configuration
brakeSwitch = new Input(BRAKE_SWITCH_PORT, BRAKE_SWITCH_PIN);
brakeCheck = new Input(BRAKE_CHECK_PORT, BRAKE_CHECK_PIN);
//illumination input configuration
illumination = new Input(ILLUMINATION_PORT, ILLUMINATION_PIN);
//ambient light input configuration
ambientLight = new Input(AMBIENT_LIGHT_PORT, AMBIENT_LIGHT_PIN);
//diagnostics interface configuration
klWake = new IO(KL_WAKE_PORT, KL_WAKE_PIN, true);
kline = new Uart(KLINE_TX_PORTNUM, KLINE_TX_PINNUM, KLINE_RX_PORTNUM, KLINE_RX_PINNUM, KLINE_BAUD, UART_PARITY_EVEN, &interruptManager);
lline = new Uart(LLINE_TX_PORTNUM, LLINE_TX_PINNUM, LLINE_RX_PORTNUM, LLINE_RX_PINNUM, LLINE_BAUD, UART_PARITY_EVEN, &interruptManager);
DS2Bus* k = new Bus(*kline);
DS2Bus* l = new Bus(*lline);
diag = new DS2(*k, *l);
disableComms = false;
// while(1)
// {
// int length = (rand() % 15) + 1;
// uint8_t* data = new uint8_t[length];
// uint32_t r = rand();
// for(int i = 0; i < length; i++)
// data[i] = r >> (i*2);
// DS2Packet packet(0x55, data, length, DS2_16BIT);
// delete[] data;
// k->write(packet.getRawPacket(), packet.getPacketLength());
// // diag->query(packet, DS2_K);
// delay(200);
// }
zke = new E36ZKE4(*diag);
srs = new E36SRS(*diag);
ihkr = new E36IHKR(*diag);
kombi = new E36Kombi(*diag);
mk4 = new E36MK4(*diag);
//ignition/run/on input configuration
run = new Input(RUN_PORT, RUN_PIN);
PINSEL_CFG_Type pincfg;
pincfg.Funcnum = PINSEL_FUNC_1;
pincfg.OpenDrain = PINSEL_PINMODE_NORMAL;
pincfg.Pinmode = PINSEL_PINMODE_TRISTATE;
pincfg.Portnum = RUN_PORT;
pincfg.Pinnum = RUN_PIN;
interruptManager.attach(IRQ_EINT1, &runHandler); //do this first...
PINSEL_ConfigPin(&pincfg); //because this immediately triggers an unwanted interrupt
EXTI_Init();
EXTI_InitTypeDef EXTICfg;
EXTICfg.EXTI_Line = EXTI_EINT1;
EXTICfg.EXTI_Mode = EXTI_MODE_EDGE_SENSITIVE;
EXTICfg.EXTI_polarity = EXTI_POLARITY_HIGH_ACTIVE_OR_RISING_EDGE;
EXTI_ClearEXTIFlag(EXTI_EINT1);
EXTI_Config(&EXTICfg);
NVIC_SetPriorityGrouping(4);
NVIC_SetPriority(EINT1_IRQn, 0);
NVIC_EnableIRQ(EINT1_IRQn);
//fuel consumption configuration
Input* fuelConsInput = new Input(FUEL_CONS_PORT, FUEL_CONS_PIN);
fuelConsInput->setPullup();
fuelCons = new FuelConsumption(*fuelConsInput, interruptManager);
//speed configuration
Input* speedPin = new Input(SPEED_PORT, SPEED_PIN);
// speedPin->setPullup(); //for bench use only
speed = new SpeedInput(*speedPin, interruptManager);
//sd card configuration
sdcardDetect = new Input(SDCARD_DETECT_PORT, SDCARD_DETECT_PIN, false);
sdcardDetect->setPullup();
//keypad configuration
IO* x0 = new IO(1, 20);
IO* x1 = new IO(1, 21);
IO* x2 = new IO(1, 22);
IO* x3 = new IO(1, 25);
IO* x4 = new IO(1, 27);
Input* y0 = new Input(0, 0);
Input* y1 = new Input(0, 1);
Input* y2 = new Input(0, 10);
Input* y3 = new Input(0, 11);
keypad = new ObcKeypad(*x0, *x1, *x2, *x3, *x4, *y0, *y1, *y2, *y3, interruptManager);
keypad->attach(BUTTON_1000, this, &OpenOBC::button1000);
keypad->attach(BUTTON_100, this, &OpenOBC::button100);
keypad->attach(BUTTON_10, this, &OpenOBC::button10);
keypad->attach(BUTTON_1, this, &OpenOBC::button1);
//analog input configuration
batteryVoltage = new AnalogIn(BATTERY_VOLTAGE_PORT, BATTERY_VOLTAGE_PIN, REFERENCE_VOLTAGE, (10.0 + 1.0) / 1.0 * REFERENCE_VOLTAGE, atof(config->getValueByName("BatteryVoltageCalibration").c_str()));
temperature = new AnalogIn(EXT_TEMP_PORT,EXT_TEMP_PIN, REFERENCE_VOLTAGE);
analogIn1 = new AnalogIn(ANALOG_IN1_PORT, ANALOG_IN1_PIN, REFERENCE_VOLTAGE);
analogIn2 = new AnalogIn(ANALOG_IN2_PORT, ANALOG_IN2_PIN, REFERENCE_VOLTAGE);
vstart = new AnalogIn(VSTART_PORT, VSTART_PIN, REFERENCE_VOLTAGE, (10.0 + 1.0) / 1.0 * REFERENCE_VOLTAGE);
//analog output configuration
analogOut = new AnalogOut(ANALOG_OUT_PORT, ANALOG_OUT_PIN, REFERENCE_VOLTAGE);
analogOut->writeVoltage(1.0);
// averageFuelConsumptionSeconds = 0;
// averageLitresPer100km = 0;
debug->attach(this, &OpenOBC::uartHandler);
if(wdt.wasReset())
{
printf("WATCHDOG RESET\r\n");
lcd->printfClock("!!!!");
Timer flashTimer;
while((keypad->getKeys() & BUTTON_SET_MASK) != BUTTON_SET_MASK)
{
if(flashTimer.read_ms() <= 1500)
{
lcd->printf("WATCHDOG RESET");
}
else if(flashTimer.read_ms() <= 3000)
{
lcd->printf("push SET to continue");
}
else
{
flashTimer.start();
}
wdt.feed();
}
lcd->clear();
lcd->clearClock();
}
coolantTemperature = 0;
ui = new ObcUI(*lcd, *keypad, *config);
keypad->attachRaw(ui, &ObcUI::handleButtonEvent);
ui->addTask(new ObcClock(*this));
ui->addTask(new ObcOdometer(*this));
ui->addTask(new ObcSpeed(*this));
ui->addTask(new ObcMemo(*this));
ui->addTask(new ObcTemp(*this));
ui->addTask(new ObcCheck(*this));
ui->addTask(new ObcLimit(*this));
ui->addTask(new ObcConsum(*this));
ui->addTask(new ObcDist(*this));
ui->addTask(new ObcRange(*this));
ui->addTask(new ObcTimer(*this));
ui->addTask(new ObcKmmls(*this));
ui->addTask(new ObcCode(*this));
ui->wake();
lcd->printf("");
lcd->printfClock("");
ccmLight->off();
codeLed->off();
limitLed->off();
timerLed->off();
// if(keypad->getKeys() == (BUTTON_1000_MASK | BUTTON_10_MASK))
// if(*stalkButton)
if(doHardwareTest)
hardwareTest();
doSleepCheck = true;
}
/*********************************************************************//**
* @brief c_entry: Main ADC program body
* @param[in] None
* @return None
**********************************************************************/
void c_entry(void)
{
volatile uint32_t tmp;
#if !__DMA_USED__
uint32_t adc_value;
#endif
uint8_t quit;
EXTI_InitTypeDef EXTICfg;
#if __DMA_USED__
GPDMA_Channel_CFG_Type GPDMACfg;
#endif
GPIO_Init();
/* Initialize debug via UART0
* – 115200bps
* – 8 data bit
* – No parity
* – 1 stop bit
* – No flow control
*/
debug_frmwrk_init();
// print welcome screen
print_menu();
/*
* Init ADC pin connect
* AD0.2 on P0.25
*/
PINSEL_ConfigPin(BRD_ADC_PREPARED_CH_PORT, BRD_ADC_PREPARED_CH_PIN, BRD_ADC_PREPARED_CH_FUNC_NO);
PINSEL_SetAnalogPinMode(BRD_ADC_PREPARED_CH_PORT,BRD_ADC_PREPARED_CH_PIN,ENABLE);
#ifdef LPC177x_8x_ADC_BURST_MULTI
/*
* Init ADC pin connect
* AD0.3 on P0.26
*/
PINSEL_ConfigPin(0, 26, 1);
PINSEL_SetAnalogPinMode(0,26,ENABLE);
#endif
/* Configuration for ADC:
* select: ADC channel 2
* ADC channel 3
* ADC conversion rate = 400KHz
*/
ADC_Init(LPC_ADC, 400000);
ADC_ChannelCmd(LPC_ADC,BRD_ADC_PREPARED_CHANNEL,ENABLE);
#ifdef LPC177x_8x_ADC_BURST_MULTI
ADC_ChannelCmd(LPC_ADC,_ADC_CHANNEL_n,ENABLE);
#endif
#ifdef LPC177x_8x_ADC_INJECT_TEST
//Config P2.10 as EINT0
PINSEL_ConfigPin(2,10,1);
EXTI_Init();
EXTICfg.EXTI_Line = EXTI_EINT0;
/* edge sensitive */
EXTICfg.EXTI_Mode = EXTI_MODE_EDGE_SENSITIVE;
EXTICfg.EXTI_polarity = EXTI_POLARITY_LOW_ACTIVE_OR_FALLING_EDGE;
EXTI_Config(&EXTICfg);
GPIO_SetDir(LED_PORT,LED_PIN,1);
GPIO_SetValue(LED_PORT,LED_PIN);
NVIC_EnableIRQ(EINT0_IRQn);
#endif
#if __DMA_USED__
/* Initialize GPDMA controller */
GPDMA_Init();
// Setup GPDMA channel --------------------------------
// channel 0
GPDMACfg.ChannelNum = 0;
// Source memory - unused
GPDMACfg.SrcMemAddr = 0;
// Destination memory
GPDMACfg.DstMemAddr = (uint32_t)s_buf;
// Transfer size
GPDMACfg.TransferSize = DMA_SIZE;
// Transfer width - unused
GPDMACfg.TransferWidth = 0;
// Transfer type
GPDMACfg.TransferType = GPDMA_TRANSFERTYPE_P2M;
// Source connection
GPDMACfg.SrcConn = GPDMA_CONN_ADC;
// Destination connection - unused
GPDMACfg.DstConn = 0;
// Linker List Item - unused
GPDMACfg.DMALLI = 0;
/* Enable GPDMA interrupt */
NVIC_EnableIRQ(DMA_IRQn);
while(1)
{
for(tmp = 0; tmp < 0x1000; tmp++);
/* Reset terminal counter */
Channel0_TC = 0;
/* Reset Error counter */
Channel0_Err = 0;
for(tmp = 0; tmp < DMA_SIZE; tmp++)
{
s_buf[tmp] = 0;
}
//Start burst conversion
ADC_BurstCmd(LPC_ADC,ENABLE);
GPDMA_Setup(&GPDMACfg);
// Enable GPDMA channel 1
GPDMA_ChannelCmd(0, ENABLE);
/* Wait for GPDMA processing complete */
while ((Channel0_TC == 0));
GPDMA_ChannelCmd(0, DISABLE);
for(tmp = 0; tmp < DMA_SIZE; tmp++)
{
if(s_buf[tmp] & ADC_GDR_DONE_FLAG)
{
_DBG("ADC value on channel "); _DBD(ADC_GDR_CH(s_buf[tmp])); _DBG(": ");
_DBD32(ADC_GDR_RESULT(s_buf[tmp]));_DBG_("");
}
}
if(_DG_NONBLOCK(&quit) &&
(quit == 'Q' || quit == 'q'))
break;
}
#else
//Start burst conversion
ADC_BurstCmd(LPC_ADC,ENABLE);
while(1)
{
adc_value = ADC_ChannelGetData(LPC_ADC,BRD_ADC_PREPARED_CHANNEL);
_DBG("ADC value on channel "); _DBD(BRD_ADC_PREPARED_CHANNEL); _DBG(": ");
_DBD32(adc_value);
_DBG_("");
#ifdef LPC177x_8x_ADC_BURST_MULTI
adc_value = ADC_ChannelGetData(LPC_ADC,_ADC_CHANNEL_n);
_DBG("ADC value on channel 3: ");
_DBD32(adc_value);
_DBG_("");
#endif
// Wait for a while
for(tmp = 0; tmp < 1500000; tmp++);
if(_DG_NONBLOCK(&quit) &&
(quit == 'Q' || quit == 'q'))
break;
}
#endif /*__DMA_USED__*/
_DBG_("Demo termination!!!");
ADC_DeInit(LPC_ADC);
GPIO_Deinit();
}
/*********************************************************************//**
* @brief c_entry: Main program body
* @param[in] None
* @return int
**********************************************************************/
int c_entry (void)
{
PINSEL_CFG_Type PinCfg;
EXTI_InitTypeDef EXTICfg;
/* Initialize debug via UART0
* – 115200bps
* – 8 data bit
* – No parity
* – 1 stop bit
* – No flow control
*/
debug_frmwrk_init();
// print welcome screen
print_menu();
/* Initialize LEDs
* - If using MCB1700 board:
* LEDs: P1.28 and P1.29 are available
* - If using IAR1700 board:
* LEDs: LED1(P1.25) and LED2(P0.4) are available
* Turn off LEDs after initialize
*/
InitLED();
/* Initialize EXT pin and registers
* - If using MCB1700 board: EXTI0 is configured
* - If using IAR1700 board: EXTI2 is configured
*/
#ifdef MCB_LPC_1768
/* P2.10 as /EINT0 */
PinCfg.Funcnum = 1;
PinCfg.OpenDrain = 0;
PinCfg.Pinmode = 0;
PinCfg.Pinnum = 10;
PinCfg.Portnum = 2;
PINSEL_ConfigPin(&PinCfg);
#elif defined (IAR_LPC_1768)
/* P2.12 as /EINT2 */
PinCfg.Funcnum = 1;
PinCfg.OpenDrain = 0;
PinCfg.Pinmode = 0;
PinCfg.Pinnum = 12;
PinCfg.Portnum = 2;
PINSEL_ConfigPin(&PinCfg);
#endif
EXTI_Init();
EXTICfg.EXTI_Line = _EXTINT;
/* edge sensitive */
EXTICfg.EXTI_Mode = EXTI_MODE_EDGE_SENSITIVE;
EXTICfg.EXTI_polarity = EXTI_POLARITY_LOW_ACTIVE_OR_FALLING_EDGE;
EXTI_ClearEXTIFlag(_EXTINT);
EXTI_Config(&EXTICfg);
NVIC_SetPriorityGrouping(4);
NVIC_SetPriority(_EXT_IRQ, 0);
NVIC_EnableIRQ(_EXT_IRQ);
_DBG_("First LED is blinking in normal mode...\n\r" \
"Press '1' to enter system in sleep mode.\n\r"\
"If you want to wake-up the system, press INT/WAKE-UP button.");
while(_DG !='1')
{
//Blink first LED
#ifdef MCB_LPC_1768
//blink LED P1.28
GPIO_SetValue(1, (1<<28));
delay();
GPIO_ClearValue(1, (1<<28));
delay();
#elif defined (IAR_LPC_1768)
//blink LED1 (P1.25)
GPIO_SetValue(1, (1<<25));
delay();
GPIO_ClearValue(1, (1<<25));
delay();
#endif
}
_DBG_("Sleeping...");
// Enter target power down mode
CLKPWR_Sleep();
// MCU will be here after waking up
_DBG_("System wake-up! Second LED is blinking...");
//turn off first LED
#ifdef MCB_LPC_1768
GPIO_ClearValue(1, (1<<29));
#elif defined (IAR_LPC_1768)
GPIO_SetValue(1, (1<<25));
#endif
while (1)
{
//Blink second LED
#ifdef MCB_LPC_1768
//blink LED P1.29
GPIO_SetValue(1, (1<<29));
delay();
GPIO_ClearValue(1, (1<<29));
delay();
#elif defined (IAR_LPC_1768)
//blink LED2 (P0.4)
GPIO_SetValue(0, (1<<4));
delay();
GPIO_ClearValue(0, (1<<4));
delay();
#endif
}
}
void gpio_init() {
SPI_CFG_Type spiInitialization;
PINSEL_CFG_Type PinSelCfg;
/*-----------------------------------------------------------------
*--------------------------PORT 0---------------------------------
*-----------------------------------------------------------------
*/
GPIO_SetDir(0, 0xFFFFFFFF, OUTPUT);
PinSelCfg.Portnum = PINSEL_PORT_0;
// PinSelCfg.Pinnum = PINSEL_PIN_0;
// PinSelCfg.Funcnum = PINSEL_FUNC_3;
// PinSelCfg.OpenDrain = 0;
// PinSelCfg.Pinmode = 0;
// PINSEL_ConfigPin(&PinSelCfg);
//
// PinSelCfg.Pinnum = PINSEL_PIN_1;
// PINSEL_ConfigPin(&PinSelCfg);
PinSelCfg.Pinnum = PINSEL_PIN_2;
PinSelCfg.Funcnum = PINSEL_FUNC_1;
PinSelCfg.OpenDrain = PINSEL_PINMODE_NORMAL;
PinSelCfg.Pinmode = PINSEL_PINMODE_PULLUP;
PINSEL_ConfigPin(&PinSelCfg);
PinSelCfg.Pinnum = PINSEL_PIN_3;
PINSEL_ConfigPin(&PinSelCfg);
GPIO_SetDir(0, 0x01<<22, INPUT);
PinSelCfg.Pinnum = PINSEL_PIN_10;
PinSelCfg.Funcnum = PINSEL_FUNC_2;
PinSelCfg.OpenDrain = PINSEL_PINMODE_OPENDRAIN;
PinSelCfg.Pinmode = PINSEL_PINMODE_TRISTATE;
PINSEL_ConfigPin(&PinSelCfg);
PinSelCfg.Pinnum = PINSEL_PIN_11;
PINSEL_ConfigPin(&PinSelCfg);
//SET FMC_I2C1 in Hi-Z
GPIO_SetDir(0, 0x01<<19, INPUT);
GPIO_SetDir(0, 0x01<<20, INPUT);
// PinSelCfg.Pinmode = PINSEL_PINMODE_TRISTATE;
// PinSelCfg.Funcnum = PINSEL_FUNC_3;
// PinSelCfg.Pinnum = PINSEL_PIN_19;
// PINSEL_ConfigPin(&PinSelCfg);
//
// PinSelCfg.Pinnum = PINSEL_PIN_20;
// PINSEL_ConfigPin(&PinSelCfg);
PinSelCfg.Funcnum = PINSEL_FUNC_1;
PinSelCfg.OpenDrain = PINSEL_PINMODE_OPENDRAIN;
PinSelCfg.Pinnum = PINSEL_PIN_27;
PINSEL_ConfigPin(&PinSelCfg);
PinSelCfg.Pinnum = PINSEL_PIN_28;
PINSEL_ConfigPin(&PinSelCfg);
PinSelCfg.Pinnum = PINSEL_PIN_29;
PinSelCfg.Funcnum = PINSEL_FUNC_1;
PinSelCfg.OpenDrain = PINSEL_PINMODE_NORMAL;
PinSelCfg.Pinmode = PINSEL_PINMODE_TRISTATE;
PINSEL_ConfigPin(&PinSelCfg);
PinSelCfg.Pinnum = PINSEL_PIN_30;
PINSEL_ConfigPin(&PinSelCfg);
/*-----------------------------------------------------------------
*---------------------END OF PORT 0-------------------------------
*-----------------------------------------------------------------
*-----------------------------------------------------------------
* ------------------------PORT 1----------------------------------
* ----------------------------------------------------------------
*/
GPIO_SetDir(1, 0xFFFFFFFF, OUTPUT);
GPIO_SetDir(1, ( 0x01<<IPMI_GA0 | 0x01<<IPMI_GA1 | 0x01<<IPMI_GA2 ), INPUT);
GPIO_SetValue(LED_PORT, (0x01<<_IPMI_BLLED|0x01<<_IPMI_LED1|0x01<<_IPMI_LED2) );
// PinSelCfg.Portnum = PINSEL_PORT_1;
// PinSelCfg.Pinnum = PINSEL_PIN_30;
// PinSelCfg.Funcnum = PINSEL_FUNC_2;
// PinSelCfg.OpenDrain = PINSEL_PINMODE_NORMAL;
// PinSelCfg.Pinmode = PINSEL_PINMODE_TRISTATE;
// PINSEL_ConfigPin(&PinSelCfg);
GPIO_SetDir(ENA_PORT, 0x001<<ENA_PIN, INPUT);
GPIO_SetDir(2, 0x0400, INPUT);
GPIO_SetDir(IPMI_EJTHDL_PORT, 0x001<<IPMI_EJTHDL, INPUT);
GPIO_SetDir(1, 0x01<<22, INPUT);
GPIO_SetDir(PGOOD_PORT, 0x01<<PGOOD_PIN, INPUT);
gpio_clr_gpio_pin(_IPMI_BLLED, LED_PORT); // turn on blue LED ASAP
gpio_clr_gpio_pin(UC_PWRENOUT, UC_PWRENOUT_PORT); // turn off power enable
PinSelCfg.Pinnum = PINSEL_PIN_15;
PinSelCfg.Portnum = PINSEL_PORT_0;
PinSelCfg.Pinmode = PINSEL_PINMODE_PULLUP;
PinSelCfg.Funcnum = PINSEL_FUNC_3;
PinSelCfg.OpenDrain = PINSEL_PINMODE_NORMAL;
PINSEL_ConfigPin(&PinSelCfg);
PinSelCfg.Pinnum = PINSEL_PIN_18;
PINSEL_ConfigPin(&PinSelCfg);
GPIO_SetDir(1, 0x01<<26, OUTPUT);
GPIO_SetValue(1, 0x01<<26);
GPIO_SetDir(0, 0x01<<21, OUTPUT);
GPIO_SetDir(0, 0x01<<16, OUTPUT);
GPIO_SetValue(0, 0x01<<16);
GPIO_SetValue(0, 0x01<<21);
GPIO_SetDir(PORT_EN_0, 0x01<<EN_P1V2, OUTPUT);
GPIO_SetDir(PORT_EN_0, 0x01<<EN_P1V8, OUTPUT);
GPIO_SetDir(PORT_EN_0, 0x01<<EN_FMC2_P3V3, OUTPUT);
GPIO_SetDir(PORT_EN_0, 0x01<<EN_FMC1_P3V3, OUTPUT);
GPIO_SetDir(PORT_EN_0, 0x01<<EM_FMC1_P12V, OUTPUT);
GPIO_SetDir(PORT_EN_0, 0x01<<EN_FMC2_P12V, OUTPUT);
GPIO_SetDir(PORT_EN_1, 0x01<<EN_FMC1_PVADJ, OUTPUT);
GPIO_SetDir(PORT_EN_1, 0x01<<EN_FMC2_PVADJ, OUTPUT);
GPIO_SetDir(PORT_EN_1, 0x01<<EN_P3V3, OUTPUT);
GPIO_SetDir(PORT_EN_1, 0x01<<EN_1V5_VTT, OUTPUT);
GPIO_SetDir(PORT_EN_1, 0x01<<EN_RTM_CONN, OUTPUT);
GPIO_SetDir(PORT_EN_0, 0x01<<RTM_PRESENCE, INPUT);
GPIO_SetDir(PORT_EN_3, 0x01<<EN_P1V0, OUTPUT);
setDC_DC_ConvertersON();
spiInitialization.CPOL = SPI_CPOL_LO;
spiInitialization.CPHA = SPI_CPHA_FIRST;
spiInitialization.Databit = SPI_DATABIT_10;
spiInitialization.DataOrder = SPI_DATA_MSB_FIRST;
spiInitialization.ClockRate = 10000000;
SPI_Init(LPC_SPI, &spiInitialization);
LPC_SPI->SPCR = 0xA24;
GPIO_ClearValue(0, 0x01<<16);
SPI_SendData(LPC_SPI, 0x0125);
while(! ( SPI_CheckStatus( SPI_GetStatus(LPC_SPI), SPI_STAT_SPIF) ));
GPIO_SetValue(0, 0x01<<16);
GPIO_ClearValue(0, 0x01<<16);
SPI_SendData(LPC_SPI, 0x0025);
while(! ( SPI_CheckStatus( SPI_GetStatus(LPC_SPI), SPI_STAT_SPIF) ));
GPIO_SetValue(0, 0x01<<16);
//SCANSTA JTAG
GPIO_SetDir(2, 0x0FF, OUTPUT);
GPIO_SetValue(2, 0x080);
GPIO_ClearValue(2, 0x07F);
SPI_ConfigStructInit(&spiInitialization);
//FPGA_SPI
GPIO_SetDir(1, 0x01<<20, INPUT);
GPIO_SetDir(1, 0x01<<21, INPUT);
GPIO_SetDir(1, 0x01<<23, INPUT);
GPIO_SetDir(1, 0x01<<24, INPUT);
// SET PROGRAM_B AS OUTPUT
// AND SET THIS PIN HIGH
GPIO_SetDir(0, 0x01<<17, INPUT);
// SET DONE PIN AS INPUT
// turn on pull-up
GPIO_SetDir(0, 0x01<<22, INPUT);
////////////////////////////////////////
// P0_6 - FCS_B - use as FPGA - RST
////////////////////////////////////////
GPIO_SetDir(0, 0x01<<6, INPUT);
//GPIO_ClearValue(0, 0x01<<6);
///////////////////////////////////////
GPIO_SetDir(0, 0x01<<7, INPUT);
GPIO_SetDir(0, 0x01<<8, INPUT);
#ifdef AMC_CPU_COM_Express
GPIO_SetDir(0, 0x01<<9, OUTPUT);
#else
GPIO_SetDir(0, 0x01<<9, INPUT);
#endif
///////////////////////////////////////
//FMC STATUS PORTS
///////////////////////////////////////
GPIO_SetDir(1, 0x1<<14, INPUT);
GPIO_SetDir(1, 0x1<<15, INPUT);
GPIO_SetDir(1, 0x1<<16, INPUT);
GPIO_SetDir(1, 0x1<<17, INPUT);
GPIO_SetDir(1, 0x1<<18, INPUT);
GPIO_SetDir(1, 0x1<<19, INPUT);
///////////////////////////////////////
// FPGA Reset Button
EXTI_InitTypeDef EXTICfg;
/* Initialize EXT pin and register */
/* P2.12 as /EINT2*/
PinSelCfg.Funcnum = PINSEL_FUNC_1;
PinSelCfg.OpenDrain = PINSEL_PINMODE_NORMAL;
PinSelCfg.Pinmode = PINSEL_PINMODE_PULLUP;
PinSelCfg.Pinnum = FPGA_RST_SW;
PinSelCfg.Portnum = FPGA_RST_SW_PORT;
PINSEL_ConfigPin(&PinSelCfg);
PinSelCfg.Funcnum = PINSEL_FUNC_0;
PinSelCfg.OpenDrain = PINSEL_PINMODE_NORMAL;
PinSelCfg.Pinmode = PINSEL_PINMODE_PULLUP;
PinSelCfg.Pinnum = FPGA_RESETn;
PinSelCfg.Portnum = FPGA_RESETn_PORT;
PINSEL_ConfigPin(&PinSelCfg);
GPIO_SetDir(FPGA_RESETn_PORT, 1<<FPGA_RESETn, OUTPUT);
EXTI_Init();
EXTICfg.EXTI_Line = EXTI_EINT2;
EXTICfg.EXTI_Mode = EXTI_MODE_EDGE_SENSITIVE;
EXTICfg.EXTI_polarity = EXTI_POLARITY_LOW_ACTIVE_OR_FALLING_EDGE;
EXTI_ClearEXTIFlag(EXTI_EINT2);
EXTI_Config(&EXTICfg);
GPIO_IntCmd(0, 0x01 << FPGA_RST_SW, 1);
NVIC_SetPriorityGrouping(0);
NVIC_SetPriority(EINT2_IRQn, 4);
NVIC_EnableIRQ(EINT2_IRQn);
}
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
}
}
