/**
* @brief Configure hardware
* @param None
* @retval None
*/
void HW_Config(void)
{
/* GPIO clock enable */
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
GPIO_Config();
GPIOB->BSRRH = GPIO_Pin_14; // BT module in uart comm mode
GPIOB->BSRRH = GPIO_Pin_15; // BT module RESET
//ADC_Config();
PWM_TIM_Config();
//ENC_TIM_Config();
USART6_Config();
// LED TEST
GPIOA->BSRRH = GPIO_Pin_8; // RED LED LD4
GPIOA->BSRRH = GPIO_Pin_9; // GREEN LED LD6
GPIOA->BSRRH = GPIO_Pin_10; // GREEN LED LD5
Comm_Init();
Driver_Enable(1,1); // Enable motor drivers
GPIOB->BSRRL = GPIO_Pin_15; // BT module RUN
ADC_Config();
}
static int
Init(void *argsIn)
{
int rc = -1;
unsigned int maxChannels = 8;
/*
* On the host side, infinite timeout. 1 polling cycle.
* see kernel/time.c: msecs_to_jiffies()
*/
#if defined(COMM_BUILDING_SERVER)
unsigned int pollingMillis = (unsigned int)-1;
#else
unsigned int pollingMillis = 2000;
#endif
unsigned int pollingCycles = 1;
const char *args = argsIn;
if (args && *args) {
/* coverity[secure_coding] */
sscanf(args,
"max_channels:%u,poll_millis:%u,poll_cycles:%u",
&maxChannels, &pollingMillis, &pollingCycles);
CommOS_Debug(("%s: arguments [%s].\n", __FUNCTION__, args));
}
rc = Comm_Init(maxChannels);
if (rc) {
goto out;
}
rc = CommOS_StartIO(COMM_OS_MOD_SHORT_NAME_STRING "-disp",
Comm_DispatchAll, pollingMillis, pollingCycles,
COMM_OS_MOD_SHORT_NAME_STRING "-aio");
if (rc) {
unsigned long long timeout = 0;
Comm_Finish(&timeout); /* Nothing started, guaranteed to succeed. */
goto out;
}
running = 1;
rc = 0;
out:
return rc;
}
static int
Init(void *argsIn)
{
int rc = -1;
unsigned int maxChannels = 8;
#if defined(COMM_BUILDING_SERVER)
unsigned int pollingMillis = (unsigned int)-1;
#else
unsigned int pollingMillis = 2000;
#endif
unsigned int pollingCycles = 1;
const char *args = argsIn;
if (args && *args) {
sscanf(args,
"max_channels:%u,poll_millis:%u,poll_cycles:%u",
&maxChannels, &pollingMillis, &pollingCycles);
CommOS_Debug(("%s: arguments [%s].\n", __FUNCTION__, args));
}
rc = Comm_Init(maxChannels);
if (rc) {
goto out;
}
rc = CommOS_StartIO(COMM_OS_MOD_SHORT_NAME_STRING "-disp",
Comm_DispatchAll, pollingMillis, pollingCycles,
COMM_OS_MOD_SHORT_NAME_STRING "-aio");
if (rc) {
unsigned long long timeout = 0;
Comm_Finish(&timeout);
goto out;
}
running = 1;
rc = 0;
out:
return rc;
}
/*! \brief The main function.
*
* This contains initialization and the main loop as described in the
* application note. Interrupt service routines (ISR) supply all data to be
* processed in the main loop.
*
*/
int main( void )
{
/*
Initialisation of peripheral modules:
- Watchdog (timeout must be more than longer than the longest conversion period used for the CC-ADC)
- Turn of unneeded modules (only SPI atm)
- Bandgap
- Set Current protection levels/configuration
- V-ADC
- Get cell voltages (requires that interrupts are enabled)
- Get temperature
- Estimate capacity ("gas gauging")
- Check battery voltage and release the device from DUVR mode
- CC-ADC
- Get initial current reading (momentary and average).
- Communication
*/
// Initialization.
wdt_reset();
WDT_SetTimeOut( WDTO_2Kms ); //Set Watchdog timeout to 2 sec
#ifdef DEBUG
if( FAILURE == Reset_Initialization() ){
// TODO: The Watchdog has triggered several times - might want to shut down the battery?!
// POWMAN_Shutdown();
}
#endif
// Enable power reduction for SPI
PRR_DISABLE_SPI();
// Disable digital input for port a
DIDR0 = (1<<PA0DID) | (1<<PA1DID);
// Enable pull-ups for port b to avoid floating pins
PORTB = (1<<PORTB0) | (1<<PORTB1) | (1<<PORTB2) | (1<<PORTB3) | (1<<PORTB4) | (1<<PORTB5) | (1<<PORTB6) | (1<<PORTB7);
DDRB = (1<<PORTB0) | (1<<PORTB1) | (1<<PORTB2) | (1<<PORTB3) | (1<<PORTB4);
// Check the CRC checksum of the battery parameters struct
errorFlags.checksumFailure = (! BATTPARAM_CheckCRC()) | (! CheckSignatureCRC());
// Initialize Bandgap reference.
if( FAILURE == BANDGAP_Initialization() ){
errorFlags.criticalConditionDetected = true;
}
// Initialize the battery protection module (enabled by default - if default settings are good
// this is not required).
BATTPROT_SetShortCircuitDetection( battParams.shortcircuitCurrent, battParams.shortcircuitReactionTime );
BATTPROT_SetOverCurrentDetection( battParams.overcurrentDischarge, battParams.overcurrentCharge, battParams.overcurrentReactionTime );
BATTPROT_SetHighCurrentDetection( battParams.highcurrentDischarge, battParams.highcurrentCharge, battParams.highcurrentReactionTime );
BATTPROT_DisableDischargeHighCurrentProtection(); // Don't want a high discharging current to disable the FETs (in this example)
BATTPROT_EnableAllInterrupts();
//Enable DUVR from the start, so we are always in DUVR mode.
FETCTRL_EnableDeepUnderVoltageMode();
// On reset, the device is in DUVR mode depending on the fuse setting DUVRINIT
stateFlags.inDUVR = true;
// Enable the voltage regulator monitor interrupt
VREGMON_InterruptEnable();
// Initialize V-ADC and configure a full scan
VADC_InitializeVadcCoefficients();
VADC_ScanConfig( (vadcIndex_t)HIGHEST_CELL, (vadcIndex_t)HIGHEST_VADC, (vadcIndex_t)VTEMP );
// This can only fail if the VADC is set-up with an invalid ScanConfig
if( FAILURE == VADC_StartScan() ){
errorFlags.criticalConditionDetected = true;
} else {
__enable_interrupt();
while( VADC_RUNNING == VADC_ScanState() );
__disable_interrupt();
VADC_ClearReadyFlags();
disableDUVRIfPossible();
VBSoC_Init();
}
// Calculate the coefficient numbers for ccadc_ticks to mA and gas_gauging to mAh functions
BATTCUR_CalculateShuntCoeffient(battParams.shuntResistance, CCADC_VOLTREF);
uint16_t temp = RCCAL_CalculateUlpRCperiod(coreTemperature/10);
CCGASG_CalculateShuntCoefficients(temp, battParams.shuntResistance, CCADC_VOLTREF);
// Calculate values for the sram battery parameters struct
BATTPARAM_InitSramParameters();
// Copy key from eeprom to sram if using AES
#if defined(AUTH_USE_AES)
AUTH_CopyKeyToSram();
#endif
// If a critical condition occured, (either bandgap didn't work or VADC was configured wrong)
// don't do any of the battery related initialization and disable DUVR mode (so charging is impossible)
if( errorFlags.criticalConditionDetected ) {
FETCTRL_DisableDeepUnderVoltageMode();
FETCTRL_DisableFets();
} else {
// Initialize the CC-ADC (sample every second and set regular current level)
CCADC_Init( ACCT_1024, RCCI_1024, 10 mA );
CCADC_SetMode( CCADC_ACC );
// Set initial remaining capacity in the CCGASG module, it is marked as in-
// accurate and updated the first time the VBSoC have an accurate reading.
CCGASG_SetStateofCharge(VBSoC_EstimatedSoC());
stateFlags.remainingCapacityInaccurate = 1;
__enable_interrupt();
// Needs to run a conversion before the value is correct
while( !CCADC_isAccResultReady() );
wdt_reset();
__disable_interrupt();
// Calculate the CCADC Accumulating conversion offset if it is invalid and
// DUVR mode is disabled
if(CCADC_GetRawAccOffset() == 32767 && ! stateFlags.inDUVR) {
FETCTRL_DisableFets(); // Disable FETs when measuring offset as changes in current not is good
__enable_interrupt();
CCADC_CalculateAccOffset();
__disable_interrupt();
}
// If two or more cells, init the misbalance corrector
#if BATTPARAM_CELLS_IN_SERIES >= 2
BATTVMON_InitializeBalancing();
#endif
//Store information about the current (to be able to reply if host asks...)
BATTCUR_StoreCurrent( CCADC_GetAccResult() );
BATTCUR_InitializeAverageCurrent( BATTCUR_GetOffsetCalibratedCurrent() );
}
//init communication bus
T1init(); // Used for SmBus timeout and LED
Comm_Init();
// Ready to run - enable interrupts and enter main loop.
__enable_interrupt();
/*******************************************************************
Main loop:
If any of the task flags are set the corresponding task should be run.
The following things are done:
- Reset watchdog
- If a new CCADC result is ready, start a VADC conversion
- If the communication interface have a new byte, handle it
- and if a whole new command has arrived, handle that
- Handle new CCADC result if it's ready
- Check new core temperature if ready, and run a fast RC calibration if needed
- Check new cell temperature if ready
- Check new cell voltage if ready
- Set different sleep modes depending on what's running
- If in communication or RC calibration (any timer running) can only enter idle mode
- If VADC is running, can't go deeper than ADC noise reduction mode
- If "nothing" is running, enter power-save
- Disable/enable fets depeding on various things
- Go to sleep
*/
while(1) {
//Tickle the watchdog every cycle
wdt_reset();
if( CCADC_isAccResultReady() )
{
// Start a new VADC scan as fast as possible so that all scans hopefully are
// ready before the main loop ends, to avoid having to cycle again.
#if BATTPARAM_CELLS_IN_SERIES >= 2
// Disable cell balancing if more than one cell
BATTVMON_DisableCellBalancing();
#endif
// Start a new VADC scan
configureVADC();
VADC_StartScan();
taskFlags.newCurrentAvailable = true;
}
//See if there were any received commands.
taskFlags.newSbsCommandAvailable = Comm_Handle( &sbs );
//If complete sbs command has been received: process it
if( taskFlags.newSbsCommandAvailable )
{
handleSBSCommand();
// If authentication is needed, run. It will use and change the values in sbs,
// make sure the communication interrupt does not write directly to it.
// Note that this can take long time and communication will not work
// during that time.
if(taskFlags.doAuthentication) {
AUTH_Execute(&sbs);
taskFlags.doAuthentication = false;
}
}
/*
A new CC-ADC conversion is available. Update momentary current, average current,
and calculate new battery capacity. Update discharge_cycle_accumulator (and increment
cycle_count if required).
*/
if( taskFlags.newCurrentAvailable )
{
taskFlags.newCurrentAvailable = false;
// We use the CCADC interrupt as counter for the RTC
runEverySecond();
BATTCUR_StoreCurrent( CCADC_GetAccResult() );
handleNewCCADCResult();
CCGASG_AccumulateCCADCMeasurements( BATTCUR_GetCurrent(), slowRcOscillatorPeriod );
//Set ledstatus
SoC_State = GASG_StateOfCharge(BATTCUR_GetAverageCurrent(), battParams_sram.capacityInCCAccumulated);
SetLEDs((uint8_t)SoC_State);
}
/*
New core temperature reading is available and should be processed. If
temperature has changed set RC_calibration_required flag.
*/
if( VADC_VTempReady() )
{
handleNewCoreTemperature();
}
/*
RC oscillator requires calibration (new CC_ADC conversion period time should
be calculated and stored.
*/
if( taskFlags.rcCalibrationRequired )
{
taskFlags.rcCalibrationRequired = false;
RCCAL_StartCalibrateFastRC();
// Calculating slow RC period works on whole kelvins
slowRcOscillatorPeriod = RCCAL_CalculateSlowRCperiod( coreTemperature/10 );
}
if( RCCAL_GetState() == RCCAL_CALIB_INIT || RCCAL_GetState() == RCCAL_CALIB_WORKING ) {
RCCAL_CalibrateFastRCruntime(slowRcOscillatorPeriod);
}
/*
Battery cell temperature is available. Check it and disable FETs if too high/low
*/
if( VADC_CellTempReady() )
{
handleNewCellTemperature();
}
/*
Check battery voltage cell . Check if critical (high/low voltage)
*/
if( VADC_Cell1VoltageReady() )
{
handleNewCellVoltage(CELL1);
}
/*
Check battery voltage cell . Check if critical (high/low voltage) and misbalance
*/
#if BATTPARAM_CELLS_IN_SERIES >= 2
if( VADC_Cell2VoltageReady() )
{
handleNewCellVoltage(CELL2);
}
#if BATTPARAM_CELLS_IN_SERIES >= 3
if( VADC_Cell3VoltageReady() )
{
handleNewCellVoltage(CELL3);
}
#if BATTPARAM_CELLS_IN_SERIES >= 4
if( VADC_Cell4VoltageReady() )
{
handleNewCellVoltage(CELL4);
}
#endif
#endif
#endif
// Check what sleep mode we can enter. Needs disable interrupt because otherwise it might
// overwrite what interrupts write to SMCR (for example, if external interrupt is triggered and
// wants the sleep mode to be no higher than idle, it should be run after all those checks)
uint8_t interruptState = __save_interrupt();
__disable_interrupt();
if( Comm_IsIdle() && ! RCCAL_IS_RUNNING() ) {
if( VADC_RUNNING == VADC_ScanState() ) {
SLEEP_SET_ADC_NR_MODE();
} else {
SLEEP_SET_POWER_SAVE_MODE();
}
} else {
SLEEP_SET_IDLE_MODE();
}
__restore_interrupt(interruptState);
/*
Disable FETs if critical condition have been detected.
Enable them if that is okey, depending om various factors like
temperature and voltage.
*/
if( errorFlags.criticalConditionDetected
|| stateFlags.simulatePowerOff)
{
FETCTRL_DisableFets();
}
else if( ! errorFlags.cellTemperatureTooHigh &&
! errorFlags.cellTemperatureTooLow &&
! stateFlags.inDUVR)
{
if( ! errorFlags.voltageTooHigh
&& ! errorFlags.reoccuringChargeProtection
&& ! stateFlags.chargingProhibited
&& ! sbsFlags.forceChargeFETDisabled)
{
FETCTRL_EnableChargeFet();
}
if( ! errorFlags.voltageTooLow
&& ! stateFlags.dischargingProhibited
&& ! sbsFlags.forceDischargeFETDisabled)
{
FETCTRL_EnableDischargeFet();
}
}
// The reason to do this is that on short-circuit, the cell voltage reading can
// be wrong and if that causes both chargingProhibited and dischargingProhibited
// to get set, they would never be cleared unless we do this.
// If they should be set, they will be set again before the FETs are enabled again
// on next cycle.
if( stateFlags.chargingProhibited && stateFlags.dischargingProhibited ) {
stateFlags.chargingProhibited = false;
stateFlags.dischargingProhibited = false;
}
// Enter sleep if no new byte just have been received on the communication
// If a byte is received between the check for Comm_Flag and __sleep is
// executed, the sleep function will have no effect as the software communication
// interrupt clears sleep mode enable.
if( (Comm_Flag() == 0 ) && (ShowLedBusy()==false) ){
__sleep();
}
}
}
void setup()
{
uint8_t i=0;
//初始化系统时钟
setupSystemClock();
//初始化串口
setupUART();
UART_NVIC_INIT();
//初始化System_tick
setup_system_tick(SYSTEM_TICK_FREQ);
//初始化IIC
I2C_Init();
//初始化FLASH
FlashInit();
LoadParamsFromFlash();
//初始化低电压检测
BatteryCheckInit();
//初始化遥控
Comm_Init();
//初始化LED
LED_Init();
//初始化SENSOR
#ifdef IMU_SW //软件姿态解算
// MPU6050_initialize();
// DelayMsec(1000); //必须加延迟，否则读取陀螺仪数据出错
#else
MPU6050_initialize();
DelayMsec(1000); //必须加延迟，否则读取陀螺仪数据出错
MPU6050_DMP_Initialize(); //初始化DMP引擎
#endif
//初始化自稳定
// LED_ON();
// //测试用，延迟启动时间
// for(i=0;i<6;i++)
// {
// DelayMsec(1000);
// LED_OFF();
// }
//初始化电机
Motor_Init();
//printf("Motor_Init(); \n");
//IMU_Init(); // sample rate and cutoff freq. sample rate is too low now due to using dmp.
// printf("\n\nCPU @ %dHz\n", SystemCoreClock);
//MotorPwmOutput(100,100,0,0);
}
/*------------------------------------------------------------------------------
FUNCTION NAME: Main_Init
DESCRIPTION: Main init function
PARAMETERS:
- INPUT:
- OUTPUT:
RETURN:
NOTES: Don't try to change the order of the function calls!
------------------------------------------------------------------------------*/
void Main_Init (void)
{
t_uchar Lines[2] = {9,END_OF_LINES};
char Msg[40];
#ifdef PC_TERMINAL
int RetVal;
t_uchar PrinterIndex, NumPrinters, Type, Port, Model;
t_boolean InvertScreen;
int LCDContrastOffset;
#endif
/* MAEL Version */
#if defined (FEDORA_VERSION)
srand48((long) time(NULL));
#else
#if !defined (NT_VERSION)
/* Standart Random generation init */
randomize ();
#else
/* Seed the random-number generator with current time so that
* the numbers will be different every time we run.
*/
srand( (unsigned)time( NULL ) );
#endif
#endif /* MAEL Version */
/*---------------------------------------------------*/
/* Set dos verify option to on and other DOS options */
/*---------------------------------------------------*/
#if !defined (NT_VERSION)
#if !defined (FEDORA_VERSION)
/* In Linux Verify is always on */
/* And cbreak does nothing */
setverify (1);
setcbrk (0);
#endif /* FEDORA_VERSION */
/* Set new Ctrl-Break handler */
#if defined (DOSX286)
DosSetPassToProtVec (0x1B, (PIHANDLER) Main_HandlerCtrlBreak,
&OldCtrlBrkProt, &OldCtrlBrkReal);
DosSetPassToProtVec (0x23, (PIHANDLER) Main_HandlerCtrlBreak,
&OldCtrlCProt, &OldCtrlCReal);
#else
#if defined (FEDORA_VERSION)
signal(SIGINT,SIG_IGN);
#else
ctrlbrk (Main_HandlerCtrlBreak);
#endif /* MAEL VERSION */
#endif
#endif
/*-------------*/
/* Nosdos Init */
/*-------------*/
/*----------*/
/* Init. Kb */
/*----------*/
#if defined (FEDORA_VERSION)
Kb_Init (KEYBOARD_STANDART);
#else
#if defined (TERMINAL_WT9602)
Kb_Init (KEYBOARD_WT9602);
#else
#if defined (TERMINAL_WT9603)
Kb_Init (KEYBOARD_WT9603);
#else
Kb_Init (KEYBOARD_STANDART);
#endif
#endif
#endif /* MAEL Version */
/*-----------------------*/
/* Init. display modules */
/*-----------------------*/
#if defined(FEDORA_VERSION)
#if defined(SVGALIB_MODE) || defined(XLIB_MODE)
GrphDisp_Init (240, 128, 8);
#else
GrphDisp_Init (40, 14, 8);
#endif
#else
#if defined(NT_VERSION)
GrphDisp_Init (40, 14, 8);
#else
GrphDisp_Init (240, 128, 8);
#endif
#endif /* MAEL Version */
GrphText_Init (240, 128, 8);
#if defined (TERMINAL_WT9602) || defined (TERMINAL_WT9603)
#if defined (LNSE)
GrphDisp_SetInvertScreen (FALSE);
#else
GrphDisp_SetInvertScreen (TRUE);
#endif
Util_SetLCDContrast (INITIAL_LCD_CONTRAST);
#endif
/* Init message */
GrphText_PopUp (54, 0, 240, Lines, TRUE);
strcpy (Msg, S0252);
GrphText_Cls (TRUE);
GrphText_Write (1, (t_uchar) ((40-strlen (Msg))/2*6), Msg, FONT_7X5, (t_uchar) strlen (Msg),
TRUE, FALSE);
/*-------------------------------------------------------*/
/* Ramdisk init (includes error handler for disk access) */
/*-------------------------------------------------------*/
RamDisk_Init ();
/* Init. Error logger */
Errlog_Init ();
/*------------*/
/* Init. UART */
/*------------*/
Uart_Init ();
/*----------------------*/
/* Init. Parallel ports */
/*----------------------*/
Parallel_Init ();
/*-----------------*/
/* End nosdos Init */
/*-----------------*/
/*-----------*/
/* App. Init */
/*-----------*/
/* Database Init */
DB_InitStaticData ();
RetVal = DB_LoadDBGlobal ();
if (RetVal != DB_OK)
{ /* ??? Disk error: ends application ??? */
} /* endif */
DB_InitDynamicData ();
/* Communication Init */
Comm_Init ();
#if defined (FEDORA_VERSION)
/* Ping IP address to initialize vpn */
TCPIP_Init_Tunnel();
/* Get and Apply last contrast value configured */
Display_GetAndApplyContrast();
#endif
/* Comm_Init for backup line if it is configured */
{
t_uchar CurrentLineType, OperationMode;
DB_OperationMode (&OperationMode);
if (OperationMode == OPERATIONMODE_REALONLINEBACKUP)
{
DB_CurrentLineType (&CurrentLineType);
/* Change current line type to backup line */
DB_GLOBAL_DYNAMIC.DB_CURRENT_LINETYPE = LINETYPE_BACKUP;
/* Init backup line */
Comm_PhysInitComm ();
/* Restore main line type */
DB_GLOBAL_DYNAMIC.DB_CURRENT_LINETYPE = CurrentLineType;
} /* endif */
}
/* Comm_Init for main line */
Comm_PhysInitComm ();
Comm_CommDeviceInit ();
/* App. protocol */
Prot_Init ();
#if defined(ENABLE_HOTLINE)
/* Init. Answer logger of hotline */
Hotline_Init ();
#endif
/* Devices init */
Devices_Init ();
/* Enable OMR device */
Devices_SetState (DEVCODE_OMR, DEVSTATE_ENABLED);
/*----------------------*/
/* Init. printer module */
/*----------------------*/
DB_NumPrinters (&NumPrinters);
for (PrinterIndex = 0; PrinterIndex < NumPrinters; PrinterIndex++)
{
DB_PrinterInfo (PrinterIndex, &Type, &Port, &Model);
Printer_Init (PrinterIndex, Type, Port, Model, TRUE);
} /* endfor */
/* Invert screen option */
DB_GetInvertScreen (&InvertScreen);
GrphDisp_SetInvertScreen (InvertScreen);
/* LCD Contrast */
DB_GetLCDContrastOffset (&LCDContrastOffset);
Util_SetLCDContrast (LCDContrastOffset);
/*----------------------*/
/* App. crc error check */
/*----------------------*/
DB_ChkAppCrcError ();
/*---------------*/
/* End App. Init */
/*---------------*/
/* Clear message from virtual display */
GrphText_Cls (FALSE);
GrphText_PopDown (FALSE);
} /* Main_Init */
/*********************************************************************//*!
* @brief Initialize everything so the application is fully operable
* after a call to this function.
*
* @return SUCCESS or an appropriate error code.
*//*********************************************************************/
static OSC_ERR init(const int argc, const char * argv[])
{
OSC_ERR err = SUCCESS;
uint8 multiBufferIds[2] = {0, 1};
char strVersion[15];
struct CFG_KEY configKey;
struct CFG_VAL_STR strCfg;
#ifdef HAS_CPLD
uint16 exposureDelay;
#endif /* HAS_CPLD */
memset(&data, 0, sizeof(struct DATA));
/* Print software version */
GetVersionString( strVersion);
fprintf(stderr, "Software rich-view version: %s\n", strVersion);
/******* Create the framework **********/
err = OscCreate(&data.hFramework);
if (err < 0)
{
fprintf(stderr, "%s: Unable to create framework.\n", __func__);
return err;
}
/******* Load the framework module dependencies. **********/
err = OscLoadDependencies(data.hFramework, deps, sizeof(deps)/sizeof(struct OSC_DEPENDENCY));
if (err != SUCCESS)
{
fprintf(stderr, "%s: ERROR: Unable to load dependencies! (%d)\n", __func__, err);
goto dep_err;
}
/* Set logging levels */
OscLogSetConsoleLogLevel(INFO);
OscLogSetFileLogLevel(DEBUG);
/* Print framework version */
OscGetVersionString( strVersion);
OscLog(INFO, "Oscar framework version: %s\n", strVersion);
/* Disable watchdog (probably activated from previous application) */
OscSupWdtInit();
OscSupWdtClose();
/* Set LED to green, util the idle state is entered */
OscGpioSetTestLed( TRUE);
OscGpioSetTestLedColor(FALSE, TRUE); /* R, G*/
/* Register configuration file */
err = OscCfgRegisterFile(&data.hConfig, CONFIG_FILE_NAME, CONFIG_FILE_SIZE);
if (err != SUCCESS)
{
OscLog(ERROR, "Cannot access config file.\n");
goto cfg_err;
}
/* Get perspective setting from config file- */
configKey.strSection = NULL;
configKey.strTag = "PER";
strcpy( strCfg.str, "");
err = OscCfgGetStr( data.hConfig,
&configKey,
&strCfg);
err |= OscCamPerspectiveStr2Enum( strCfg.str, &data.perspective);
if( err != SUCCESS)
{
OscLog(WARN,
"%s: No (valid) camera-scene perspective configured (%s). "
"Use default (%s).\n",
__func__, strCfg.str, OSC_CAM_PERSPECTIVE_DEFAULT);
data.perspective = OSC_CAM_PERSPECTIVE_DEFAULT;
}
/* Get exposure time setting from configuration. */
configKey.strSection = NULL;
configKey.strTag = "EXP";
err = OscCfgGetUInt32( data.hConfig,
&configKey,
&data.exposureTime);
if( err != SUCCESS)
{
OscLog(WARN,
"%s: No (valid) Exposure Time defined in configuration (%d). "
"Use default (%d).\n",
__func__, data.exposureTime, DEFAULT_EXPOSURE_TIME);
data.exposureTime = DEFAULT_EXPOSURE_TIME;
}
#ifdef HAS_CPLD
/* Get exposure delay setting from configuration. */
configKey.strSection = NULL;
configKey.strTag = "DEL";
err = OscCfgGetUInt16Range( data.hConfig,
&configKey,
&exposureDelay,
0,
FINECLK2CLK_RATIO-1);
data.exposureDelay = exposureDelay & 0x00ff;
if( err != SUCCESS)
{
OscLog(WARN,
"%s: No (valid) Exposure Delay defined in configuration (%d). "
"Use default (%d).\n",
__func__, data.exposureDelay, DEFAULT_EXPOSURE_DELAY);
data.exposureDelay = DEFAULT_EXPOSURE_DELAY;
}
#endif /* HAS_CPLD */
#ifdef HAS_CPLD
/* Get firmware version */
err = OscCpldRget(OSC_LGX_FWREV, &data.firmwareRevision);
if(err != SUCCESS)
{
OscLog(ERROR, "Cannot read firmware version. (%d)\n", err);
goto cpld_err;
}
/* Apply exposure delay to CPLD and disable. */
err = OscCpldRset(OSC_LGX_CLKDELAY,
(const uint8)(data.exposureDelay & !OSC_LGX_CLKDELAY_ENABLE));
if(err != SUCCESS)
{
OscLog(ERROR, "Cannot disable clock-delay in CPLD.\n");
goto cpld_err;
}
/* Set CPLD to synchronous mode. */
err = OscCpldFset(OSC_LGX_VARCTRL, OSC_LGX_VARCTRL_SYNCOUT, OSC_LGX_VARCTRL_SYNCOUT);
if(err != SUCCESS)
{
OscLog(ERROR, "Cannot set CPLD to synchronous mode.\n");
goto cpld_err;
}
#endif /* HAS_CPLD */
/* Set the camera registers to sane default values. */
err = OscCamPresetRegs();
if (err != SUCCESS)
{
OscLog(ERROR, "%s: Unable to preset camera registers! (%d)\n", __func__, err);
goto cam_err;
}
/* Set up two frame buffers with enough space for the maximum
* camera resolution in cached memory. */
err = OscCamSetFrameBuffer(0, IMAGE_AERA, data.u8FrameBuffers[0], TRUE);
if (err != SUCCESS)
{
OscLog(ERROR, "%s: Unable to set up first frame buffer!\n", __func__);
goto cam_err;
}
err = OscCamSetFrameBuffer(1, IMAGE_AERA, data.u8FrameBuffers[1], TRUE);
if (err != SUCCESS)
{
OscLog(ERROR, "%s: Unable to set up second frame buffer!\n", __func__);
goto cam_err;
}
/* Create a double-buffer from the frame buffers initilalized above.*/
err = OscCamCreateMultiBuffer(2, multiBufferIds);
if (err != SUCCESS)
{
OscLog(ERROR, "%s: Unable to set up multi buffer!\n", __func__);
goto mb_err;
}
OscCamSetupPerspective( data.perspective);
/* Make the register file known to the communication protocol. */
data.comm.pRegFile = regfile;
data.comm.nRegs = (sizeof(regfile)/sizeof(struct CBP_PARAM));
/* Init communication sockets. */
err = Comm_Init(&data.comm);
if (err != SUCCESS)
{
OscLog(ERROR, "Communication initialization failed.\n");
goto comm_err;
}
return SUCCESS;
comm_err:
cfg_err:
#ifdef HAS_CPLD
cpld_err:
#endif /* HAS_CPLD */
mb_err:
cam_err:
OscUnloadDependencies(data.hFramework,
deps,
sizeof(deps)/sizeof(struct OSC_DEPENDENCY));
dep_err:
OscDestroy(&data.hFramework);
return err;
}
void test_Comm()
{
int i;
InputMsg msg;
char c;
msg.cmdType = 1;
msg.leftPwm = 10.0; msg.rightPwm = -1337;
msg.checksum = 0;
printf("Sizeof InputMsg %d\n", sizeof(InputMsg));
char* msgStr = msgAsStr(msg);
printf("Len of msgStr : %d\n", strlen(msgStr));
char* msgBuf = malloc(strlen(msgStr) + 3);
msgBuf[0] = 0x02; msgBuf[strlen(msgStr)+1] = '\n'; msgBuf[strlen(msgStr) + 2] = 0;
memcpy(msgBuf + 1, msgStr, strlen(msgStr));
printf("%s", msgBuf);
Comm_Init();
char lotsOfTrash[] = "lkjhdflkjaslfhsjfhslkdhfhsalkfhsdjfhlkshfhflkhsdflkhsadlkfhsalkjfhsalkdhflkjsahflkjshflkjlkjhlkjhsflkjdsfkjsfkjhsjfkashf";
/*
* First lets test if still works after overflow of trash.
*/
Comm_Process(lotsOfTrash, strlen(lotsOfTrash));
/*
* Let's send just perfect message and see if it works.
*/
Comm_Process(msgBuf, strlen(msgStr) + 2);
InputMsg recvMsg;
recvMsg.leftPwm = 0; recvMsg.rightPwm = 0;
if(Comm_NewMsg() == 1)
printf("New message detected!\n");
else
printf("!!! New message NOT detected!\n");
recvMsg = Comm_LastMsg();
if(memcmp(&msg, &recvMsg, sizeof(InputMsg)) == 0)
printf("Messages are equal!\n");
else
printf("!!! Messages are NOT equal!\n");
if(Comm_NewMsg() == 1)
printf("!!! New message detected AGAIN!\n");
/*
* Repeating message. Still works?
*/
printf(" ---- TEST 2 ----\n");
Comm_Process(msgBuf, strlen(msgStr) + 2);
recvMsg.leftPwm = 0; recvMsg.rightPwm = 0;
if(Comm_NewMsg() == 1)
printf("New message detected!\n");
else
printf("!!! New message NOT detected!\n");
recvMsg = Comm_LastMsg();
if(memcmp(&msg, &recvMsg, sizeof(InputMsg)) == 0)
printf("Messages are equal!\n");
else
printf("!!! Messages are NOT equal!\n");
if(Comm_NewMsg() == 1)
printf("!!! New message detected AGAIN!\n");
/*
* First sending trash, then correct message. Then trash again.
*/
printf(" ---- TEST 3 ----\n");
char trash[] = "kdfjsfsjh6576576567";
Comm_Process(trash, strlen(trash));
Comm_Process(msgBuf, strlen(msgStr) + 2);
Comm_Process(trash, strlen(trash));
recvMsg.leftPwm = 0; recvMsg.rightPwm = 0;
if(Comm_NewMsg() == 1)
printf("New message detected!\n");
else
printf("!!! New message NOT detected!\n");
recvMsg = Comm_LastMsg();
if(memcmp(&msg, &recvMsg, sizeof(InputMsg)) == 0)
printf("Messages are equal!\n");
else
printf("!!! Messages are NOT equal!\n");
if(Comm_NewMsg() == 1)
printf("!!! New message detected AGAIN!\n");
/*
* Sending message piece-by-piece. Last part with trash.
*/
printf(" ---- TEST 4 ----\n");
Comm_Process(msgBuf, 8);
char* lastPieceWithTrash = malloc( strlen(msgStr) + 2 - 8 + strlen(trash));
memcpy(lastPieceWithTrash, msgBuf + 8, strlen(msgStr) + 2 - 8);
memcpy(lastPieceWithTrash + strlen(msgStr) + 2 - 8, trash, strlen(trash));
Comm_Process(lastPieceWithTrash, strlen(trash));
recvMsg.leftPwm = 0; recvMsg.rightPwm = 0;
if(Comm_NewMsg() == 1)
printf("New message detected!\n");
else
printf("!!! New message NOT detected!\n");
recvMsg = Comm_LastMsg();
if(memcmp(&msg, &recvMsg, sizeof(InputMsg)) == 0)
printf("Messages are equal!\n");
else
printf("!!! Messages are NOT equal!\n");
if(Comm_NewMsg() == 1)
printf("!!! New message detected AGAIN!\n");
/*
* Sending half of correct message, trash, then other half.
*/
printf(" ---- TEST 5 ----\n");
Comm_Process(msgBuf, 5);
Comm_Process(trash, strlen(trash));
Comm_Process(msgBuf + 5, strlen(msgStr) + 2 - 5);
recvMsg.leftPwm = 0; recvMsg.rightPwm = 0;
if(Comm_NewMsg() == 0)
printf("New message not detected!\n");
else
printf("!!! New message detected!\n");
/*
* Repeating message. Still works?
*/
printf(" ---- TEST 6 ----\n");
Comm_Process(msgBuf, strlen(msgStr) + 2);
recvMsg.leftPwm = 0; recvMsg.rightPwm = 0;
if(Comm_NewMsg() == 1)
printf("New message detected!\n");
else
printf("!!! New message NOT detected!\n");
recvMsg = Comm_LastMsg();
if(memcmp(&msg, &recvMsg, sizeof(InputMsg)) == 0)
printf("Messages are equal!\n");
else
printf("!!! Messages are NOT equal!\n");
if(Comm_NewMsg() == 1)
printf("!!! New message detected AGAIN!\n");
/*
* Send message char-by-char
*/
printf(" ---- TEST 7 ----\n");
for(i = 0 ; i < strlen(msgStr) + 2 ; i++)
{
c = msgBuf[i];
Comm_Process(&c, 1);
}
recvMsg.leftPwm = 0; recvMsg.rightPwm = 0;
if(Comm_NewMsg() == 1)
printf("New message detected!\n");
else
printf("!!! New message NOT detected!\n");
recvMsg = Comm_LastMsg();
if(memcmp(&msg, &recvMsg, sizeof(InputMsg)) == 0)
printf("Messages are equal!\n");
else
printf("!!! Messages are NOT equal!\n");
if(Comm_NewMsg() == 1)
printf("!!! New message detected AGAIN!\n");
char* recvMsgStr = msgAsStr(recvMsg);
printf("%s\n", recvMsgStr);
}
