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 mixerCLI()
{
float tempFloat;
uint8_t index;
uint8_t rows, columns;
uint8_t mixerQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Mixer CLI....\n\n");
while(true)
{
cliPortPrint("Mixer CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
mixerQuery = cliPortRead();
cliPortPrint("\n");
switch(mixerQuery)
{
///////////////////////////
case 'a': // Mixer Configuration
cliPortPrint("\nMixer Configuration: ");
switch (eepromConfig.mixerConfiguration)
{
case MIXERTYPE_TRI:
cliPortPrint(" MIXERTYPE TRI\n");
break;
case MIXERTYPE_QUADX:
cliPortPrint("MIXERTYPE QUAD X\n");
break;
case MIXERTYPE_HEX6X:
cliPortPrint(" MIXERTYPE HEX X\n");
break;
case MIXERTYPE_FREE:
cliPortPrint(" MIXERTYPE FREE\n");
break;
}
cliPortPrintF("Number of Motors: %1d\n", numberMotor);
cliPortPrintF("ESC PWM Rate: %3ld\n", eepromConfig.escPwmRate);
cliPortPrintF("Servo PWM Rate: %3ld\n", eepromConfig.servoPwmRate);
if (eepromConfig.yawDirection == 1.0f)
cliPortPrintF("Yaw Direction: Normal\n\n");
else if (eepromConfig.yawDirection == -1.0f)
cliPortPrintF("Yaw Direction: Reverse\n\n");
else
cliPortPrintF("Yaw Direction: Undefined\n\n");
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
cliPortPrintF("TriCopter Yaw Servo PWM Rate: %3ld\n", eepromConfig.triYawServoPwmRate);
cliPortPrintF("TriCopter Yaw Servo Min PWM: %4ld\n", (uint16_t)eepromConfig.triYawServoMin);
cliPortPrintF("TriCopter Yaw Servo Mid PWM: %4ld\n", (uint16_t)eepromConfig.triYawServoMid);
cliPortPrintF("TriCopter Yaw Servo Max PWM: %4ld\n\n", (uint16_t)eepromConfig.triYawServoMax);
cliPortPrintF("Tricopter Yaw Cmd Time Constant: %5.3f\n\n", eepromConfig.triCopterYawCmd500HzLowPassTau);
}
if (eepromConfig.mixerConfiguration == MIXERTYPE_FREE)
{
cliPortPrintF("\nNumber of Free Mixer Motors: %1d\n Roll Pitch Yaw\n\n", eepromConfig.freeMixMotors);
for ( index = 0; index < eepromConfig.freeMixMotors; index++ )
{
cliPortPrintF("Motor%1d %6.3f %6.3f %6.3f\n", index,
eepromConfig.freeMix[index][ROLL ],
eepromConfig.freeMix[index][PITCH],
eepromConfig.freeMix[index][YAW ]);
}
cliPortPrint("\n");
}
validQuery = false;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Mixer CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Read Mixer Configuration
eepromConfig.mixerConfiguration = (uint8_t)readFloatCLI();
initMixer();
pwmEscInit();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Read ESC and Servo PWM Update Rates
eepromConfig.escPwmRate = (uint16_t)readFloatCLI();
eepromConfig.servoPwmRate = (uint16_t)readFloatCLI();
pwmEscInit();
pwmServoInit();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // Read yaw direction
tempFloat = readFloatCLI();
if (tempFloat >= 0.0)
tempFloat = 1.0;
else
tempFloat = -1.0;
eepromConfig.yawDirection = tempFloat;
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // Read TriCopter Servo PWM Rate
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triYawServoPwmRate = (uint16_t)readFloatCLI();
pwmEscInit();
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Read TriCopter Servo Min Point
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triYawServoMin = readFloatCLI();
pwmEscWrite(7, (uint16_t)eepromConfig.triYawServoMin);
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'G': // Read TriCopter Servo Mid Point
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triYawServoMid = readFloatCLI();
pwmEscWrite(7, (uint16_t)eepromConfig.triYawServoMid);
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'H': // Read TriCopter Servo Max Point
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triYawServoMax = readFloatCLI();
pwmEscWrite(7, (uint16_t)eepromConfig.triYawServoMax);
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'I': // Read TriCopter Yaw Command Time Constant
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triCopterYawCmd500HzLowPassTau = readFloatCLI();
initFirstOrderFilter();
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'J': // Read Free Mix Motor Number
if (eepromConfig.mixerConfiguration == MIXERTYPE_FREE)
{
eepromConfig.freeMixMotors = (uint8_t)readFloatCLI();
initMixer();
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nFree Mix not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'K': // Read Free Mix Matrix Element
if (eepromConfig.mixerConfiguration == MIXERTYPE_FREE)
{
rows = (uint8_t)readFloatCLI() - 1;
columns = (uint8_t)readFloatCLI() - 1;
eepromConfig.freeMix[rows][columns] = readFloatCLI();
}
else
{
tempFloat = readFloatCLI();
tempFloat = readFloatCLI();
tempFloat = readFloatCLI();
cliPortPrintF("\nFree Mix not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Mixer Configuration Data 'A' Set Mixer Configuration A0 thru 3, see aq32Plus.h\n");
cliPortPrint(" 'B' Set PWM Rates BESC;Servo\n");
cliPortPrint(" 'D' Set Yaw Direction D1 or D-1\n");
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
cliPortPrint(" 'E' Set TriCopter Servo PWM Rate ERate\n");
cliPortPrint(" 'F' Set TriCopter Servo Min Point FMin\n");
cliPortPrint(" 'G' Set TriCopter Servo Mid Point GMid\n");
cliPortPrint(" 'H' Set TriCopter Servo Max Point HMax\n");
cliPortPrint(" 'I' Set TriCopter Yaw Cmd Time Constant ITimeConstant\n");
}
if (eepromConfig.mixerConfiguration == MIXERTYPE_FREE)
{
cliPortPrint(" 'J' Set Number of FreeMix Motors JNumb\n");
cliPortPrint(" 'K' Set FreeMix Matrix Element KRow;Col;Value\n");
}
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Mixer CLI '?' Command Summary\n\n");
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 mixerCLI()
{
float tempFloat;
uint8_t index;
uint8_t rows, columns;
uint8_t mixerQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Mixer CLI....\n\n");
while(true)
{
cliPrint("Mixer CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
mixerQuery = cliRead();
cliPrint("\n");
switch(mixerQuery)
{
///////////////////////////
case 'a': // Mixer Configuration
cliPrint("\nMixer Configuration: ");
switch (eepromConfig.mixerConfiguration)
{
case MIXERTYPE_GIMBAL:
cliPrint("MIXERTYPE GIMBAL\n");
break;
///////////////////////
case MIXERTYPE_FLYING_WING:
cliPrint("MIXERTYPE FLYING WING\n");
break;
///////////////////////
case MIXERTYPE_BI:
cliPrint("MIXERTYPE BICOPTER\n");
break;
///////////////////////
case MIXERTYPE_TRI:
cliPrint("MIXERTYPE TRICOPTER\n");
break;
///////////////////////
case MIXERTYPE_QUADP:
cliPrint("MIXERTYPE QUAD PLUS\n");
break;
case MIXERTYPE_QUADX:
cliPrint("MIXERTYPE QUAD X\n");
break;
case MIXERTYPE_VTAIL4_NO_COMP:
cliPrint("MULTITYPE VTAIL NO COMP\n");
break;
case MIXERTYPE_VTAIL4_Y_COMP:
cliPrint("MULTITYPE VTAIL Y COMP\n");
break;
case MIXERTYPE_VTAIL4_RY_COMP:
cliPrint("MULTITYPE VTAIL RY COMP\n");
break;
case MIXERTYPE_VTAIL4_PY_COMP:
cliPrint("MULTITYPE VTAIL PY COMP\n");
break;
case MIXERTYPE_VTAIL4_RP_COMP:
cliPrint("MULTITYPE VTAIL RP COMP\n");
break;
case MIXERTYPE_VTAIL4_RPY_COMP:
cliPrint("MULTITYPE VTAIL RPY COMP\n");
break;
case MIXERTYPE_Y4:
cliPrint("MIXERTYPE Y4\n");
break;
///////////////////////
case MIXERTYPE_HEX6P:
cliPrint("MIXERTYPE HEX PLUS\n");
break;
case MIXERTYPE_HEX6X:
cliPrint("MIXERTYPE HEX X\n");
break;
case MIXERTYPE_Y6:
cliPrint("MIXERTYPE Y6\n");
break;
///////////////////////
case MIXERTYPE_OCTOF8P:
cliPrint("MIXERTYPE FLAT OCTO PLUS\n");
break;
case MIXERTYPE_OCTOF8X:
cliPrint("MIXERTYPE FLAT OCTO X\n");
break;
case MIXERTYPE_OCTOX8P:
cliPrint("MIXERTYPE COAXIAL OCTO PLUS\n");
break;
case MIXERTYPE_OCTOX8X:
cliPrint("MIXERTYPE COAXIAL OCTO X\n");
break;
///////////////////////
case MIXERTYPE_FREEMIX:
cliPrint("MIXERTYPE FREE MIX\n");
break;
}
cliPrintF("Number of Motors: %1d\n", numberMotor);
cliPrintF("ESC PWM Rate: %3ld\n", eepromConfig.escPwmRate);
cliPrintF("Servo PWM Rate: %3ld\n", eepromConfig.servoPwmRate);
if ( eepromConfig.mixerConfiguration == MIXERTYPE_BI )
{
cliPrintF("BiCopter Left Servo Min: %4ld\n", (uint16_t)eepromConfig.biLeftServoMin);
cliPrintF("BiCopter Left Servo Mid: %4ld\n", (uint16_t)eepromConfig.biLeftServoMid);
cliPrintF("BiCopter Left Servo Max: %4ld\n", (uint16_t)eepromConfig.biLeftServoMax);
cliPrintF("BiCopter Right Servo Min: %4ld\n", (uint16_t)eepromConfig.biRightServoMin);
cliPrintF("BiCopter Right Servo Mid: %4ld\n", (uint16_t)eepromConfig.biRightServoMid);
cliPrintF("BiCopter Right Servo Max: %4ld\n", (uint16_t)eepromConfig.biRightServoMax);
}
if ( eepromConfig.mixerConfiguration == MIXERTYPE_FLYING_WING )
{
cliPrintF("Roll Direction Left: %4ld\n", (uint16_t)eepromConfig.rollDirectionLeft);
cliPrintF("Roll Direction Right: %4ld\n", (uint16_t)eepromConfig.rollDirectionRight);
cliPrintF("Pitch Direction Left: %4ld\n", (uint16_t)eepromConfig.pitchDirectionLeft);
cliPrintF("Pitch Direction Right: %4ld\n", (uint16_t)eepromConfig.pitchDirectionRight);
cliPrintF("Wing Left Minimum: %4ld\n", (uint16_t)eepromConfig.wingLeftMinimum);
cliPrintF("Wing Left Maximum: %4ld\n", (uint16_t)eepromConfig.wingLeftMaximum);
cliPrintF("Wing Right Minimum: %4ld\n", (uint16_t)eepromConfig.wingRightMinimum);
cliPrintF("Wing Right Maximum: %4ld\n", (uint16_t)eepromConfig.wingRightMaximum);
}
if ( eepromConfig.mixerConfiguration == MIXERTYPE_GIMBAL )
{
cliPrintF("Gimbal Roll Servo Min: %4ld\n", (uint16_t)eepromConfig.gimbalRollServoMin);
cliPrintF("Gimbal Roll Servo Mid: %4ld\n", (uint16_t)eepromConfig.gimbalRollServoMid);
cliPrintF("Gimbal Roll Servo Max: %4ld\n", (uint16_t)eepromConfig.gimbalRollServoMax);
cliPrintF("Gimbal Roll Servo Gain: %7.3f\n", eepromConfig.gimbalRollServoGain);
cliPrintF("Gimbal Pitch Servo Min: %4ld\n", (uint16_t)eepromConfig.gimbalPitchServoMin);
cliPrintF("Gimbal Pitch Servo Mid: %4ld\n", (uint16_t)eepromConfig.gimbalPitchServoMid);
cliPrintF("Gimbal Pitch Servo Max: %4ld\n", (uint16_t)eepromConfig.gimbalPitchServoMax);
cliPrintF("Gimbal Pitch Servo Gain: %7.3f\n", eepromConfig.gimbalPitchServoGain);
}
if ( eepromConfig.mixerConfiguration == MIXERTYPE_TRI )
{
cliPrintF("TriCopter Yaw Servo Min: %4ld\n", (uint16_t)eepromConfig.triYawServoMin);
cliPrintF("TriCopter Yaw Servo Mid: %4ld\n", (uint16_t)eepromConfig.triYawServoMid);
cliPrintF("TriCopter Yaw Servo Max: %4ld\n", (uint16_t)eepromConfig.triYawServoMax);
}
if (eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_Y_COMP ||
eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_RY_COMP ||
eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_PY_COMP ||
eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_RP_COMP ||
eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_RPY_COMP)
{
cliPrintF("V Tail Angle %6.2f\n", eepromConfig.vTailAngle);
}
cliPrintF("Yaw Direction: %2d\n\n", (uint16_t)eepromConfig.yawDirection);
validQuery = false;
break;
///////////////////////////
case 'b': // Free Mix Matrix
cliPrintF("\nNumber of Free Mixer Motors: %1d\n Roll Pitch Yaw\n", eepromConfig.freeMixMotors);
for ( index = 0; index < eepromConfig.freeMixMotors; index++ )
{
cliPrintF("Motor%1d %6.3f %6.3f %6.3f\n", index,
eepromConfig.freeMix[index][ROLL ],
eepromConfig.freeMix[index][PITCH],
eepromConfig.freeMix[index][YAW ]);
}
cliPrint("\n");
validQuery = false;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Mixer CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Read Mixer Configuration
eepromConfig.mixerConfiguration = (uint8_t)readFloatCLI();
initMixer();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Read ESC and Servo PWM Update Rates
eepromConfig.escPwmRate = (uint16_t)readFloatCLI();
eepromConfig.servoPwmRate = (uint16_t)readFloatCLI();
pwmEscInit(eepromConfig.escPwmRate);
pwmServoInit(eepromConfig.servoPwmRate);
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // Read BiCopter Left Servo Parameters
eepromConfig.biLeftServoMin = readFloatCLI();
eepromConfig.biLeftServoMid = readFloatCLI();
eepromConfig.biLeftServoMax = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // Read BiCopter Right Servo Parameters
eepromConfig.biRightServoMin = readFloatCLI();
eepromConfig.biRightServoMid = readFloatCLI();
eepromConfig.biRightServoMax = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // Read Flying Wing Servo Directions
eepromConfig.rollDirectionLeft = readFloatCLI();
eepromConfig.rollDirectionRight = readFloatCLI();
eepromConfig.pitchDirectionLeft = readFloatCLI();
eepromConfig.pitchDirectionRight = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Read Flying Wing Servo Limits
eepromConfig.wingLeftMinimum = readFloatCLI();
eepromConfig.wingLeftMaximum = readFloatCLI();
eepromConfig.wingRightMinimum = readFloatCLI();
eepromConfig.wingRightMaximum = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'G': // Read Free Mix Motor Number
eepromConfig.freeMixMotors = (uint8_t)readFloatCLI();
initMixer();
mixerQuery = 'b';
validQuery = true;
break;
///////////////////////////
case 'H': // Read Free Mix Matrix Element
rows = (uint8_t)readFloatCLI();
columns = (uint8_t)readFloatCLI();
eepromConfig.freeMix[rows][columns] = readFloatCLI();
mixerQuery = 'b';
validQuery = true;
break;
///////////////////////////
case 'I': // Read Gimbal Roll Servo Parameters
eepromConfig.gimbalRollServoMin = readFloatCLI();
eepromConfig.gimbalRollServoMid = readFloatCLI();
eepromConfig.gimbalRollServoMax = readFloatCLI();
eepromConfig.gimbalRollServoGain = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'J': // Read Gimbal Pitch Servo Parameters
eepromConfig.gimbalPitchServoMin = readFloatCLI();
eepromConfig.gimbalPitchServoMid = readFloatCLI();
eepromConfig.gimbalPitchServoMax = readFloatCLI();
eepromConfig.gimbalPitchServoGain = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'K': // Read TriCopter YawServo Parameters
eepromConfig.triYawServoMin = readFloatCLI();
eepromConfig.triYawServoMid = readFloatCLI();
eepromConfig.triYawServoMax = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'L': // Read V Tail Angle
eepromConfig.vTailAngle = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'M': // Read yaw direction
tempFloat = readFloatCLI();
if (tempFloat >= 0.0)
tempFloat = 1.0;
else
tempFloat = -1.0;
eepromConfig.yawDirection = tempFloat;
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Mixer Configuration Data 'A' Set Mixer Configuration A1 thru 21, see aq32Plus.h\n");
cliPrint("'b' Free Mixer Configuration 'B' Set PWM Rates BESC;Servo\n");
cliPrint(" 'C' Set BiCopter Left Servo Parameters CMin;Mid;Max\n");
cliPrint(" 'D' Set BiCopter Right Servo Parameters DMin;Mid;Max\n");
cliPrint(" 'E' Set Flying Wing Servo Directions ERollLeft;RollRight;PitchLeft;PitchRight\n");
cliPrint(" 'F' Set Flying Wing Servo Limits FLeftMin;LeftMax;RightMin;RightMax\n");
cliPrint(" 'G' Set Number of FreeMix Motors GNumber\n");
cliPrint(" 'H' Set FreeMix Matrix Element HRow;Column;Element\n");
cliPrint(" 'I' Set Gimbal Roll Servo Parameters IMin;Mid;Max;Gain\n");
cliPrint(" 'J' Set Gimbal Pitch Servo Parameters JMin;Mid;Max;Gain\n");
cliPrint(" 'K' Set TriCopter Servo Parameters KMin;Mid;Max\n");
cliPrint(" 'L' Set V Tail Angle LAngle\n");
cliPrint(" 'M' Set Yaw Direction M1 or M-1\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPrint("\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();
}
void mixerCLI()
{
float tempFloat;
uint8_t mixerQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Mixer CLI....\n\n");
while(true)
{
cliPrint("Mixer CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
mixerQuery = cliRead();
cliPrint("\n");
switch(mixerQuery)
{
///////////////////////////
case 'a': // Mixer Configuration
cliPrint("\nMixer Configuration: ");
switch (eepromConfig.mixerConfiguration)
{
case MIXERTYPE_QUADX:
cliPrint("MIXERTYPE QUAD X\n");
break;
case MIXERTYPE_HEX6X:
cliPrint(" MIXERTYPE HEX X\n");
break;
}
cliPrintF("Number of Motors: %1d\n", numberMotor);
cliPrintF("ESC PWM Rate: %3ld\n", eepromConfig.escPwmRate);
cliPrintF("Servo PWM Rate: %3ld\n\n", eepromConfig.servoPwmRate);
validQuery = false;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Mixer CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Read Mixer Configuration
eepromConfig.mixerConfiguration = (uint8_t)readFloatCLI();
initMixer();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Read ESC and Servo PWM Update Rates
eepromConfig.escPwmRate = (uint16_t)readFloatCLI();
eepromConfig.servoPwmRate = (uint16_t)readFloatCLI();
pwmEscInit(eepromConfig.escPwmRate);
pwmServoInit(eepromConfig.servoPwmRate);
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'M': // Read yaw direction
tempFloat = readFloatCLI();
if (tempFloat >= 0.0)
tempFloat = 1.0;
else
tempFloat = -1.0;
eepromConfig.yawDirection = tempFloat;
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Mixer Configuration Data 'A' Set Mixer Configuration A1 thru 21, see aq32Plus.h\n");
cliPrint("'b' Free Mixer Configuration 'B' Set PWM Rates BESC;Servo\n");
cliPrint(" 'C' Set BiCopter Left Servo Parameters CMin;Mid;Max\n");
cliPrint(" 'D' Set BiCopter Right Servo Parameters DMin;Mid;Max\n");
cliPrint(" 'E' Set Flying Wing Servo Directions ERollLeft;RollRight;PitchLeft;PitchRight\n");
cliPrint(" 'F' Set Flying Wing Servo Limits FLeftMin;LeftMax;RightMin;RightMax\n");
cliPrint(" 'G' Set Number of FreeMix Motors GNumber\n");
cliPrint(" 'H' Set FreeMix Matrix Element HRow;Column;Element\n");
cliPrint(" 'I' Set Gimbal Roll Servo Parameters IMin;Mid;Max;Gain\n");
cliPrint(" 'J' Set Gimbal Pitch Servo Parameters JMin;Mid;Max;Gain\n");
cliPrint(" 'K' Set TriCopter Servo Parameters KMin;Mid;Max\n");
cliPrint(" 'L' Set V Tail Angle LAngle\n");
cliPrint(" 'M' Set Yaw Direction M1 or M-1\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////////
}
}
}
