static int SetIAPBoot(void)
{
uint32_t au32Config[2];
uint32_t u32CBS;
/* Read current boot mode */
u32CBS = (FMC->ISPSTA & FMC_ISPSTA_CBS_Msk) >> FMC_ISPSTA_CBS_Pos;
if(u32CBS & 1)
{
/* Modify User Configuration when it is not in IAP mode */
FMC_ReadConfig(au32Config, 2);
if(au32Config[0] & 0x40)
{
FMC_EnableConfigUpdate();
au32Config[0] &= ~0x40;
FMC_Erase(FMC_CONFIG_BASE);
FMC_WriteConfig(au32Config, 2);
// Perform chip reset to make new User Config take effect
SYS_ResetChip();
}
}
return 0;
}
//----- (00002C38) --------------------------------------------------------
__myevic__ uint32_t hidResetSysCmd( CMD_T *pCmd )
{
myprintf("Reset system command\n");
SYS_UnlockReg();
SYS_ResetChip();
while ( 1 )
;
}
__myevic__ void Plantouille( int xpsr, int* stack )
{
int i, k;
k = 0;
SYS_UnlockReg();
WDT_Close();
SYS_LockReg();
InitDisplay();
while ( 1 )
{
ClearScreenBuffer();
DrawImage( 0, 0, 'X'+0x27 );
DrawHexLong( 16, 0, xpsr, 0 );
DrawHexDigit( 0, 16, k );
for ( i = 0; i < 14 ; ++i )
{
DrawHexLong( 16, 16+i*8, stack[i+k*14], 0 );
}
DisplayRefresh();
while ( !PE0 || !PD2 || !PD3 )
CLK_SysTickDelay( 10000 );
while ( PE0 && PD2 && PD3 )
CLK_SysTickDelay( 10000 );
if ( !PE0 )
{
SYS_UnlockReg();
SYS_ResetChip();
while ( 1 )
;
}
if ( !PD2 ) ++k;
if ( !PD3 ) --k;
if ( k < 0 ) k = 0;
else if ( k > 15 ) k = 15;
}
}
/* Sensors Init */
void SensorInitACC()
{
float Cal[ACC_CAL_DATA_SIZE];
bool FlashValid;
#if defined(LSM6DS3)
status_t status;
#endif
if(!SensorInitState.ACC_Done) {
#if defined(MPU6050) || defined(MPU6500)
SensorInitState.ACC_Done = MPU6050_initialize();
SensorInitState.GYRO_Done = SensorInitState.ACC_Done;
#else
LSM6DS3_init();
status = begin();
if(status==0)
SensorInitState.ACC_Done = true;
else
SensorInitState.ACC_Done = false;
SensorInitState.GYRO_Done = SensorInitState.ACC_Done;
#endif
}
if(SensorInitState.ACC_Done) {
printf("ACC connect - [OK]\n");
FlashValid = GetFlashCal(SENSOR_ACC, Cal);
if(FlashValid) {
CalFlashState.ACC_FLASH = true;
AccOffset[0] = Cal[0];
AccOffset[1] = Cal[1];
AccOffset[2] = Cal[2];
AccScale[0] = Cal[3];
AccScale[1] = Cal[4];
AccScale[2] = Cal[5];
AccRotate[0] = Cal[6];
AccRotate[1] = Cal[7];
AccRotate[2] = Cal[9];
AccRotate[3] = Cal[9];
AccRotate[4] = Cal[10];
AccRotate[5] = Cal[11];
AccRotate[6] = Cal[12];
AccRotate[7] = Cal[13];
AccRotate[8] = Cal[14];
printf("ACC calibration from - [FLASH]\n");
}
else {
AccOffset[0] = 0;
AccOffset[1] = 0;
AccOffset[2] = 0;
AccScale[0] = IMU_G_PER_LSB_CFG;
AccScale[1] = IMU_G_PER_LSB_CFG;
AccScale[2] = IMU_G_PER_LSB_CFG;
AccRotate[0] = 1;
AccRotate[1] = 0;
AccRotate[2] = 0;
AccRotate[3] = 0;
AccRotate[4] = 1;
AccRotate[5] = 0;
AccRotate[6] = 0;
AccRotate[7] = 0;
AccRotate[8] = 1;
printf("ACC calibration from - [DEFAULT]\n");
}
printf("Offset: %f %f %f\n", AccOffset[0], AccOffset[1], AccOffset[2]);
printf("Scale: %f %f %f\n", AccScale[0], AccScale[1], AccScale[2]);
printf("M[0][1][2]: %f %f %f\n", AccRotate[0], AccRotate[1], AccRotate[2]);
printf("M[3][4][5]: %f %f %f\n", AccRotate[3], AccRotate[4], AccRotate[5]);
printf("M[6][7][8]: %f %f %f\n", AccRotate[6], AccRotate[7], AccRotate[8]);
nvtSetAccScale(AccScale);
nvtSetAccOffset(AccOffset);
nvtSetAccRotate(AccRotate);
#if defined(MPU6050) || defined(MPU6500)
nvtSetAccG_PER_LSB(IMU_G_PER_LSB_CFG);
#else
nvtSetAccG_PER_LSB(calcAccel(1)/*IMU_G_PER_LSB_CFG*/);
#endif
}
else {
__disable_irq();
SYS_UnlockReg();
SYS_ResetChip();
printf("ACC connect - [FAIL]\n");
}
}
