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 systemInit(void) { GPIO_InitTypeDef GPIO_InitStructure; uint8_t i; gpio_config_t gpio_cfg[] = { {LED0_GPIO, LED0_PIN, GPIO_Mode_Out_PP}, // PB3 (LED) {LED1_GPIO, LED1_PIN, GPIO_Mode_Out_PP}, // PB4 (LED) }; uint8_t gpio_count = sizeof(gpio_cfg) / sizeof(gpio_cfg[0]); // 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_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, 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); // Configure gpio for (i = 0; i < gpio_count; i++) { GPIO_InitStructure.GPIO_Pin = gpio_cfg[i].pin; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = gpio_cfg[i].mode; GPIO_Init(gpio_cfg[i].gpio, &GPIO_InitStructure); } // Init cycle counter cycleCounterInit(); // SysTick SysTick_Config(SystemCoreClock / 1000); LED0_OFF; LED1_OFF; NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority checkFirstTime(false, false); readEEPROM(); initMixer(); pwmOutputConfig.escPwmRate = systemConfig.escPwmRate; pwmOutputConfig.servoPwmRate = systemConfig.servoPwmRate; //i2cInit(SENSOR_I2C); pwmInputInit(); pwmOutputInit(&pwmOutputConfig); uartInit(115200); //delay(10000); // 10 sec delay for sensor stabilization - probably not long enough..... //initAccel(); //initGyro(); //initMag(); adcInit(); //initPressure(); // no pressure sensor initPID(); }
void mixerCLI() { float tempFloat; uint8_t index; uint8_t rows, columns; uint8_t mixerQuery; 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_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(); pwmOutputConfig.escPwmRate = eepromConfig.escPwmRate; pwmOutputConfig.servoPwmRate = eepromConfig.servoPwmRate; pwmOutputInit(&pwmOutputConfig); 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 17, see baseFlightPlus.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 testInit(void) { GPIO_InitTypeDef GPIO_InitStructure; uint8_t i; struct __gpio_config_t { GPIO_TypeDef *gpio; uint16_t pin; GPIOMode_TypeDef mode; } gpio_cfg[] = { {LED0_GPIO, LED0_PIN, GPIO_Mode_Out_PP}, // PB3 (LED) {LED1_GPIO, LED1_PIN, GPIO_Mode_Out_PP}, // PB4 (LED) }; uint8_t gpio_count = sizeof(gpio_cfg) / sizeof(gpio_cfg[0]); // 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_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, 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); // Configure gpio for (i = 0; i < gpio_count; i++) { GPIO_InitStructure.GPIO_Pin = gpio_cfg[i].pin; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_Mode = gpio_cfg[i].mode; GPIO_Init(gpio_cfg[i].gpio, &GPIO_InitStructure); } // Init cycle counter cycleCounterInit(); // SysTick SysTick_Config(SystemCoreClock / 1000); LED0_OFF; LED1_OFF; NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 2 bits for pre-emption priority, 2 bits for subpriority checkFirstTime(true,true); initMixer(); pwmOutputConfig.motorPwmRate = 10*1000; pwmOutputConfig.noEsc = true; pwmOutputInit(&pwmOutputConfig); i2cInit(SENSOR_I2C); initGyro(); initAccel(); initMag(); nrf_init(); nrf_detect(); }
void pwm_mode_set(void) { pwmOutputInit(&pwmOutputConfig); }