bstNode *bstFind(void *info, BST * bst) { bstNode *current; if (bst == NULL && info == NULL) { fprintf(stderr, "Queue is empty"); return NULL; } if (!bst->init) { bstInit(bst); return (NULL); } current = bst->root; while (current != NULL) { int compval; compval = bst->compare(current->info, info); /* compare the current node with info */ if (compval <= -1) /* if the node's data was less than info */ current = current->right; /* search in the right subtree */ else if (compval >= 1) /* if the node's data was greater than info */ current = current->left; /* search inthe left subtree */ else break; /* otherwise, the data equals info */ } return current; /* returns null if not found, or the node that equals info */ }
bstNode *bstInsert(void *info, BST * bst) { bstNode *newNode; bstNode *current; int done = 0; if (bst == NULL && info == NULL) { fprintf(stderr, "Queue is empty"); return NULL; } if (!bst->init) { bstInit(bst); } newNode = (bstNode *) malloc(sizeof(bstNode)); /* allocate memory for the node */ if (newNode == NULL) { bst->malloc_fail_handler(); /* call the malloc fail handler if memory is not allocated */ return NULL; } newNode->info = info; /* initialize fields of the new node */ newNode->left = NULL; newNode->right = NULL; bst->nodes++; /* increment the node counter */ if (bst->root == NULL) { /* check if this is the only node in the tree */ bst->root = newNode; newNode->parent = NULL; } else { /* else find a location to insert the node */ current = bst->root; do { int compval; done = 0; compval = bst->compare(info, current->info); if (compval >= 0) { /* new node is greater than the current node */ if (current->right == NULL) { /* if right is null then a location has been found *//* for the new node */ current->right = newNode; newNode->parent = current; done = 1; } current = current->right; /* else go right */ } else { /* the new node is less than the current node */ if (current->left == NULL) { /* if left is null then put new node to the left */ current->left = newNode; newNode->parent = current; done = 1; } current = current->left; /* else go left */ } } while (!done); /* keep searching until a location for new node has been found */ } return newNode; }
int bstGetCount(BST * bst) { if (bst == NULL) { fprintf(stderr, "Queue is empty"); return -1; } if (!bst->init) { bstInit(bst); return (0); } return bst->nodes; }
void bstDeleteTree(BST * bst) { if (bst == NULL) { fprintf(stderr, "Queue is empty"); return; } if (!bst->init) { bstInit(bst); return; } bstDeleteNodes(bst, bst->root); bst->root = NULL; bst->nodes = 0; bst->deltype = 0; }
void init(void) { #ifdef USE_HAL_DRIVER HAL_Init(); #endif printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; systemInit(); //i2cSetOverclock(masterConfig.i2c_overclock); // initialize IO (needed for all IO operations) IOInitGlobal(); debugMode = masterConfig.debug_mode; #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); #ifdef ALIENFLIGHTF3 ledInit(hardwareRevision == AFF3_REV_1 ? false : true); #else ledInit(false); #endif LED2_ON; #ifdef USE_EXTI EXTIInit(); #endif #if defined(BUTTONS) gpio_config_t buttonAGpioConfig = { BUTTON_A_PIN, Mode_IPU, Speed_2MHz }; gpioInit(BUTTON_A_PORT, &buttonAGpioConfig); gpio_config_t buttonBGpioConfig = { BUTTON_B_PIN, Mode_IPU, Speed_2MHz }; gpioInit(BUTTON_B_PORT, &buttonBGpioConfig); // Check status of bind plug and exit if not active delayMicroseconds(10); // allow GPIO configuration to settle if (!isMPUSoftReset()) { uint8_t secondsRemaining = 5; bool bothButtonsHeld; do { bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN); if (bothButtonsHeld) { if (--secondsRemaining == 0) { resetEEPROM(); systemReset(); } delay(1000); LED0_TOGGLE; } } while (bothButtonsHeld); } #endif #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated #if !defined(USE_HAL_DRIVER) dmaInit(); #endif #if defined(AVOID_UART1_FOR_PWM_PPM) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE); #elif defined(AVOID_UART2_FOR_PWM_PPM) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); #elif defined(AVOID_UART3_FOR_PWM_PPM) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); #else serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #ifdef USE_SERVOS servoMixerInit(masterConfig.customServoMixer); #endif uint16_t idlePulse = masterConfig.motorConfig.mincommand; if (feature(FEATURE_3D)) { idlePulse = masterConfig.flight3DConfig.neutral3d; } if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) { featureClear(FEATURE_3D); idlePulse = 0; // brushed motors } #ifdef USE_QUAD_MIXER_ONLY motorInit(&masterConfig.motorConfig, idlePulse, QUAD_MOTOR_COUNT); #else motorInit(&masterConfig.motorConfig, idlePulse, mixers[masterConfig.mixerMode].motorCount); #endif #ifdef USE_SERVOS if (isMixerUsingServos()) { //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); servoInit(&masterConfig.servoConfig); } #endif #ifndef SKIP_RX_PWM_PPM if (feature(FEATURE_RX_PPM)) { ppmRxInit(&masterConfig.ppmConfig, masterConfig.motorConfig.motorPwmProtocol); } else if (feature(FEATURE_RX_PARALLEL_PWM)) { pwmRxInit(&masterConfig.pwmConfig); } pwmRxSetInputFilteringMode(masterConfig.inputFilteringMode); #endif mixerConfigureOutput(); #ifdef USE_SERVOS servoConfigureOutput(); #endif systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER beeperInit(&masterConfig.beeperConfig); #endif /* temp until PGs are implemented. */ #ifdef INVERTER initInverter(); #endif #ifdef USE_BST bstInit(BST_DEVICE); #endif #ifdef USE_SPI #ifdef USE_SPI_DEVICE_1 spiInit(SPIDEV_1); #endif #ifdef USE_SPI_DEVICE_2 spiInit(SPIDEV_2); #endif #ifdef USE_SPI_DEVICE_3 #ifdef ALIENFLIGHTF3 if (hardwareRevision == AFF3_REV_2) { spiInit(SPIDEV_3); } #else spiInit(SPIDEV_3); #endif #endif #ifdef USE_SPI_DEVICE_4 spiInit(SPIDEV_4); #endif #endif #ifdef VTX vtxInit(); #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #if defined(NAZE) if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } else { serialRemovePort(SERIAL_PORT_USART3); } #endif #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #if defined(SPRACINGF3MINI) || defined(OMNIBUS) || defined(X_RACERSPI) #if defined(SONAR) && defined(USE_SOFTSERIAL1) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } else { if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } #else i2cInit(I2C_DEVICE); #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableVBat = feature(FEATURE_VBAT); adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif #ifdef USE_RTC6705 if (feature(FEATURE_VTX)) { rtc6705_soft_spi_init(); current_vtx_channel = masterConfig.vtx_channel; rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power); } #endif #ifdef OSD if (feature(FEATURE_OSD)) { osdInit(); } #endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, masterConfig.mag_declination, masterConfig.gyro_lpf, masterConfig.gyro_sync_denom)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; LED1_ON; LED0_OFF; LED2_OFF; for (int i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif imuInit(); mspFcInit(); mspSerialInit(); #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( &masterConfig.gpsProfile, ¤tProfile->pidProfile ); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { sonarInit(&masterConfig.sonarConfig); } #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USB_CABLE_DETECTION usbCableDetectInit(); #endif #ifdef TRANSPONDER if (feature(FEATURE_TRANSPONDER)) { transponderInit(masterConfig.transponderData); transponderEnable(); transponderStartRepeating(); systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(IO_TAG_NONE); } #elif defined(USE_FLASH_M25P16) m25p16_init(IO_TAG_NONE); #endif flashfsInit(); #endif #ifdef USE_SDCARD bool sdcardUseDMA = false; sdcardInsertionDetectInit(); #ifdef SDCARD_DMA_CHANNEL_TX #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip #if defined(STM32F4) || defined(STM32F7) sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM; #else sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; #endif #else sdcardUseDMA = true; #endif #endif sdcard_init(sdcardUseDMA); afatfs_init(); #endif if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) { masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed masterConfig.gyro_sync_denom = 1; } setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime #ifdef BLACKBOX initBlackbox(); #endif if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayResetPageCycling(); displayEnablePageCycling(); #endif } #endif #ifdef CJMCU LED2_ON; #endif // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; fcTasksInit(); systemState |= SYSTEM_STATE_READY; }
void *bstDelete(bstNode * node, BST * bst) { bstNode *current; void *ret; bstNode **parentspointer; if (node == NULL && bst == NULL) { fprintf(stderr, "Queue is empty"); return NULL; } if (!bst->init) { bstInit(bst); return (node); } bst->nodes--; /* decrement the node counter */ if (node->parent != NULL) { /* if node is not the root, *//* then get the address of the parent's pointer to node */ if (node->parent->left == node) parentspointer = &(node->parent->left); else parentspointer = &(node->parent->right); } else parentspointer = &(bst->root); if (node->left == NULL) { /* if there is no left subtree of node */ *parentspointer = node->right; if (node->right != NULL) node->right->parent = node->parent; /* link up parent pointer if a right tree exists */ } else if (node->right == NULL) { /* if right is null, but not left then link up with left subtree */ *parentspointer = node->left; node->left->parent = node->parent; } else if (bst->deltype == RIGHTLEAF) { /* neither left nor right is null */ bst->deltype = 1 - bst->deltype; /* toggle deltype between 1 and 0 */ current = node->right; /* get inner child from right side */ if (current->left == NULL) { /* if left is null then inner child is found */ *parentspointer = current; current->parent = node->parent; current->left = node->left; current->left->parent = current; } else { while (current->left != NULL) /* go as far left as possible */ current = current->left; current->parent->left = current->right; /* replace the deleted node with inner child */ if (current->right != NULL) current->right->parent = current->parent; *parentspointer = current; current->parent = node->parent; current->left = node->left; current->right = node->right; current->left->parent = current; current->right->parent = current; } } else { /* using left leaf deletion to keep from getting unbalanced due to deletions */ bst->deltype = 1 - bst->deltype; /* toggle deltype back to right leaf deletion */ current = node->left; if (current->right == NULL) { /* do the same right leaf deletion except in other direction */ *parentspointer = current; current->parent = node->parent; current->right = node->right; current->right->parent = current; } else { while (current->right != NULL) current = current->right; current->parent->right = current->left; if (current->left != NULL) current->left->parent = current->parent; *parentspointer = current; current->parent = node->parent; current->left = node->left; current->right = node->right; current->right->parent = current; current->left->parent = current; } } ret = node->info; /* return a pointer to the data contained in the node */ free(node); /* free memory associated with the deleted node */ return ret; }
void bstTraverseTree(BST * bst) { bstNode *current; int done = 0; int st = 2; if (bst == NULL) { fprintf(stderr, "Queue is empty"); return; } if (!bst->init) { bstInit(bst); return; } current = bst->root; /* start at the root */ if (current == NULL) return; while (!done) { switch (st) { /* switch based upon the current state of traversing the tree */ case 1: /* go up to the parent of current node */ { current = current->parent; if (current->parent == NULL) done = 1; else if (current == current->parent->left) st = 3; } break; case 2: /* traverse left until null is found */ { while (current->left != NULL) current = current->left; st = 4; } break; case 3: /* go to the parent of current node */ { current = current->parent; st = 4; } break; case 4: /* do something to the current node, then figure out where to go */ { bst->traverse_node(current->info); if (current->right == NULL) { if (current->parent == NULL) done = 1; else if (current->parent->left == current) st = 3; else st = 1; } else { current = current->right; st = 2; } } break; } } }
void init(void) { printfSupportInit(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); systemState |= SYSTEM_STATE_CONFIG_LOADED; systemInit(); //i2cSetOverclock(masterConfig.i2c_overclock); // initialize IO (needed for all IO operations) IOInitGlobal(); debugMode = masterConfig.debug_mode; #ifdef USE_HARDWARE_REVISION_DETECTION detectHardwareRevision(); #endif // Latch active features to be used for feature() in the remainder of init(). latchActiveFeatures(); #ifdef ALIENFLIGHTF3 ledInit(hardwareRevision == AFF3_REV_1 ? false : true); #else ledInit(false); #endif LED2_ON; #ifdef USE_EXTI EXTIInit(); #endif #if defined(BUTTONS) gpio_config_t buttonAGpioConfig = { BUTTON_A_PIN, Mode_IPU, Speed_2MHz }; gpioInit(BUTTON_A_PORT, &buttonAGpioConfig); gpio_config_t buttonBGpioConfig = { BUTTON_B_PIN, Mode_IPU, Speed_2MHz }; gpioInit(BUTTON_B_PORT, &buttonBGpioConfig); // Check status of bind plug and exit if not active delayMicroseconds(10); // allow GPIO configuration to settle if (!isMPUSoftReset()) { uint8_t secondsRemaining = 5; bool bothButtonsHeld; do { bothButtonsHeld = !digitalIn(BUTTON_A_PORT, BUTTON_A_PIN) && !digitalIn(BUTTON_B_PORT, BUTTON_B_PIN); if (bothButtonsHeld) { if (--secondsRemaining == 0) { resetEEPROM(); systemReset(); } delay(1000); LED0_TOGGLE; } } while (bothButtonsHeld); } #endif #ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: // Spektrum satellite binding if enabled on startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // The rest of Spektrum initialization will happen later - via spektrumInit() spektrumBind(&masterConfig.rxConfig); break; } } #endif delay(100); timerInit(); // timer must be initialized before any channel is allocated dmaInit(); #if defined(AVOID_UART1_FOR_PWM_PPM) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE); #elif defined(AVOID_UART2_FOR_PWM_PPM) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); #elif defined(AVOID_UART3_FOR_PWM_PPM) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); #else serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif #ifdef USE_SERVOS mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer); #else mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #endif drv_pwm_config_t pwm_params; memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR if (feature(FEATURE_SONAR)) { const sonarHardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType); if (sonarHardware) { pwm_params.useSonar = true; pwm_params.sonarIOConfig.triggerTag = sonarHardware->triggerTag; pwm_params.sonarIOConfig.echoTag = sonarHardware->echoTag; } } #endif // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(USE_UART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif #ifdef STM32F303xC pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3); #endif #if defined(USE_UART2) && defined(STM32F40_41xxx) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif #if defined(USE_UART6) && defined(STM32F40_41xxx) pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6); #endif pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); #ifdef USE_SERVOS pwm_params.useServos = isMixerUsingServos(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL || masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED; // Configurator feature abused for enabling Fast PWM pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED); pwm_params.pwmProtocolType = masterConfig.motor_pwm_protocol; pwm_params.motorPwmRate = use_unsyncedPwm ? masterConfig.motor_pwm_rate : 0; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED) { featureClear(FEATURE_3D); pwm_params.idlePulse = 0; // brushed motors } #ifdef CC3D pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false; #endif #ifndef SKIP_RX_PWM_PPM pwmRxInit(masterConfig.inputFilteringMode); #endif // pwmInit() needs to be called as soon as possible for ESC compatibility reasons pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); mixerUsePWMOutputConfiguration(pwmOutputConfiguration, use_unsyncedPwm); systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER beeperConfig_t beeperConfig = { .ioTag = IO_TAG(BEEPER), #ifdef BEEPER_INVERTED .isOD = false, .isInverted = true #else .isOD = true, .isInverted = false #endif }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.isOD = false; beeperConfig.isInverted = true; } #endif /* temp until PGs are implemented. */ #ifdef BLUEJAYF4 if (hardwareRevision <= BJF4_REV2) { beeperConfig.ioTag = IO_TAG(BEEPER_OPT); } #endif #ifdef CC3D if (masterConfig.use_buzzer_p6 == 1) beeperConfig.ioTag = IO_TAG(BEEPER_OPT); #endif beeperInit(&beeperConfig); #endif #ifdef INVERTER initInverter(); #endif #ifdef USE_BST bstInit(BST_DEVICE); #endif #ifdef USE_SPI #ifdef USE_SPI_DEVICE_1 spiInit(SPIDEV_1); #endif #ifdef USE_SPI_DEVICE_2 spiInit(SPIDEV_2); #endif #ifdef USE_SPI_DEVICE_3 #ifdef ALIENFLIGHTF3 if (hardwareRevision == AFF3_REV_2) { spiInit(SPIDEV_3); } #else spiInit(SPIDEV_3); #endif #endif #endif #ifdef VTX vtxInit(); #endif #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif #if defined(NAZE) if (hardwareRevision == NAZE32_SP) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } else { serialRemovePort(SERIAL_PORT_USART3); } #endif #if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #if defined(SPRACINGF3MINI) || defined(OMNIBUS) || defined(X_RACERSPI) #if defined(SONAR) && defined(USE_SOFTSERIAL1) if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif #endif #ifdef USE_I2C #if defined(NAZE) if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } else { if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } } #elif defined(CC3D) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { i2cInit(I2C_DEVICE); } #else i2cInit(I2C_DEVICE); #endif #endif #ifdef USE_ADC drv_adc_config_t adc_params; adc_params.enableVBat = feature(FEATURE_VBAT); adc_params.enableRSSI = feature(FEATURE_RSSI_ADC); adc_params.enableCurrentMeter = feature(FEATURE_CURRENT_METER); adc_params.enableExternal1 = false; #ifdef OLIMEXINO adc_params.enableExternal1 = true; #endif #ifdef NAZE // optional ADC5 input on rev.5 hardware adc_params.enableExternal1 = (hardwareRevision >= NAZE32_REV5); #endif adcInit(&adc_params); #endif initBoardAlignment(&masterConfig.boardAlignment); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); } #endif #ifdef USE_RTC6705 if (feature(FEATURE_VTX)) { rtc6705_soft_spi_init(); current_vtx_channel = masterConfig.vtx_channel; rtc6705_soft_spi_set_channel(vtx_freq[current_vtx_channel]); rtc6705_soft_spi_set_rf_power(masterConfig.vtx_power); } #endif #ifdef OSD if (feature(FEATURE_OSD)) { osdInit(); } #endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, masterConfig.mag_declination, masterConfig.gyro_lpf, masterConfig.gyro_sync_denom)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } systemState |= SYSTEM_STATE_SENSORS_READY; LED1_ON; LED0_OFF; LED2_OFF; for (int i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON; delay(25); BEEP_OFF; } LED0_OFF; LED1_OFF; #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif imuInit(); mspInit(&masterConfig.serialConfig); #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( &masterConfig.gpsProfile, ¤tProfile->pidProfile ); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { sonarInit(); } #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USB_CABLE_DETECTION usbCableDetectInit(); #endif #ifdef TRANSPONDER if (feature(FEATURE_TRANSPONDER)) { transponderInit(masterConfig.transponderData); transponderEnable(); transponderStartRepeating(); systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; } #endif #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { m25p16_init(IOTAG_NONE); } #elif defined(USE_FLASH_M25P16) m25p16_init(IOTAG_NONE); #endif flashfsInit(); #endif #ifdef USE_SDCARD bool sdcardUseDMA = false; sdcardInsertionDetectInit(); #ifdef SDCARD_DMA_CHANNEL_TX #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip #ifdef STM32F4 sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM; #else sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; #endif #else sdcardUseDMA = true; #endif #endif sdcard_init(sdcardUseDMA); afatfs_init(); #endif if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) { masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed masterConfig.gyro_sync_denom = 1; } setTargetPidLooptime(gyro.targetLooptime * masterConfig.pid_process_denom); // Initialize pid looptime #ifdef BLACKBOX initBlackbox(); #endif if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif // start all timers // TODO - not implemented yet timerStart(); ENABLE_STATE(SMALL_ANGLE); DISABLE_ARMING_FLAG(PREVENT_ARMING); #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); if (!loopbackPort->vTable) { loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED); } serialPrint(loopbackPort, "LOOPBACK\r\n"); #endif // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayResetPageCycling(); displayEnablePageCycling(); #endif } #endif #ifdef CJMCU LED2_ON; #endif // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; systemState |= SYSTEM_STATE_READY; } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif void main_init(void) { init(); /* Setup scheduler */ schedulerInit(); rescheduleTask(TASK_GYROPID, gyro.targetLooptime); setTaskEnabled(TASK_GYROPID, true); if (sensors(SENSOR_ACC)) { setTaskEnabled(TASK_ACCEL, true); switch (gyro.targetLooptime) { // Switch statement kept in place to change acc rates in the future case 500: case 375: case 250: case 125: accTargetLooptime = 1000; break; default: case 1000: #ifdef STM32F10X accTargetLooptime = 1000; #else accTargetLooptime = 1000; #endif } rescheduleTask(TASK_ACCEL, accTargetLooptime); } setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC)); setTaskEnabled(TASK_SERIAL, true); #ifdef BEEPER setTaskEnabled(TASK_BEEPER, true); #endif setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); setTaskEnabled(TASK_RX, true); #ifdef GPS setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); #endif #ifdef MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #if defined(USE_SPI) && defined(USE_MAG_AK8963) // fixme temporary solution for AK6983 via slave I2C on MPU9250 rescheduleTask(TASK_COMPASS, 1000000 / 40); #endif #endif #ifdef BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); #endif #ifdef SONAR setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); #endif #if defined(BARO) || defined(SONAR) setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); #endif #ifdef DISPLAY setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); // Reschedule telemetry to 500hz for Jeti Exbus if (feature(FEATURE_TELEMETRY) || masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) rescheduleTask(TASK_TELEMETRY, 2000); #endif #ifdef LED_STRIP setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); #endif #ifdef TRANSPONDER setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); #endif #ifdef OSD setTaskEnabled(TASK_OSD, feature(FEATURE_OSD)); #endif #ifdef USE_BST setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif }
void main() { short done=0; int newnum; char ch; char buffer[256]; ITEM* newitem; BST bst; bstNode* node; ITEM tempitem; bstInit(&bst); bstSetCompareFunc(&bst,compare); while(!done) { fprintf(stderr,"(1) Add Item\n(2) Lookup\n(3) Remove Item\n"); ch=getchar(); fflush(stdin); switch(ch) { case '1': newitem=(ITEM*)malloc(sizeof(ITEM)); printf("\nEnter a string\n"); scanf("%s",buffer); newitem->data=strdup(buffer); printf("Enter key\n"); scanf("%d",&newnum); newitem->key=newnum; printf("inserting at %d\n",newnum); bstInsert(newitem,&bst); break; case '2': printf("Enter key\n"); scanf("%d",&newnum); printf("Looking for %d\n",newnum); tempitem.key=newnum; node=bstFind(&tempitem,&bst); if(node != NULL) { newitem=(ITEM*)bstGetInfo(node); printf("found %s\n",newitem->data); } else { printf("Item not found\n"); } break; case '3': printf("Enter key\n"); scanf("%d",&newnum); tempitem.key=newnum; node=bstFind(&tempitem,&bst); if(node != NULL) { newitem=(ITEM*)bstGetInfo(node); printf("removing %s\n",newitem->data); bstDelete(node,&bst); } else { printf("Item not found\n"); } break; default:done=1; } fflush(stdin); } }