void check_factory_uart1 (void) { uint8_t * buf_ptr; buf_ptr = getUSART1buf(); if(strncmp(buf_ptr, FACTORY_CHECK_STR, sizeof(FACTORY_CHECK_STR)) == 0) { // Serial data buffer clear USART1_flush(); factory_test_2nd(); #if defined(FACTORY_FW_FLASH) release_factory_flag(); #else //g_factoryfw_flag = 0; #endif //delay_ms(200); //NVIC_SystemReset(); } }
int main() { setvbuf(stdout, NULL, _IONBF, 0); setvbuf(stderr, NULL, _IONBF, 0); /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup file (startup_stm32f30x.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f30x.c file */ /* SysTick end of count event each 10ms */ RCC_GetClocksFreq(&RCC_Clocks); SysTick_Config(RCC_Clocks.HCLK_Frequency / 100); /* initialise USART1 debug output (TX on pin PA9 and RX on pin PA10) */ USART1_Init(); //printf("Starting\n"); USART1_flush(); /* printf("Initialising USB\n"); USBHID_Init(); printf("Initialising USB HID\n"); Joystick_init(); */ /* Initialise LEDs */ //printf("Initialising LEDs\n"); int i; for (i = 0; i < 8; ++i) { STM_EVAL_LEDInit(leds[i]); STM_EVAL_LEDOff(leds[i]); } /* Initialise gyro */ //printf("Initialising gyroscope\n"); Gyro_Init(); /* Initialise compass */ //printf("Initialising compass\n"); Compass_Init(); Delay(100); calibrate(); int C = 0, noAccelCount = 0; while (1) { float *acc = accs[C&1], *prevAcc = accs[(C&1)^1], *vel = vels[C&1], *prevVel = vels[(C&1)^1], *pos = poss[C&1], *prevPos = poss[(C&1)^1], *angRate = angRates[C&1], *prevAngRate = angRates[(C&1)^1], *ang = angs[C&1], *prevAng = angs[(C&1)^1], *mag = mags[C&1], *prevmag = mags[(C&1)^1]; /* Wait for data ready */ #if 0 Compass_ReadAccAvg(acc, 10); vecMul(axes, acc); //printf("X: %9.3f Y: %9.3f Z: %9.3f\n", acc[0], acc[1], acc[2]); float grav = acc[2]; acc[2] = 0; if (noAccelCount++ > 50) { for (i = 0; i < 2; ++i) { vel[i] = 0; prevVel[i] = 0; } noAccelCount = 0; } if (vecLen(acc) > 50.f) { for (i = 0; i < 2; ++i) { vel[i] += prevAcc[i] + (acc[i]-prevAcc[i])/2.f; pos[i] += prevVel[i] + (vel[i]-prevVel[i])/2.f; } noAccelCount = 0; } C += 1; if (((C) & 0x7F) == 0) { printf("%9.3f %9.3f %9.3f %9.3f %9.3f\n", vel[0], vel[1], pos[0], pos[1], grav); //printf("%3.1f%% %d %5.1f %6.3f\n", (float) timeReadI2C*100.f / totalTime, C, (float) C*100.f / (totalTime), grav); } #endif Compass_ReadMagAvg(mag, 2); vecMul(axes, mag); float compassAngle = atan2f(mag[1], mag[0]) * 180.f / PI; if (compassAngle > 180.f) compassAngle -= 360.f; //vecNorm(mag); Gyro_ReadAngRateAvg(mag, 2); printf("%6.3f:%6.3f,%6.3f,%6.3f\n", compassAngle, mag[0], mag[1], mag[2]); #if 0 Gyro_ReadAngRateAvg(angRate, 2); angRate[0] *= 180.f / PI; angRate[1] *= 180.f / PI; angRate[2] *= 180.f / PI; float s[3] = {sin(angRate[0]), sin(angRate[1]), sin(angRate[2])}; float c[3] = {cos(angRate[0]), cos(angRate[1]), cos(angRate[2])}; float gyroMat[3][3] = { {c[0]*c[1], c[0]*s[1], -s[1]}, {c[0]*s[1]*s[2]-s[0]*c[2], c[0]*c[2]+s[0]*s[1]*s[2], c[1]*s[2]}, {c[0]*s[1]*c[2]+s[0]*s[2], -c[0]*s[2]+s[0]*s[1]*c[2], c[1]*c[2]}}; /* float gyroWorldMat[3][3]; vecMulMatTrans(gyroWorldMat, axes, gyroMat); *ang = gyroWorldMat[2][0]; *ang += gyroWorldMat[2][1]; *ang += gyroWorldMat[2][2]; *ang /= 300.f; static const float ANGALPHA = 0.0f; *ang += ANGALPHA*(compassAngle - *ang); */ float rotObsVec[3]; memcpy(rotObsVec, axes[0], sizeof(rotObsVec)); vecMul(gyroMat, rotObsVec); vecMul(axes, rotObsVec); rotObsVec[2] = 0.f; vecNorm(rotObsVec); float angDelta = acos(rotObsVec[0]); if (((++C) & 0x7) == 0) { printf("%6.3f %6.3f %6.3f %6.3f\n", rotObsVec[0], rotObsVec[1], rotObsVec[2], angDelta); } #endif #if 0 float angRate[3]; /* Read Gyro Angular data */ Gyro_ReadAngRate(angRate); printf("X: %f Y: %f Z: %f\n", angRate[0], angRate[1], angRate[2]); float MagBuffer[3] = {0.0f}, AccBuffer[3] = {0.0f}; float fNormAcc,fSinRoll,fCosRoll,fSinPitch,fCosPitch = 0.0f, RollAng = 0.0f, PitchAng = 0.0f; float fTiltedX,fTiltedY = 0.0f; Compass_ReadMag(MagBuffer); Compass_ReadAcc(AccBuffer); for(i=0;i<3;i++) AccBuffer[i] /= 100.0f; fNormAcc = sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2])); fSinRoll = -AccBuffer[1]/fNormAcc; fCosRoll = sqrt(1.0-(fSinRoll * fSinRoll)); fSinPitch = AccBuffer[0]/fNormAcc; fCosPitch = sqrt(1.0-(fSinPitch * fSinPitch)); if ( fSinRoll >0) { if (fCosRoll>0) { RollAng = acos(fCosRoll)*180/PI; } else { RollAng = acos(fCosRoll)*180/PI + 180; } } else { if (fCosRoll>0) { RollAng = acos(fCosRoll)*180/PI + 360; } else { RollAng = acos(fCosRoll)*180/PI + 180; } } if ( fSinPitch >0) { if (fCosPitch>0) { PitchAng = acos(fCosPitch)*180/PI; } else { PitchAng = acos(fCosPitch)*180/PI + 180; } } else { if (fCosPitch>0) { PitchAng = acos(fCosPitch)*180/PI + 360; } else { PitchAng = acos(fCosPitch)*180/PI + 180; } } if (RollAng >=360) { RollAng = RollAng - 360; } if (PitchAng >=360) { PitchAng = PitchAng - 360; } fTiltedX = MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch; fTiltedY = MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch; HeadingValue = (float) ((atan2f((float)fTiltedY,(float)fTiltedX))*180)/PI; printf("Compass heading: %f\n", HeadingValue); #endif } return 1; }
void factory_test_1st (void) { int i; //uint16 adc_val = 0; int fail_count; // check A0~A3 if (teststep == 0) { fail_count = 0; #if 0 for(i = 0; i < 4; i++) { adc_val = ADC_DualConvertedValueTab[i]; if (adc_val > 100) //TODO { delay_ms(1); //printf("########## A%d[%d] OK.\r\n", i, adc_val); } else { fail_count += 1; printf("########## A%d[%d] Fail.\r\n", i, adc_val); } } #endif if(fail_count == 0) { printf("########## A0 = %d || A1 = %d || A2 = %d || A3 = %d\r\n", ADC_DualConvertedValueTab[0] , ADC_DualConvertedValueTab[1] , ADC_DualConvertedValueTab[2] , ADC_DualConvertedValueTab[3]); } teststep = 1; } // check RS422 if (teststep == 1) { printf("########## RS422 TX:TEST\r\n"); UART2_flush(); UART_write(FACTORY_TEST_STR, sizeof(FACTORY_TEST_STR)); UART_write("\r", 1); teststep = 2; } // check SW1~SW3 if (teststep == 2) { Delay(10000000); EXTI_Configuration(); USART1_flush(); teststep = 3; } #if 0 // check VCC, Temperature if (teststep == 5) { adc_val = ADC_DualConvertedValueTab[0]; if (adc_val > 4000) { printf("########## VCC[%d] OK.\r\n", adc_val); adc_val = ADC_DualConvertedValueTab[1]; if (adc_val > 100) //TODO printf("########## Temperature[%d] OK.\r\n", adc_val); else printf("########## Temperature[%d] Fail.\r\n", adc_val); teststep = 6; } } #endif //check_factory_uart1(); }
int main() { setvbuf(stdout, NULL, _IONBF, 0); setvbuf(stderr, NULL, _IONBF, 0); /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup file (startup_stm32f30x.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f30x.c file */ /* SysTick end of count event each 10ms */ RCC_GetClocksFreq(&RCC_Clocks); SysTick_Config(RCC_Clocks.HCLK_Frequency / 100); /* initialise USART1 debug output (TX on pin PA9 and RX on pin PA10) */ USART1_Init(); //printf("Starting\n"); USART1_flush(); /* Initialise LEDs */ //printf("Initialising LEDs\n"); int i; for (i = 0; i < 8; ++i) { STM_EVAL_LEDInit(leds[i]); STM_EVAL_LEDOff(leds[i]); } /* Initialise gyro */ //printf("Initialising gyroscope\n"); Gyro_Init(); /* Initialise compass */ //printf("Initialising compass\n"); Compass_Init(); Delay(100); // perform calibration calibrate(); while (1) { float angRate[3], mag[3]; // read average compass values Compass_ReadMagAvg(mag, 2); // rotate the compass values so that they are aligned with Earth vecMul(axes, mag); // calculate the heading through inverse tan of the Y/X magnetic strength float compassAngle = atan2f(mag[1], mag[0]) * 180.f / PI; // fix heading to be in range -180 to 180 if (compassAngle > 180.f) compassAngle -= 360.f; // read average gyro values Gyro_ReadAngRateAvg(angRate, 2); // print out everything printf("c%6.3f\ng%6.3f\n", compassAngle, angRate[2]-zeroAngRate[2]); } return 1; }