void systemInit(void) { RCC_ClocksTypeDef rccClocks; /////////////////////////////////// // Init cycle counter cycleCounterInit(); // SysTick SysTick_Config(SystemCoreClock / 1000); // Turn on peripherial clocks RCC_AHBPeriphClockCmd(RCC_AHBPeriph_ADC12, ENABLE); RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); // USART1, USART2 RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE); // ADC2 RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE); RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); // PPM RX RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); // PWM ESC Out 1 & 2 RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); // PWM ESC Out 5 & 6 RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); // PWM Servo Out 1, 2, 3, & 4 RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); // 500 Hz dt Counter RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); // 100 Hz dt Counter RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM15, ENABLE); // PWM ESC Out 3 & 4 RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM16, ENABLE); // RangeFinder PWM RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM17, ENABLE); // Spektrum Frame Sync RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); // Telemetry RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); // GPS RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); // Spektrum RX /////////////////////////////////////////////////////////////////////////// spiInit(SPI2); /////////////////////////////////// checkSensorEEPROM(false); checkSystemEEPROM(false); readSensorEEPROM(); readSystemEEPROM(); /////////////////////////////////// if (systemConfig.receiverType == SPEKTRUM) checkSpektrumBind(); /////////////////////////////////// checkResetType(); NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority /////////////////////////////////// gpsPortClearBuffer = &uart2ClearBuffer; gpsPortNumCharsAvailable = &uart2NumCharsAvailable; gpsPortPrintBinary = &uart2PrintBinary; gpsPortRead = &uart2Read; telemPortAvailable = &uart1Available; telemPortPrint = &uart1Print; telemPortPrintF = &uart1PrintF; telemPortRead = &uart1Read; /////////////////////////////////// initMixer(); usbInit(); gpioInit(); uart1Init(); uart2Init(); LED0_OFF; delay(10000); // 10 seconds of 20 second delay for sensor stabilization checkUsbActive(); /////////////////////////////////// #ifdef __VERSION__ cliPortPrintF("\ngcc version " __VERSION__ "\n"); #endif cliPortPrintF("\nFF32mini Firmware V%s, Build Date " __DATE__ " "__TIME__" \n", __FF32MINI_VERSION); if ((RCC->CR & RCC_CR_HSERDY) != RESET) { cliPortPrint("\nRunning on external HSE clock....\n"); } else { cliPortPrint("\nERROR: Running on internal HSI clock....\n"); } RCC_GetClocksFreq(&rccClocks); cliPortPrintF("\nHCLK-> %2d MHz\n", rccClocks.HCLK_Frequency / 1000000); cliPortPrintF( "PCLK1-> %2d MHz\n", rccClocks.PCLK1_Frequency / 1000000); cliPortPrintF( "PCLK2-> %2d MHz\n", rccClocks.PCLK2_Frequency / 1000000); cliPortPrintF( "SYSCLK-> %2d MHz\n\n", rccClocks.SYSCLK_Frequency / 1000000); if (systemConfig.receiverType == PPM) cliPortPrint("Using PPM Receiver....\n\n"); else cliPortPrint("Using Spektrum Satellite Receiver....\n\n"); initUBLOX(); delay(10000); // Remaining 10 seconds of 20 second delay for sensor stabilization - probably not long enough.. /////////////////////////////////// adcInit(); aglInit(); pwmServoInit(); if (systemConfig.receiverType == SPEKTRUM) spektrumInit(); else ppmRxInit(); timingFunctionsInit(); batteryInit(); initFirstOrderFilter(); initMavlink(); initPID(); LED0_ON; initMPU6000(); initMag(); initPressure(); }
int main(void) { /////////////////////////////////////////////////////////////////////////// uint32_t currentTime; systemReady = false; systemInit(); systemReady = true; evrPush(EVR_StartingMain, 0); while (1) { evrCheck(); /////////////////////////////// if (frame_50Hz) { frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; processFlightCommands(); if (newTemperatureReading && newPressureReading) { d1Value = d1.value; d2Value = d2.value; calculateTemperature(); calculatePressureAltitude(); newTemperatureReading = false; newPressureReading = false; } sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]); executionTime50Hz = micros() - currentTime; } /////////////////////////////// if (frame_10Hz) { frame_10Hz = false; currentTime = micros(); deltaTime10Hz = currentTime - previous10HzTime; previous10HzTime = currentTime; if (newMagData == true) { sensors.mag10Hz[XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]; sensors.mag10Hz[YAXIS] = -((float)rawMag[YAXIS].value * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS]); sensors.mag10Hz[ZAXIS] = -((float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]); newMagData = false; magDataUpdate = true; } decodeUbloxMsg(); batMonTick(); checkUsbActive(); cliCom(); if (systemConfig.mavlinkEnabled == true) { mavlinkSendAttitude(); mavlinkSendVfrHud(); } executionTime10Hz = micros() - currentTime; } /////////////////////////////// if (frame_500Hz) { frame_500Hz = false; currentTime = micros(); deltaTime500Hz = currentTime - previous500HzTime; previous500HzTime = currentTime; TIM_Cmd(TIM6, DISABLE); timerValue = TIM_GetCounter(TIM6); TIM_SetCounter(TIM6, 0); TIM_Cmd(TIM6, ENABLE); dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop computeMPU6000TCBias(); sensors.accel500Hz[XAXIS] = -((float)accelSummedSamples500Hz[XAXIS] * 0.5f - sensorConfig.accelBiasMPU[XAXIS] - accelTCBias[XAXIS]) * sensorConfig.accelScaleFactorMPU[XAXIS]; sensors.accel500Hz[YAXIS] = ((float)accelSummedSamples500Hz[YAXIS] * 0.5f - sensorConfig.accelBiasMPU[YAXIS] - accelTCBias[YAXIS]) * sensorConfig.accelScaleFactorMPU[YAXIS]; sensors.accel500Hz[ZAXIS] = -((float)accelSummedSamples500Hz[ZAXIS] * 0.5f - sensorConfig.accelBiasMPU[ZAXIS] - accelTCBias[ZAXIS]) * sensorConfig.accelScaleFactorMPU[ZAXIS]; //sensors.accel500Hz[XAXIS] = firstOrderFilter(sensors.accel500Hz[XAXIS], &firstOrderFilters[ACCEL500HZ_X_LOWPASS]); //sensors.accel500Hz[YAXIS] = firstOrderFilter(sensors.accel500Hz[YAXIS], &firstOrderFilters[ACCEL500HZ_Y_LOWPASS]); //sensors.accel500Hz[ZAXIS] = firstOrderFilter(sensors.accel500Hz[ZAXIS], &firstOrderFilters[ACCEL500HZ_Z_LOWPASS]); sensors.gyro500Hz[ROLL ] = -((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[PITCH] = ((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; MargAHRSupdate( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], sensorConfig.accelCutoff, magDataUpdate, dt500Hz ); magDataUpdate = false; computeAxisCommands(dt500Hz); mixTable(); writeServos(); writeMotors(); executionTime500Hz = micros() - currentTime; } /////////////////////////////// if (frame_100Hz) { frame_100Hz = false; currentTime = micros(); deltaTime100Hz = currentTime - previous100HzTime; previous100HzTime = currentTime; TIM_Cmd(TIM7, DISABLE); timerValue = TIM_GetCounter(TIM7); TIM_SetCounter(TIM7, 0); TIM_Cmd(TIM7, ENABLE); dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] * 0.1f - sensorConfig.accelBiasMPU[XAXIS] - accelTCBias[XAXIS]) * sensorConfig.accelScaleFactorMPU[XAXIS]; sensors.accel100Hz[YAXIS] = ((float)accelSummedSamples100Hz[YAXIS] * 0.1f - sensorConfig.accelBiasMPU[YAXIS] - accelTCBias[YAXIS]) * sensorConfig.accelScaleFactorMPU[YAXIS]; sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] * 0.1f - sensorConfig.accelBiasMPU[ZAXIS] - accelTCBias[ZAXIS]) * sensorConfig.accelScaleFactorMPU[ZAXIS]; //sensors.accel100Hz[XAXIS] = firstOrderFilter(sensors.accel100Hz[XAXIS], &firstOrderFilters[ACCEL100HZ_X_LOWPASS]); //sensors.accel100Hz[YAXIS] = firstOrderFilter(sensors.accel100Hz[YAXIS], &firstOrderFilters[ACCEL100HZ_Y_LOWPASS]); //sensors.accel100Hz[ZAXIS] = firstOrderFilter(sensors.accel100Hz[ZAXIS], &firstOrderFilters[ACCEL100HZ_Z_LOWPASS]); createRotationMatrix(); bodyAccelToEarthAccel(); vertCompFilter(dt100Hz); if (armed == true) { if ( systemConfig.activeTelemetry == 1 ) { // 500 Hz Accels telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS]); } if ( systemConfig.activeTelemetry == 2 ) { // 500 Hz Gyros telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW ]); } if ( systemConfig.activeTelemetry == 4 ) { // 500 Hz Attitudes telemPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ], sensors.attitude500Hz[PITCH], sensors.attitude500Hz[YAW ]); } if ( systemConfig.activeTelemetry == 8 ) { // Vertical Variables telemPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld\n", earthAxisAccels[ZAXIS], sensors.pressureAlt50Hz, hDotEstimate, hEstimate, ms5611Temperature); } if ( systemConfig.activeTelemetry == 16 ) { // Vertical Variables telemPortPrintF("%9.4f, %9.4f, %9.4f, %4ld, %1d, %9.4f, %9.4f\n", verticalVelocityCmd, hDotEstimate, hEstimate, ms5611Temperature, verticalModeState, throttleCmd, systemConfig.PID[HDOT_PID].iTerm); } } executionTime100Hz = micros() - currentTime; } /////////////////////////////// if (frame_5Hz) { frame_5Hz = false; currentTime = micros(); deltaTime5Hz = currentTime - previous5HzTime; previous5HzTime = currentTime; gpsUpdated(); //if (systemConfig.mavlinkEnabled == true) //{ // mavlinkSendGpsRaw(); //} if (batMonVeryLowWarning > 0) { BEEP_TOGGLE; batMonVeryLowWarning--; } if (execUp == true) LED0_TOGGLE; executionTime5Hz = micros() - currentTime; } /////////////////////////////// if (frame_1Hz) { frame_1Hz = false; currentTime = micros(); deltaTime1Hz = currentTime - previous1HzTime; previous1HzTime = currentTime; if (execUp == false) execUpCount++; if ((execUpCount == 5) && (execUp == false)) { execUp = true; pwmEscInit(); homeData.magHeading = sensors.attitude500Hz[YAW]; } if (batMonLowWarning > 0) { BEEP_TOGGLE; batMonLowWarning--; } if (systemConfig.mavlinkEnabled == true) { mavlinkSendHeartbeat(); mavlinkSendSysStatus(); } executionTime1Hz = micros() - currentTime; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }
void systemInit(void) { RCC_ClocksTypeDef rccClocks; /////////////////////////////////// // Init cycle counter cycleCounterInit(); // SysTick SysTick_Config(SystemCoreClock / 1000); // Turn on peripherial clcoks RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC2, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI3, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM10, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM11, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); /////////////////////////////////// checkFirstTime(false); readEEPROM(); if (eepromConfig.receiverType == SPEKTRUM) checkSpektrumBind(); checkResetType(); NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority /////////////////////////////////// gpsPortClearBuffer = &uart2ClearBuffer; gpsPortNumCharsAvailable = &uart2NumCharsAvailable; gpsPortPrintBinary = &uart2PrintBinary; gpsPortRead = &uart2Read; telemPortAvailable = &uart1Available; telemPortPrint = &uart1Print; telemPortPrintF = &uart1PrintF; telemPortRead = &uart1Read; /////////////////////////////////// initMixer(); usbInit(); ledInit(); uart1Init(); uart2Init(); BLUE_LED_ON; /////////////////////////////////// delay(10000); // 10 seconds of 20 second delay for sensor stabilization checkUsbActive(); #ifdef __VERSION__ cliPortPrintF("\ngcc version " __VERSION__ "\n"); #endif cliPortPrintF("\nAQ32Plus Firmware V%s, Build Date " __DATE__ " "__TIME__" \n", __AQ32PLUS_VERSION); if ((RCC->CR & RCC_CR_HSERDY) != RESET) { cliPortPrint("\nRunning on external HSE clock....\n"); } else { cliPortPrint("\nERROR: Running on internal HSI clock....\n"); } RCC_GetClocksFreq(&rccClocks); cliPortPrintF("\nHCLK-> %3d MHz\n", rccClocks.HCLK_Frequency / 1000000); cliPortPrintF( "PCLK1-> %3d MHz\n", rccClocks.PCLK1_Frequency / 1000000); cliPortPrintF( "PCLK2-> %3d MHz\n", rccClocks.PCLK2_Frequency / 1000000); cliPortPrintF( "SYSCLK-> %3d MHz\n\n", rccClocks.SYSCLK_Frequency / 1000000); initUBLOX(); delay(10000); // Remaining 10 seconds of 20 second delay for sensor stabilization - probably not long enough.. /////////////////////////////////// adcInit(); i2cInit(I2C1); i2cInit(I2C2); pwmServoInit(); rxInit(); spiInit(SPI2); spiInit(SPI3); timingFunctionsInit(); batteryInit(); initFirstOrderFilter(); initMavlink(); initMax7456(); initPID(); GREEN_LED_ON; initMPU6000(); initMag(); initPressure(); }
int main(void) { /////////////////////////////////////////////////////////////////////////// uint32_t currentTime; arm_matrix_instance_f32 a; arm_matrix_instance_f32 b; arm_matrix_instance_f32 x; systemReady = false; systemInit(); systemReady = true; evrPush(EVR_StartingMain, 0); #ifdef _DTIMING #define LA1_ENABLE GPIO_SetBits(GPIOA, GPIO_Pin_4) #define LA1_DISABLE GPIO_ResetBits(GPIOA, GPIO_Pin_4) #define LA4_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_5) #define LA4_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_5) #define LA2_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_2) #define LA2_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_2) #define LA3_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_3) #define LA3_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_3) GPIO_InitTypeDef GPIO_InitStructure; RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); GPIO_StructInit(&GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOA, &GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOB, &GPIO_InitStructure); // Init pins GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_5; //GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOC, &GPIO_InitStructure); // PB0_DISABLE; LA4_DISABLE; LA2_DISABLE; LA3_DISABLE; LA1_DISABLE; #endif while (1) { checkUsbActive(false); evrCheck(); /////////////////////////////// if (frame_50Hz) { #ifdef _DTIMING LA2_ENABLE; #endif frame_50Hz = false; currentTime = micros(); deltaTime50Hz = currentTime - previous50HzTime; previous50HzTime = currentTime; processFlightCommands(); if (newTemperatureReading && newPressureReading) { d1Value = d1.value; d2Value = d2.value; calculateTemperature(); calculatePressureAltitude(); newTemperatureReading = false; newPressureReading = false; } sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]); rssiMeasure(); updateMax7456(currentTime, 0); executionTime50Hz = micros() - currentTime; #ifdef _DTIMING LA2_DISABLE; #endif } /////////////////////////////// if (frame_10Hz) { #ifdef _DTIMING LA4_ENABLE; #endif frame_10Hz = false; currentTime = micros(); deltaTime10Hz = currentTime - previous10HzTime; previous10HzTime = currentTime; if (newMagData == true) { nonRotatedMagData[XAXIS] = (rawMag[XAXIS].value * magScaleFactor[XAXIS]) - eepromConfig.magBias[XAXIS + eepromConfig.externalHMC5883]; nonRotatedMagData[YAXIS] = (rawMag[YAXIS].value * magScaleFactor[YAXIS]) - eepromConfig.magBias[YAXIS + eepromConfig.externalHMC5883]; nonRotatedMagData[ZAXIS] = (rawMag[ZAXIS].value * magScaleFactor[ZAXIS]) - eepromConfig.magBias[ZAXIS + eepromConfig.externalHMC5883]; arm_mat_init_f32(&a, 3, 3, (float *)hmcOrientationMatrix); arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedMagData); arm_mat_init_f32(&x, 3, 1, sensors.mag10Hz); arm_mat_mult_f32(&a, &b, &x); newMagData = false; magDataUpdate = true; } decodeUbloxMsg(); batMonTick(); cliCom(); if (eepromConfig.mavlinkEnabled == true) { mavlinkSendAttitude(); mavlinkSendVfrHud(); } executionTime10Hz = micros() - currentTime; #ifdef _DTIMING LA4_DISABLE; #endif } /////////////////////////////// if (frame_500Hz) { #ifdef _DTIMING LA1_ENABLE; #endif frame_500Hz = false; currentTime = micros(); deltaTime500Hz = currentTime - previous500HzTime; previous500HzTime = currentTime; TIM_Cmd(TIM10, DISABLE); timerValue = TIM_GetCounter(TIM10); TIM_SetCounter(TIM10, 0); TIM_Cmd(TIM10, ENABLE); dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop computeMPU6000TCBias(); nonRotatedAccelData[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] * 0.5f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR; nonRotatedAccelData[YAXIS] = ((float)accelSummedSamples500Hz[YAXIS] * 0.5f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR; nonRotatedAccelData[ZAXIS] = ((float)accelSummedSamples500Hz[ZAXIS] * 0.5f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR; arm_mat_init_f32(&a, 3, 3, (float *)mpuOrientationMatrix); arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedAccelData); arm_mat_init_f32(&x, 3, 1, sensors.accel500Hz); arm_mat_mult_f32(&a, &b, &x); nonRotatedGyroData[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] * 0.5f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR; nonRotatedGyroData[PITCH] = ((float)gyroSummedSamples500Hz[PITCH] * 0.5f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR; nonRotatedGyroData[YAW ] = ((float)gyroSummedSamples500Hz[YAW] * 0.5f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR; arm_mat_init_f32(&a, 3, 3, (float *)mpuOrientationMatrix); arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedGyroData); arm_mat_init_f32(&x, 3, 1, sensors.gyro500Hz); arm_mat_mult_f32(&a, &b, &x); MargAHRSupdate(sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], eepromConfig.accelCutoff, magDataUpdate, dt500Hz); magDataUpdate = false; computeAxisCommands(dt500Hz); mixTable(); writeServos(); writeMotors(); executionTime500Hz = micros() - currentTime; #ifdef _DTIMING LA1_DISABLE; #endif } /////////////////////////////// if (frame_100Hz) { #ifdef _DTIMING LA3_ENABLE; #endif frame_100Hz = false; currentTime = micros(); deltaTime100Hz = currentTime - previous100HzTime; previous100HzTime = currentTime; TIM_Cmd(TIM11, DISABLE); timerValue = TIM_GetCounter(TIM11); TIM_SetCounter(TIM11, 0); TIM_Cmd(TIM11, ENABLE); dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop nonRotatedAccelData[XAXIS] = ((float)accelSummedSamples100Hz[XAXIS] * 0.1f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR; nonRotatedAccelData[YAXIS] = ((float)accelSummedSamples100Hz[YAXIS] * 0.1f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR; nonRotatedAccelData[ZAXIS] = ((float)accelSummedSamples100Hz[ZAXIS] * 0.1f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR; arm_mat_init_f32(&a, 3, 3, (float *)mpuOrientationMatrix); arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedAccelData); arm_mat_init_f32(&x, 3, 1, sensors.accel100Hz); arm_mat_mult_f32(&a, &b, &x); createRotationMatrix(); bodyAccelToEarthAccel(); vertCompFilter(dt100Hz); if (armed == true) { if ( eepromConfig.activeTelemetry == 1 ) { // Roll Loop openLogPortPrintF("1,%1d,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f\n", flightMode, rateCmd[ROLL], sensors.gyro500Hz[ROLL], ratePID[ROLL], attCmd[ROLL], sensors.attitude500Hz[ROLL], attPID[ROLL]); } if ( eepromConfig.activeTelemetry == 2 ) { // Pitch Loop openLogPortPrintF("2,%1d,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f\n", flightMode, rateCmd[PITCH], sensors.gyro500Hz[PITCH], ratePID[PITCH], attCmd[PITCH], sensors.attitude500Hz[PITCH], attPID[PITCH]); } if ( eepromConfig.activeTelemetry == 4 ) { // Sensors openLogPortPrintF("3,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,\n", sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS], sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW], sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS], sensors.attitude500Hz[ROLL], sensors.attitude500Hz[PITCH], sensors.attitude500Hz[YAW]); } if ( eepromConfig.activeTelemetry == 8 ) { } if ( eepromConfig.activeTelemetry == 16) { // Vertical Variables openLogPortPrintF("%9.4f, %9.4f, %9.4f, %4ld, %1d, %9.4f\n", verticalVelocityCmd, hDotEstimate, hEstimate, ms5611Temperature, verticalModeState, throttleCmd); } } executionTime100Hz = micros() - currentTime; #ifdef _DTIMING LA3_DISABLE; #endif } /////////////////////////////// if (frame_5Hz) { frame_5Hz = false; currentTime = micros(); deltaTime5Hz = currentTime - previous5HzTime; previous5HzTime = currentTime; if (gpsValid() == true) { } //if (eepromConfig.mavlinkEnabled == true) //{ // mavlinkSendGpsRaw(); //} if (batMonVeryLowWarning > 0) { LED1_TOGGLE; batMonVeryLowWarning--; } if (execUp == true) BLUE_LED_TOGGLE; executionTime5Hz = micros() - currentTime; } /////////////////////////////// if (frame_1Hz) { frame_1Hz = false; currentTime = micros(); deltaTime1Hz = currentTime - previous1HzTime; previous1HzTime = currentTime; if (execUp == true) GREEN_LED_TOGGLE; if (execUp == false) execUpCount++; // Initialize sensors after being warmed up if ((execUpCount == 20) && (execUp == false)) { computeMPU6000RTData(); initMag(); initPressure(); } // Initialize PWM and set mag after sensor warmup if ((execUpCount == 25) && (execUp == false)) { execUp = true; pwmEscInit(); homeData.magHeading = sensors.attitude500Hz[YAW]; } if (batMonLowWarning > 0) { LED1_TOGGLE; batMonLowWarning--; } if (eepromConfig.mavlinkEnabled == true) { mavlinkSendHeartbeat(); mavlinkSendSysStatus(); } executionTime1Hz = micros() - currentTime; } //////////////////////////////// } /////////////////////////////////////////////////////////////////////////// }