void serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { bool enabled = false; switch (rxConfig->serialrx_provider) { #ifdef USE_SERIALRX_SPEKTRUM case SERIALRX_SPEKTRUM1024: enabled = spektrumInit(rxConfig, rxRuntimeConfig); break; case SERIALRX_SPEKTRUM2048: enabled = spektrumInit(rxConfig, rxRuntimeConfig); break; #endif #ifdef USE_SERIALRX_SBUS case SERIALRX_SBUS: enabled = sbusInit(rxConfig, rxRuntimeConfig); break; #endif #ifdef USE_SERIALRX_SUMD case SERIALRX_SUMD: enabled = sumdInit(rxConfig, rxRuntimeConfig); break; #endif #ifdef USE_SERIALRX_SUMH case SERIALRX_SUMH: enabled = sumhInit(rxConfig, rxRuntimeConfig); break; #endif #ifdef USE_SERIALRX_XBUS case SERIALRX_XBUS_MODE_B: case SERIALRX_XBUS_MODE_B_RJ01: enabled = xBusInit(rxConfig, rxRuntimeConfig); break; #endif #ifdef USE_SERIALRX_IBUS case SERIALRX_IBUS: enabled = ibusInit(rxConfig, rxRuntimeConfig); break; #endif #ifdef USE_SERIALRX_JETIEXBUS case SERIALRX_JETIEXBUS: enabled = jetiExBusInit(rxConfig, rxRuntimeConfig); break; #endif default: enabled = false; break; } if (!enabled) { featureClear(FEATURE_RX_SERIAL); rxRuntimeConfig->rcReadRawFn = nullReadRawRC; } }
void serialRxInit(rxConfig_t *rxConfig) { bool enabled = false; switch (rxConfig->serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_SBUS: enabled = sbusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_SUMD: enabled = sumdInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_SUMH: enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_XBUS_MODE_B: case SERIALRX_XBUS_MODE_B_RJ01: enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; } if (!enabled) { featureClear(FEATURE_RX_SERIAL); rcReadRawFunc = nullReadRawRC; } }
void serialRxInit(rxConfig_t *rxConfig) { bool enabled = false; switch (rxConfig->serialrx_provider) { case SERIALRX_SPEKTRUM1024: rxRefreshRate = 22000; enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_SPEKTRUM2048: rxRefreshRate = 11000; enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_SBUS: rxRefreshRate = 11000; enabled = sbusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_SUMD: rxRefreshRate = 11000; enabled = sumdInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_SUMH: rxRefreshRate = 11000; enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_XBUS_MODE_B: case SERIALRX_XBUS_MODE_B_RJ01: rxRefreshRate = 11000; enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_IBUS: rxRefreshRate = 20000; // TODO - Verify speed enabled = ibusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; } if (!enabled) { featureClear(FEATURE_RX_SERIAL); rcReadRawFunc = nullReadRawRC; } }
void receiverCLI() { char rcOrderString[9]; float tempFloat; uint8_t index; uint8_t receiverQuery = 'x'; uint8_t validQuery = false; NVIC_InitTypeDef NVIC_InitStructure; cliBusy = true; cliPortPrint("\nEntering Receiver CLI....\n\n"); while(true) { cliPortPrint("Receiver CLI -> "); while ((cliPortAvailable() == false) && (validQuery == false)); if (validQuery == false) receiverQuery = cliPortRead(); cliPortPrint("\n"); switch(receiverQuery) { /////////////////////////// case 'a': // Receiver Configuration cliPortPrint("\nReceiver Type: "); switch(systemConfig.receiverType) { case PPM: cliPortPrint("PPM\n"); break; case SPEKTRUM: cliPortPrint("Spektrum\n"); break; } cliPortPrint("Current RC Channel Assignment: "); for (index = 0; index < 8; index++) rcOrderString[systemConfig.rcMap[index]] = rcChannelLetters[index]; rcOrderString[index] = '\0'; cliPortPrint(rcOrderString); cliPortPrint("\n"); cliPortPrintF("Secondary Spektrum: "); if ((systemConfig.slaveSpektrum == true) && false) // HJI Inhibit Slave Spektrum on Naze32 Pro cliPortPrintF("Installed\n"); else cliPortPrintF("Uninstalled\n"); cliPortPrintF("Mid Command: %4ld\n", (uint16_t)systemConfig.midCommand); cliPortPrintF("Min Check: %4ld\n", (uint16_t)systemConfig.minCheck); cliPortPrintF("Max Check: %4ld\n", (uint16_t)systemConfig.maxCheck); cliPortPrintF("Min Throttle: %4ld\n", (uint16_t)systemConfig.minThrottle); cliPortPrintF("Max Thottle: %4ld\n\n", (uint16_t)systemConfig.maxThrottle); tempFloat = systemConfig.rollAndPitchRateScaling * 180000.0 / PI; cliPortPrintF("Max Roll and Pitch Rate Cmd: %6.2f DPS\n", tempFloat); tempFloat = systemConfig.yawRateScaling * 180000.0 / PI; cliPortPrintF("Max Yaw Rate Cmd: %6.2f DPS\n\n", tempFloat); cliPortPrintF("Roll Rate Cmd Tau: %6.2f\n", systemConfig.rollRateCmdLowPassTau); cliPortPrintF("Pitch Rate Cmd Tau: %6.2f\n\n", systemConfig.pitchRateCmdLowPassTau); tempFloat = systemConfig.attitudeScaling * 180000.0 / PI; cliPortPrintF("Max Attitude Cmd: %6.2f Degrees\n\n", tempFloat); cliPortPrintF("Roll Attitude Cmd Tau: %6.2f\n", systemConfig.rollAttCmdLowPassTau); cliPortPrintF("Pitch Attitude Cmd Tau: %6.2f\n\n", systemConfig.pitchAttCmdLowPassTau); cliPortPrintF("Arm Delay Count: %3d Frames\n", systemConfig.armCount); cliPortPrintF("Disarm Delay Count: %3d Frames\n\n", systemConfig.disarmCount); validQuery = false; break; /////////////////////////// case 'x': cliPortPrint("\nExiting Receiver CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'A': // Toggle PPM/Spektrum Satellite Receiver if (systemConfig.receiverType == PPM) { NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE; NVIC_Init(&NVIC_InitStructure); TIM_ITConfig(TIM1, TIM_IT_CC1, DISABLE); systemConfig.receiverType = SPEKTRUM; spektrumInit(); } else { NVIC_InitStructure.NVIC_IRQChannel = TIM1_TRG_COM_TIM17_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE; NVIC_Init(&NVIC_InitStructure); TIM_ITConfig(TIM17, TIM_IT_Update, DISABLE); systemConfig.receiverType = PPM; ppmRxInit(); } receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'B': // Read RC Control Order readStringCLI( rcOrderString, 8 ); parseRcChannels( rcOrderString ); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'C': // Toggle Slave Spektrum State // HJI Inhibit Slave Spektrum on Naze32 Pro if (systemConfig.slaveSpektrum == true) systemConfig.slaveSpektrum = false; // HJI Inhibit Slave Spektrum on Naze32 Pro else // HJI Inhibit Slave Spektrum on Naze32 Pro systemConfig.slaveSpektrum = true; receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'D': // Read RC Control Points systemConfig.midCommand = readFloatCLI(); systemConfig.minCheck = readFloatCLI(); systemConfig.maxCheck = readFloatCLI(); systemConfig.minThrottle = readFloatCLI(); systemConfig.maxThrottle = readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'E': // Read Arm/Disarm Counts systemConfig.armCount = (uint8_t)readFloatCLI(); systemConfig.disarmCount = (uint8_t)readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'F': // Read Max Rate Value systemConfig.rollAndPitchRateScaling = readFloatCLI() / 180000.0f * PI; systemConfig.yawRateScaling = readFloatCLI() / 180000.0f * PI; receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'G': // Read Max Attitude Value systemConfig.attitudeScaling = readFloatCLI() / 180000.0f * PI; receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'H': // Read Rate Cmd Tau Value systemConfig.rollRateCmdLowPassTau = readFloatCLI(); systemConfig.pitchRateCmdLowPassTau = readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'I': // Read Attitude Cmd Tau Value systemConfig.rollAttCmdLowPassTau = readFloatCLI(); systemConfig.pitchAttCmdLowPassTau = readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'W': // Write System EEPROM Parameters cliPortPrint("\nWriting System EEPROM Parameters....\n\n"); writeSystemEEPROM(); validQuery = false; break; /////////////////////////// case '?': cliPortPrint("\n"); cliPortPrint("'a' Receiver Configuration Data 'A' Toggle PPM/Spektrum Receiver\n"); cliPortPrint(" 'B' Set RC Control Order BTAER1234\n"); cliPortPrint(" 'C' Toggle Slave Spektrum State\n"); cliPortPrint(" 'D' Set RC Control Points DmidCmd;minChk;maxChk;minThrot;maxThrot\n"); cliPortPrint(" 'E' Set Arm/Disarm Counts EarmCount;disarmCount\n"); cliPortPrint(" 'F' Set Maximum Rate Commands FRP;Y RP = Roll/Pitch, Y = Yaw\n"); cliPortPrint(" 'G' Set Maximum Attitude Command\n"); cliPortPrint(" 'H' Set Roll/Pitch Rate Command Filters HROLL;PITCH\n"); cliPortPrint(" 'I' Set Roll/Pitch Att Command Filters IROLL;PITCH\n"); cliPortPrint(" 'W' Write System EEPROM Parameters\n"); cliPortPrint("'x' Exit Receiver CLI '?' Command Summary\n"); cliPortPrint("\n"); break; /////////////////////////// } } }
int main(void) { uint8_t i; drv_pwm_config_t pwm_params; drv_adc_config_t adc_params; bool sensorsOK = false; #ifdef SOFTSERIAL_LOOPBACK serialPort_t* loopbackPort1 = NULL; serialPort_t* loopbackPort2 = NULL; #endif initEEPROM(); checkFirstTime(false); readEEPROM(); systemInit(mcfg.emf_avoidance); #ifdef USE_LAME_PRINTF init_printf(NULL, _putc); #endif activateConfig(); // configure power ADC if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9)) adc_params.powerAdcChannel = mcfg.power_adc_channel; else { adc_params.powerAdcChannel = 0; mcfg.power_adc_channel = 0; } adcInit(&adc_params); // Check battery type/voltage if (feature(FEATURE_VBAT)) batteryInit(); initBoardAlignment(); // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform sensorsSet(SENSORS_SET); // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead. sensorsOK = sensorsAutodetect(); // production debug output #ifdef PROD_DEBUG productionDebug(); #endif // if gyro was not detected due to whatever reason, we give up now. if (!sensorsOK) failureMode(3); LED1_ON; LED0_OFF; for (i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; imuInit(); // Mag is initialized inside imuInit mixerInit(); // this will set core.useServo var depending on mixer type serialInit(mcfg.serial_baudrate); // when using airplane/wing mixer, servo/motor outputs are remapped if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING) pwm_params.airplane = true; else pwm_params.airplane = false; pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.usePPM = feature(FEATURE_PPM); pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum pwm_params.useServos = core.useServo; pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.motorPwmRate = mcfg.motor_pwm_rate; pwm_params.servoPwmRate = mcfg.servo_pwm_rate; pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) if (feature(FEATURE_3D)) pwm_params.idlePulse = mcfg.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwm_params.servoCenterPulse = mcfg.midrc; pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold; switch (mcfg.power_adc_channel) { case 1: pwm_params.adcChannel = PWM2; break; case 9: pwm_params.adcChannel = PWM8; break; default: pwm_params.adcChannel = 0; break; } pwmInit(&pwm_params); core.numServos = pwm_params.numServos; // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled for (i = 0; i < RC_CHANS; i++) rcData[i] = 1502; rcReadRawFunc = pwmReadRawRC; core.numRCChannels = MAX_INPUTS; if (feature(FEATURE_SERIALRX)) { switch (mcfg.serialrx_type) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: spektrumInit(&rcReadRawFunc); break; case SERIALRX_SBUS: sbusInit(&rcReadRawFunc); break; case SERIALRX_SUMD: sumdInit(&rcReadRawFunc); break; case SERIALRX_MSP: mspInit(&rcReadRawFunc); break; } } else { // spektrum and GPS are mutually exclusive // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2. // gpsInit will return if FEATURE_GPS is not enabled. gpsInit(mcfg.gps_baudrate); } #ifdef SONAR // sonar stuff only works with PPM if (feature(FEATURE_PPM)) { if (feature(FEATURE_SONAR)) Sonar_init(); } #endif if (feature(FEATURE_SOFTSERIAL)) { //mcfg.softserial_baudrate = 19200; // Uncomment to override config value setupSoftSerialPrimary(mcfg.softserial_baudrate, mcfg.softserial_1_inverted); setupSoftSerialSecondary(mcfg.softserial_2_inverted); #ifdef SOFTSERIAL_LOOPBACK loopbackPort1 = (serialPort_t*)&(softSerialPorts[0]); serialPrint(loopbackPort1, "SOFTSERIAL 1 - LOOPBACK ENABLED\r\n"); loopbackPort2 = (serialPort_t*)&(softSerialPorts[1]); serialPrint(loopbackPort2, "SOFTSERIAL 2 - LOOPBACK ENABLED\r\n"); #endif //core.mainport = (serialPort_t*)&(softSerialPorts[0]); // Uncomment to switch the main port to use softserial. } if (feature(FEATURE_TELEMETRY)) initTelemetry(); previousTime = micros(); if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) calibratingA = CALIBRATING_ACC_CYCLES; calibratingG = CALIBRATING_GYRO_CYCLES; calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles f.SMALL_ANGLE = 1; // loopy while (1) { loop(); #ifdef SOFTSERIAL_LOOPBACK if (loopbackPort1) { while (serialTotalBytesWaiting(loopbackPort1)) { uint8_t b = serialRead(loopbackPort1); serialWrite(loopbackPort1, b); //serialWrite(core.mainport, 0x01); //serialWrite(core.mainport, b); }; } if (loopbackPort2) { while (serialTotalBytesWaiting(loopbackPort2)) { #ifndef OLIMEXINO // PB0/D27 and PB1/D28 internally connected so this would result in a continuous stream of data serialRead(loopbackPort2); #else uint8_t b = serialRead(loopbackPort2); serialWrite(loopbackPort2, b); //serialWrite(core.mainport, 0x02); //serialWrite(core.mainport, b); #endif // OLIMEXINO }; } #endif } }
int main(void) { uint8_t i; drv_pwm_config_t pwm_params; drv_adc_config_t adc_params; #if 0 // PC12, PA15 // using this to write asm for bootloader :) RCC->APB2ENR |= RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO; // GPIOA/C+AFIO only AFIO->MAPR &= 0xF0FFFFFF; AFIO->MAPR = 0x02000000; GPIOA->CRH = 0x34444444; // PIN 15 Output 50MHz GPIOA->BRR = 0x8000; // set low 15 GPIOC->CRH = 0x44434444; // PIN 12 Output 50MHz GPIOC->BRR = 0x1000; // set low 12 #endif #if 0 // using this to write asm for bootloader :) RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO AFIO->MAPR &= 0xF0FFFFFF; AFIO->MAPR = 0x02000000; GPIOB->BRR = 0x18; // set low 4 & 3 GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz #endif systemInit(); #ifdef USE_LAME_PRINTF init_printf(NULL, _putc); #endif checkFirstTime(false); readEEPROM(); // configure power ADC if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9)) adc_params.powerAdcChannel = mcfg.power_adc_channel; else { adc_params.powerAdcChannel = 0; mcfg.power_adc_channel = 0; } adcInit(&adc_params); // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform sensorsSet(SENSORS_SET); mixerInit(); // this will set useServo var depending on mixer type // when using airplane/wing mixer, servo/motor outputs are remapped if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING) pwm_params.airplane = true; else pwm_params.airplane = false; pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SPEKTRUM); // spektrum support uses UART too pwm_params.usePPM = feature(FEATURE_PPM); pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum pwm_params.useServos = useServo; pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.motorPwmRate = mcfg.motor_pwm_rate; pwm_params.servoPwmRate = mcfg.servo_pwm_rate; pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold; switch (mcfg.power_adc_channel) { case 1: pwm_params.adcChannel = PWM2; break; case 9: pwm_params.adcChannel = PWM8; break; default: pwm_params.adcChannel = 0; break; } pwmInit(&pwm_params); // configure PWM/CPPM read function. spektrum below will override that rcReadRawFunc = pwmReadRawRC; if (feature(FEATURE_SPEKTRUM)) { spektrumInit(); rcReadRawFunc = spektrumReadRawRC; } else { // spektrum and GPS are mutually exclusive // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2. if (feature(FEATURE_GPS)) gpsInit(mcfg.gps_baudrate); } #ifdef SONAR // sonar stuff only works with PPM if (feature(FEATURE_PPM)) { if (feature(FEATURE_SONAR)) Sonar_init(); } #endif LED1_ON; LED0_OFF; for (i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead. sensorsAutodetect(); imuInit(); // Mag is initialized inside imuInit // Check battery type/voltage if (feature(FEATURE_VBAT)) batteryInit(); serialInit(mcfg.serial_baudrate); previousTime = micros(); if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) calibratingA = 400; calibratingG = 1000; calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles f.SMALL_ANGLES_25 = 1; // loopy while (1) { loop(); } }
int main(void) { uint8_t i; drv_pwm_config_t pwm_params; #if 0 // PC12, PA15 // using this to write asm for bootloader :) RCC->APB2ENR |= RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO; // GPIOA/C+AFIO only AFIO->MAPR &= 0xF0FFFFFF; AFIO->MAPR = 0x02000000; GPIOA->CRH = 0x34444444; // PIN 15 Output 50MHz GPIOA->BRR = 0x8000; // set low 15 GPIOC->CRH = 0x44434444; // PIN 12 Output 50MHz GPIOC->BRR = 0x1000; // set low 12 #endif #if 0 // using this to write asm for bootloader :) RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO AFIO->MAPR &= 0xF0FFFFFF; AFIO->MAPR = 0x02000000; GPIOB->BRR = 0x18; // set low 4 & 3 GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz #endif systemInit(); readEEPROM(); checkFirstTime(false); serialInit(cfg.serial_baudrate); // We have these sensors #ifndef FY90Q // AfroFlight32 sensorsSet(SENSOR_ACC | SENSOR_BARO | SENSOR_MAG); #else // FY90Q sensorsSet(SENSOR_ACC); #endif mixerInit(); // this will set useServo var depending on mixer type // when using airplane/wing mixer, servo/motor outputs are remapped if (cfg.mixerConfiguration == MULTITYPE_AIRPLANE || cfg.mixerConfiguration == MULTITYPE_FLYING_WING) pwm_params.airplane = true; pwm_params.usePPM = feature(FEATURE_PPM); pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum pwm_params.useServos = useServo; pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.motorPwmRate = cfg.motor_pwm_rate; pwm_params.servoPwmRate = cfg.servo_pwm_rate; pwmInit(&pwm_params); // configure PWM/CPPM read function. spektrum will override that rcReadRawFunc = pwmReadRawRC; LED1_ON; LED0_OFF; for (i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead. sensorsAutodetect(); imuInit(); // Mag is initialized inside imuInit // Check battery type/voltage if (feature(FEATURE_VBAT)) batteryInit(); if (feature(FEATURE_SPEKTRUM)) { spektrumInit(); rcReadRawFunc = spektrumReadRawRC; } else { // spektrum and GPS are mutually exclusive // Optional GPS - available only when using PPM, otherwise required pins won't be usable if (feature(FEATURE_PPM)) { if (feature(FEATURE_GPS)) gpsInit(cfg.gps_baudrate); #ifdef SONAR if (feature(FEATURE_SONAR)) Sonar_init(); #endif } } previousTime = micros(); if (cfg.mixerConfiguration == MULTITYPE_GIMBAL) calibratingA = 400; calibratingG = 1000; f.SMALL_ANGLES_25 = 1; // loopy while (1) { loop(); } }
int main(void) { uint8_t i; drv_pwm_config_t pwm_params; drv_adc_config_t adc_params; serialPort_t* loopbackPort = NULL; systemInit(); #ifdef USE_LAME_PRINTF init_printf(NULL, _putc); #endif checkFirstTime(false); readEEPROM(); // configure power ADC if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9)) adc_params.powerAdcChannel = mcfg.power_adc_channel; else { adc_params.powerAdcChannel = 0; mcfg.power_adc_channel = 0; } adcInit(&adc_params); initBoardAlignment(); // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform sensorsSet(SENSORS_SET); mixerInit(); // this will set core.useServo var depending on mixer type // when using airplane/wing mixer, servo/motor outputs are remapped if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING) pwm_params.airplane = true; else pwm_params.airplane = false; //pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too pwm_params.useUART = feature(FEATURE_GPS); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.usePPM = feature(FEATURE_PPM); pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum pwm_params.useServos = core.useServo; pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.motorPwmRate = mcfg.motor_pwm_rate; pwm_params.servoPwmRate = mcfg.servo_pwm_rate; pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) if (feature(FEATURE_3D)) pwm_params.idlePulse = mcfg.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwm_params.servoCenterPulse = mcfg.midrc; pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold; switch (mcfg.power_adc_channel) { case 1: pwm_params.adcChannel = PWM2; break; case 9: pwm_params.adcChannel = PWM8; break; default: pwm_params.adcChannel = 0; break; } pwmInit(&pwm_params); // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled for (i = 0; i < RC_CHANS; i++) rcData[i] = 1502; rcReadRawFunc = pwmReadRawRC; core.numRCChannels = MAX_INPUTS; if (feature(FEATURE_SERIALRX)) { switch (mcfg.serialrx_type) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: spektrumInit(&rcReadRawFunc); break; case SERIALRX_SBUS: sbusInit(&rcReadRawFunc); break; case SERIALRX_SUMD: sumdInit(&rcReadRawFunc); break; #if defined(SKYROVER) case SERIALRX_HEXAIRBOT: hexairbotInit(&rcReadRawFunc); break; #endif } } else { // spektrum and GPS are mutually exclusive // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2. // gpsInit will return if FEATURE_GPS is not enabled. // Sanity check below - protocols other than NMEA do not support baud rate autodetection if (mcfg.gps_type > 0 && mcfg.gps_baudrate < 0) mcfg.gps_baudrate = 0; gpsInit(mcfg.gps_baudrate); } #ifdef SONAR // sonar stuff only works with PPM if (feature(FEATURE_PPM)) { if (feature(FEATURE_SONAR)) Sonar_init(); } #endif LED1_ON; LED0_OFF; for (i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; serialInit(mcfg.serial_baudrate); DEBUG_PRINT("Booting..\r\n"); // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead. sensorsAutodetect(); imuInit(); // Mag is initialized inside imuInit // Check battery type/voltage if (feature(FEATURE_VBAT)) batteryInit(); if (feature(FEATURE_SOFTSERIAL)) { setupSoftSerial1(mcfg.softserial_baudrate, mcfg.softserial_inverted); #ifdef SOFTSERIAL_LOOPBACK loopbackPort = (serialPort_t*)&(softSerialPorts[0]); serialPrint(loopbackPort, "LOOPBACK ENABLED\r\n"); #endif } if (feature(FEATURE_TELEMETRY)) initTelemetry(); previousTime = micros(); if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) calibratingA = CALIBRATING_ACC_CYCLES; calibratingG = CALIBRATING_GYRO_CYCLES; calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles f.SMALL_ANGLES_25 = 1; DEBUG_PRINT("Start\r\n"); // loopy while (1) { loop(); #ifdef SOFTSERIAL_LOOPBACK if (loopbackPort) { while (serialTotalBytesWaiting(loopbackPort)) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); //serialWrite(core.mainport, b); }; } #endif } }
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 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) { // Setup pins DDRB = 0x00; // All pins inputs DDRC = 0x07; // PC0,1,2 as outputs for the LEDs DDRD = 0x0b; // PD0 as output for LED, PD1 for TXD, PD3 for buzzer PORTD = 0xe0; // Pullups for switches PORTB = 0x3f; // Pullups for switches // LEDs LEDOff(); // ADC ADMUX = 0x4f; // start off with mux on internal input ADCSRA = 0x80; // ADC EN DIDR0 = 0x38; // disable digital buffers on ADC pins // Timer TCCR0A = 0x00; // Normal mode TCCR0B = 0x03; // prescaler at 64 results in 125kHz ticks // UART cli(); // disable global interrupts stickInit(); // initialise stick input spektrumInit(); // Get startup mode trainerPlugged = TRAINERPIN; bindSwitch = BINDPIN; transmitMode = 0; if(trainerPlugged == 0) transmitMode = eTM_trainer_master; // fixme or trainer slave, if PPM signal present if(bindSwitch == 0) transmitMode = eTM_bind; // Timer loop! static uint8_t fastScaler = 255; static uint8_t slowScaler = 255; static uint16_t secondScaler = 0xffff; // Buzzer TCCR2B = 0x03; // prescaler at 64 stopNote(); noteBuffer[0] = NOTE6G; noteBuffer[1] = NOTE6GS; noteBuffer[2] = NOTESTOP; noteCounter = 0; noteInterruptable = 0; while(1) { while(TCNT0 < FASTLOOPCOUNT) { if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED0Duty/3) LED0On(); // red LED too bright, reduce duty if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED1Duty) LED1On(); if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED2Duty) LED2On(); if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED3Duty) LED3On(); } TCNT0 = 0; // reduce jitter by resetting soon (SPEKTRUM does not like jitter!) LEDOff(); // should be going at about 625Hz getDigital(); stickGetRawADC(rawstickvalues); if(++fastScaler >= OVERSAMPLE) //should be 22ms for DSMX (Nano Board can not handle it faster!!) { fastScaler = 0; // this loop runs slower than 50Hz fastLoop(); /*throVoltage = 0; ruddVoltage = 0; elevVoltage = 0; aileVoltage = 0;*/ rawstickvalues[0]=0; rawstickvalues[1]=0; rawstickvalues[2]=0; rawstickvalues[3]=0; if(++slowScaler >= 6) { // should be going at about 10Hz slowScaler = 0; slowLoop(); } } // we are here every 2ms if(++secondScaler >=500) { // every second secondScaler=0; IdleSeconds++; if(auxSwitch && mixToggle) { FlySeconds++; if(FlySeconds == FLYSECONDS_MAX) oneTone(NOTE7B); if(FlySeconds == FLYSECONDS_MAX+10) twoTone(NOTE7C); if(IdleSeconds >= FLYSECONDS_MAX*2) { twoTone(NOTE7C); } } } } }
void receiverCLI() { char rcOrderString[9]; float tempFloat; uint8_t index; uint8_t receiverQuery = 'x'; uint8_t validQuery = false; cliBusy = true; cliPortPrint("\nEntering Receiver CLI....\n\n"); while(true) { cliPortPrint("Receiver CLI -> "); while ((cliPortAvailable() == false) && (validQuery == false)); if (validQuery == false) receiverQuery = cliPortRead(); cliPortPrint("\n"); switch(receiverQuery) { /////////////////////////// case 'a': // Receiver Configuration cliPortPrint("\nReceiver Type: "); switch(eepromConfig.receiverType) { case PPM: cliPortPrint("PPM\n"); break; case SPEKTRUM: cliPortPrint("Spektrum\n"); break; } cliPortPrint("Current RC Channel Assignment: "); for (index = 0; index < 8; index++) rcOrderString[eepromConfig.rcMap[index]] = rcChannelLetters[index]; rcOrderString[index] = '\0'; cliPortPrint(rcOrderString); cliPortPrint("\n"); #if defined(STM32F40XX) cliPortPrintF("Secondary Spektrum: "); if (eepromConfig.slaveSpektrum == true) cliPortPrintF("Installed\n"); else cliPortPrintF("Uninstalled\n"); #endif cliPortPrintF("Mid Command: %4ld\n", (uint16_t)eepromConfig.midCommand); cliPortPrintF("Min Check: %4ld\n", (uint16_t)eepromConfig.minCheck); cliPortPrintF("Max Check: %4ld\n", (uint16_t)eepromConfig.maxCheck); cliPortPrintF("Min Throttle: %4ld\n", (uint16_t)eepromConfig.minThrottle); cliPortPrintF("Max Thottle: %4ld\n\n", (uint16_t)eepromConfig.maxThrottle); tempFloat = eepromConfig.rollAndPitchRateScaling * 180000.0 / PI; cliPortPrintF("Max Roll and Pitch Rate Cmd: %6.2f DPS\n", tempFloat); tempFloat = eepromConfig.yawRateScaling * 180000.0 / PI; cliPortPrintF("Max Yaw Rate Cmd: %6.2f DPS\n\n", tempFloat); tempFloat = eepromConfig.attitudeScaling * 180000.0 / PI; cliPortPrintF("Max Attitude Cmd: %6.2f Degrees\n\n", tempFloat); cliPortPrintF("Arm Delay Count: %3d Frames\n", eepromConfig.armCount); cliPortPrintF("Disarm Delay Count: %3d Frames\n\n", eepromConfig.disarmCount); validQuery = false; break; /////////////////////////// case 'x': cliPortPrint("\nExiting Receiver CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'A': // Toggle PPM/Spektrum Satellite Receiver if (eepromConfig.receiverType == PPM) { #if defined(STM32F30X) TIM_ITConfig(TIM1, TIM_IT_CC1, DISABLE); #endif #if defined(STM32F40XX) TIM_ITConfig(TIM1, TIM_IT_CC4, DISABLE); #endif eepromConfig.receiverType = SPEKTRUM; spektrumInit(); } else { #if defined(STM32F30X) TIM_ITConfig(TIM17, TIM_IT_Update, DISABLE); #endif #if defined(STM32F40XX) TIM_ITConfig(TIM6, TIM_IT_Update, DISABLE); #endif eepromConfig.receiverType = PPM; #if defined(STM32F30X) ppmRxInit(); #endif #if defined(STM32F40XX) agl_ppmRxInit(); #endif } receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'B': // Read RC Control Order readStringCLI( rcOrderString, 8 ); parseRcChannels( rcOrderString ); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// #if defined(STM32F40XX) case 'C': // Toggle Slave Spektrum State if (eepromConfig.slaveSpektrum == true) eepromConfig.slaveSpektrum = false; else eepromConfig.slaveSpektrum = true; receiverQuery = 'a'; validQuery = true; break; #endif /////////////////////////// case 'D': // Read RC Control Points eepromConfig.midCommand = readFloatCLI(); eepromConfig.minCheck = readFloatCLI(); eepromConfig.maxCheck = readFloatCLI(); eepromConfig.minThrottle = readFloatCLI(); eepromConfig.maxThrottle = readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'E': // Read Arm/Disarm Counts eepromConfig.armCount = (uint8_t)readFloatCLI(); eepromConfig.disarmCount = (uint8_t)readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'F': // Read Max Rate Value eepromConfig.rollAndPitchRateScaling = readFloatCLI() / 180000.0f * PI; eepromConfig.yawRateScaling = readFloatCLI() / 180000.0f * PI; receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'G': // Read Max Attitude Value eepromConfig.attitudeScaling = readFloatCLI() / 180000.0f * PI; receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'W': // Write EEPROM Parameters cliPortPrint("\nWriting EEPROM Parameters....\n\n"); writeEEPROM(); validQuery = false; break; /////////////////////////// case '?': cliPortPrint("\n"); cliPortPrint("'a' Receiver Configuration Data 'A' Toggle PPM/Spektrum Receiver\n"); cliPortPrint(" 'B' Set RC Control Order BTAER1234\n"); #if defined(STM32F40XX) cliPortPrint(" 'C' Toggle Slave Spektrum State\n"); #endif cliPortPrint(" 'D' Set RC Control Points DmidCmd;minChk;maxChk;minThrot;maxThrot\n"); cliPortPrint(" 'E' Set Arm/Disarm Counts EarmCount;disarmCount\n"); cliPortPrint(" 'F' Set Maximum Rate Commands FRP;Y RP = Roll/Pitch, Y = Yaw\n"); cliPortPrint(" 'G' Set Maximum Attitude Command\n"); cliPortPrint(" 'W' Write EEPROM Parameters\n"); cliPortPrint("'x' Exit Receiver CLI '?' Command Summary\n"); cliPortPrint("\n"); break; /////////////////////////// } } }
void radioInit(void) { uint16_t radioType = (uint16_t)p[RADIO_TYPE]; int i; AQ_NOTICE("Radio init\n"); memset((void *)&radioData, 0, sizeof(radioData)); radioData.mode = (radioType>>12) & 0x0f; for (i = 0; i < RADIO_NUM; i++) { radioInstance_t *r = &radioData.radioInstances[i]; USART_TypeDef *uart; // determine UART switch (i) { case 0: uart = RC1_UART; break; #ifdef RC2_UART case 1: uart = RC2_UART; break; #endif #ifdef RC3_UART case 2: uart = RC3_UART; break; #endif default: uart = 0; break; } r->radioType = (radioType>>(i*4)) & 0x0f; r->channels = &radioData.allChannels[RADIO_MAX_CHANNELS * i]; utilFilterInit(&r->qualityFilter, (1.0f / 50.0f), 0.75f, 0.0f); switch (r->radioType) { case RADIO_TYPE_SPEKTRUM11: case RADIO_TYPE_SPEKTRUM10: case RADIO_TYPE_DELTANG: if (uart) { spektrumInit(r, uart); radioRCSelect(i, 0); AQ_PRINTF("Spektrum on RC port %d\n", i); } break; case RADIO_TYPE_SBUS: if (uart) { futabaInit(r, uart); radioRCSelect(i, 1); AQ_PRINTF("Futaba on RC port %d\n", i); } break; case RADIO_TYPE_PPM: ppmInit(r); AQ_PRINTF("PPM on RC port %d\n", i); break; case RADIO_TYPE_SUMD: if (uart) { grhottInit(r, uart); radioRCSelect(i, 0); AQ_PRINTF("GrHott on RC port %d\n", i); } break; case RADIO_TYPE_MLINK: if (uart) { mlinkrxInit(r, uart); radioRCSelect(i, 0); AQ_PRINTF("Mlink on RC port %d\n", i); } break; case RADIO_TYPE_NONE: break; default: AQ_NOTICE("WARNING: Invalid radio type!\n"); break; } } switch (radioData.mode) { case RADIO_MODE_DIVERSITY: // select first available radio to start with for (i = 0; i < RADIO_NUM; i++) { if (radioData.radioInstances[i].radioType > RADIO_TYPE_NONE) { radioMakeCurrent(&radioData.radioInstances[i]); break; } } break; case RADIO_MODE_SPLIT: radioMakeCurrent(&radioData.radioInstances[0]); break; } // set mode default radioData.channels[(int)p[RADIO_FLAP_CH]] = -700; radioTaskStack = aqStackInit(RADIO_STACK_SIZE, "RADIO"); radioData.radioTask = CoCreateTask(radioTaskCode, (void *)0, RADIO_PRIORITY, &radioTaskStack[RADIO_STACK_SIZE-1], RADIO_STACK_SIZE); }