void systemInit(void)
{
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
///////////////////////////////////
checkFirstTime(false);
readEEPROM();
if (eepromConfig.receiverType == SPEKTRUM)
checkSpektrumBind();
checkResetType();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
initMixer();
ledInit();
cliInit();
BLUE_LED_ON;
delay(20000); // 20 sec total delay for sensor stabilization - probably not long enough.....
adcInit();
batteryInit();
gpsInit();
i2cInit(I2C1);
i2cInit(I2C2);
pwmEscInit(eepromConfig.escPwmRate);
pwmServoInit(eepromConfig.servoPwmRate);
rxInit();
spiInit(SPI2);
spiInit(SPI3);
telemetryInit();
timingFunctionsInit();
initFirstOrderFilter();
initGPS();
initMax7456();
initPID();
GREEN_LED_ON;
initMPU6000();
initMag(HMC5883L_I2C);
initPressure(MS5611_I2C);
}
void systemInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
// Turn on clocks for stuff we use
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, 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_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE);
RCC_ClearFlag();
// Make all GPIO in by default to save power and reduce noise
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_Init(GPIOC, &GPIO_InitStructure);
// Turn off JTAG port 'cause we're using the GPIO for leds
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);
// Init cycle counter
cycleCounterInit();
// SysTick
SysTick_Config(SystemCoreClock / 1000);
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
checkFirstTime(false);
readEEPROM();
ledInit();
LED0_ON;
initMixer();
pwmOutputConfig.escPwmRate = eepromConfig.escPwmRate;
pwmOutputConfig.servoPwmRate = eepromConfig.servoPwmRate;
cliInit(115200);
i2cInit(I2C2);
pwmOutputInit(&pwmOutputConfig);
rxInit();
delay(20000); // 20 sec delay for sensor stabilization - probably not long enough.....
LED1_ON;
initAccel();
initGyro();
initMag();
initPressure();
initPID();
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPrint("Sensor CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliRead();
cliPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPrintF("\n");
cliPrintF("External HMC5883 in use: %s\n", eepromConfig.externalHMC5883 ? "Yes" : "No");
cliPrintF("External MS5611 in use: %s\n", eepromConfig.externalMS5611 ? "Yes" : "No");
cliPrintF("\n");
cliPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS],
eepromConfig.accelTCBiasSlope[YAXIS],
eepromConfig.accelTCBiasSlope[ZAXIS]);
cliPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS],
eepromConfig.accelTCBiasIntercept[YAXIS],
eepromConfig.accelTCBiasIntercept[ZAXIS]);
cliPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ],
eepromConfig.gyroTCBiasIntercept[PITCH],
eepromConfig.gyroTCBiasIntercept[YAW ]);
cliPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS],
eepromConfig.magBias[YAXIS],
eepromConfig.magBias[ZAXIS]);
cliPrintF("Accel One G: %9.4f\n", accelOneG);
cliPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff);
cliPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc);
cliPrintF("KiAcc (MARG): %9.4f\n", eepromConfig.KiAcc);
cliPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag);
cliPrintF("KiMag (MARG): %9.4f\n", eepromConfig.KiMag);
cliPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA);
cliPrintF("hdot est/h est Comp Fil B: %9.4f\n", eepromConfig.compFilterB);
cliPrint("MPU6000 DLPF: ");
switch(eepromConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPrint("42 Hz\n");
break;
}
cliPrint("Magnetic Variation: ");
if (eepromConfig.magVar >= 0.0f)
cliPrintF("E%6.4f\n\n", eepromConfig.magVar * R2D);
else
cliPrintF("W%6.4f\n\n", -eepromConfig.magVar * R2D);
if (eepromConfig.verticalVelocityHoldOnly)
cliPrint("Vertical Velocity Hold Only\n\n");
else
cliPrint("Vertical Velocity and Altitude Hold\n\n");
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6000 Calibration
mpu6000Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'e': // Toggle External HMC5883 Use
if (eepromConfig.externalHMC5883)
eepromConfig.externalHMC5883 = false;
else
eepromConfig.externalHMC5883 = true;
initMag();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'f': // Toggle External MS5611 Use
if (eepromConfig.externalMS5611)
eepromConfig.externalMS5611 = false;
else
eepromConfig.externalMS5611 = true;
initPressure();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (eepromConfig.verticalVelocityHoldOnly)
eepromConfig.verticalVelocityHoldOnly = false;
else
eepromConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6000 Digital Low Pass Filter
tempInt = (uint8_t)readFloatCLI();
switch(tempInt)
{
case DLPF_256HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
setSPIdivisor(MPU6000_SPI, 64); // 0.65625 MHz SPI clock (within 20 +/- 10%)
GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
spiTransfer(MPU6000_SPI, MPU6000_CONFIG);
spiTransfer(MPU6000_SPI, eepromConfig.dlpfSetting);
GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%)
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Accel Cutoff
eepromConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc, kiAcc
eepromConfig.KpAcc = readFloatCLI();
eepromConfig.KiAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag, kiMag
eepromConfig.KpMag = readFloatCLI();
eepromConfig.KiMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
eepromConfig.compFilterA = readFloatCLI();
eepromConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'M': // Magnetic Variation
eepromConfig.magVar = readFloatCLI() * D2R;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3, see aq32Plus.h\n");
cliPrint("'b' MPU6000 Temp Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n");
cliPrint(" 'D' Set kpMag/kiMag DkpMag;kiMag\n");
cliPrint("'e' Toggle External HMC5883 State 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPrint("'f' Toggle External MS5611 State 'M' Set Mag Variation (+ East, - West) MMagVar\n");
cliPrint("'v' Toggle Vertical Velocity Hold Only\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n\n");
validQuery = false;
break;
///////////////////////////
}
}
}
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();
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPortPrint("Sensor CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliPortRead();
cliPortPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPortPrintF("\n");
cliPortPrintF("External HMC5883 in use: %s\n", eepromConfig.externalHMC5883 ? "Yes" : "No");
cliPortPrintF("External MS5611 in use: %s\n", eepromConfig.externalMS5611 ? "Yes" : "No");
cliPortPrintF("MXR9150 Accel in use: %s\n", eepromConfig.useMXR9150 ? "Yes" : "No");
cliPortPrintF("\n");
if (eepromConfig.useMXR9150 == true)
{
cliPortPrintF("MXR Accel Bias: %9.3f, %9.3f, %9.3f\n", eepromConfig.accelBiasMXR[XAXIS],
eepromConfig.accelBiasMXR[YAXIS],
eepromConfig.accelBiasMXR[ZAXIS]);
cliPortPrintF("MXR Accel Scale Factor: %9.7f, %9.7f, %9.7f\n", eepromConfig.accelScaleFactorMXR[XAXIS],
eepromConfig.accelScaleFactorMXR[YAXIS],
eepromConfig.accelScaleFactorMXR[ZAXIS]);
}
else
{
cliPortPrintF("MPU Accel Bias: %9.3f, %9.3f, %9.3f\n", eepromConfig.accelBiasMPU[XAXIS],
eepromConfig.accelBiasMPU[YAXIS],
eepromConfig.accelBiasMPU[ZAXIS]);
cliPortPrintF("MPU Accel Scale Factor: %9.7f, %9.7f, %9.7f\n", eepromConfig.accelScaleFactorMPU[XAXIS],
eepromConfig.accelScaleFactorMPU[YAXIS],
eepromConfig.accelScaleFactorMPU[ZAXIS]);
}
cliPortPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS],
eepromConfig.accelTCBiasSlope[YAXIS],
eepromConfig.accelTCBiasSlope[ZAXIS]);
cliPortPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS],
eepromConfig.accelTCBiasIntercept[YAXIS],
eepromConfig.accelTCBiasIntercept[ZAXIS]);
cliPortPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPortPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ],
eepromConfig.gyroTCBiasIntercept[PITCH],
eepromConfig.gyroTCBiasIntercept[YAW ]);
cliPortPrintF("Internal Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS],
eepromConfig.magBias[YAXIS],
eepromConfig.magBias[ZAXIS]);
cliPortPrintF("External Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS + 3],
eepromConfig.magBias[YAXIS + 3],
eepromConfig.magBias[ZAXIS + 3]);
cliPortPrintF("Accel One G: %9.4f\n", accelOneG);
cliPortPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff);
cliPortPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc);
cliPortPrintF("KiAcc (MARG): %9.4f\n", eepromConfig.KiAcc);
cliPortPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag);
cliPortPrintF("KiMag (MARG): %9.4f\n", eepromConfig.KiMag);
cliPortPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA);
cliPortPrintF("hdot est/h est Comp Fil B: %9.4f\n", eepromConfig.compFilterB);
cliPortPrint("MPU6000 DLPF: ");
switch(eepromConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPortPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPortPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPortPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPortPrint("42 Hz\n");
break;
}
cliPortPrint("Sensor Orientation: ");
switch(eepromConfig.sensorOrientation)
{
case 1:
cliPortPrint("Normal\n");
break;
case 2:
cliPortPrint("Rotated 90 Degrees CW\n");
break;
case 3:
cliPortPrint("Rotated 180 Degrees\n");
break;
case 4:
cliPortPrint("Rotated 90 Degrees CCW\n");
break;
default:
cliPortPrint("Normal\n");
}
if (eepromConfig.verticalVelocityHoldOnly)
cliPortPrint("Vertical Velocity Hold Only\n\n");
else
cliPortPrint("Vertical Velocity and Altitude Hold\n\n");
cliPortPrintF("Voltage Monitor Scale: %9.4f\n", eepromConfig.voltageMonitorScale);
cliPortPrintF("Voltage Monitor Bias: %9.4f\n", eepromConfig.voltageMonitorBias);
cliPortPrintF("Number of Battery Cells: %1d\n\n", eepromConfig.batteryCells);
cliPortPrintF("Battery Low Setpoint: %4.2f volts\n", eepromConfig.batteryLow);
cliPortPrintF("Battery Very Low Setpoint: %4.2f volts\n", eepromConfig.batteryVeryLow);
cliPortPrintF("Battery Max Low Setpoint: %4.2f volts\n\n", eepromConfig.batteryMaxLow);
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6000 Calibration
mpu6000Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'd': // Accel Bias and Scale Factor Calibration
if (eepromConfig.useMXR9150 == true)
accelCalibrationMXR();
else
accelCalibrationMPU();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'e': // Toggle External HMC5883 Use
if (eepromConfig.externalHMC5883 == 0)
eepromConfig.externalHMC5883 = 3;
else
eepromConfig.externalHMC5883 = 0;
initMag();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'f': // Toggle External MS5611 Use
if (eepromConfig.externalMS5611)
eepromConfig.externalMS5611 = false;
else
eepromConfig.externalMS5611 = true;
initPressure();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'g': // Toggle MXR9150 Use
if (eepromConfig.useMXR9150)
eepromConfig.useMXR9150 = false;
else
eepromConfig.useMXR9150 = true;
computeMPU6000RTData();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'h': // MXR Bias
eepromConfig.accelBiasMXR[XAXIS] = readFloatCLI();
eepromConfig.accelBiasMXR[YAXIS] = readFloatCLI();
eepromConfig.accelBiasMXR[ZAXIS] = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (eepromConfig.verticalVelocityHoldOnly)
eepromConfig.verticalVelocityHoldOnly = false;
else
eepromConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6000 Digital Low Pass Filter
tempInt = (uint8_t)readFloatCLI();
switch(tempInt)
{
case DLPF_256HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
setSPIdivisor(MPU6000_SPI, 64); // 0.65625 MHz SPI clock (within 20 +/- 10%)
GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
spiTransfer(MPU6000_SPI, MPU6000_CONFIG);
spiTransfer(MPU6000_SPI, eepromConfig.dlpfSetting);
GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%)
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Accel Cutoff
eepromConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc, kiAcc
eepromConfig.KpAcc = readFloatCLI();
eepromConfig.KiAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag, kiMag
eepromConfig.KpMag = readFloatCLI();
eepromConfig.KiMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
eepromConfig.compFilterA = readFloatCLI();
eepromConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Sensor Orientation
eepromConfig.sensorOrientation = (uint8_t)readFloatCLI();
orientSensors();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'N': // Set Voltage Monitor Trip Points
eepromConfig.batteryLow = readFloatCLI();
eepromConfig.batteryVeryLow = readFloatCLI();
eepromConfig.batteryMaxLow = readFloatCLI();
thresholds[BATTERY_LOW].value = eepromConfig.batteryLow;
thresholds[BATTERY_VERY_LOW].value = eepromConfig.batteryVeryLow;
thresholds[BATTRY_MAX_LOW].value = eepromConfig.batteryMaxLow;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'V': // Set Voltage Monitor Parameters
eepromConfig.voltageMonitorScale = readFloatCLI();
eepromConfig.voltageMonitorBias = readFloatCLI();
eepromConfig.batteryCells = (uint8_t)readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3, see aq32Plus.h\n");
cliPortPrint("'b' MPU6000 Temp Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPortPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n");
cliPortPrint("'d' Accel Bias and SF Calibration 'D' Set kpMag/kiMag DkpMag;kiMag\n");
cliPortPrint("'e' Toggle External HMC5883 State 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPortPrint("'f' Toggle External MS5611 State 'F' Set Sensor Orientation F1 thru 4\n");
cliPortPrint("'g' Toggle MXR9150 Use\n");
cliPortPrint(" 'N' Set Voltage Monitor Trip Points Nlow;veryLow;maxLow\n");
cliPortPrint("'v' Toggle Vertical Velocity Hold Only 'V' Set Voltage Monitor Parameters Vscale;bias;cells\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Sensor CLI '?' Command Summary\n");
break;
///////////////////////////
}
}
}
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();
}
void systemInit(void)
{
// 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
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_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); // PWM Servo Out 1 & 2
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); // PWM ESC Out 3 & 4
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); // PWM ESC Out 5,6,7, & 8
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 1 & 2
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM16, ENABLE); // PPM RX
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM17, ENABLE); // Spektrum Frame Sync
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); // Telemetry
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); // Spektrum RX
///////////////////////////////////////////////////////////////////////////
checkFirstTime(false);
readEEPROM();
if (eepromConfig.receiverType == SPEKTRUM)
checkSpektrumBind();
checkResetType();
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority
initMixer();
cliInit();
gpioInit();
telemetryInit();
adcInit();
LED0_OFF;
delay(20000); // 20 sec total delay for sensor stabilization - probably not long enough.....
LED0_ON;
batteryInit();
pwmServoInit(eepromConfig.servoPwmRate);
if (eepromConfig.receiverType == SPEKTRUM)
spektrumInit();
else
ppmRxInit();
spiInit(SPI2);
timingFunctionsInit();
initFirstOrderFilter();
initPID();
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;
}
////////////////////////////////
}
///////////////////////////////////////////////////////////////////////////
}
