void controlInit(void) { AQ_NOTICE("Control init\n"); memset((void *)&controlData, 0, sizeof(controlData)); #ifdef USE_QUATOS quatosInit(AQ_INNER_TIMESTEP, AQ_OUTER_TIMESTEP); #endif utilFilterInit3(controlData.userPitchFilter, AQ_INNER_TIMESTEP, 0.1f, 0.0f); utilFilterInit3(controlData.userRollFilter, AQ_INNER_TIMESTEP, 0.1f, 0.0f); utilFilterInit3(controlData.navPitchFilter, AQ_INNER_TIMESTEP, 0.125f, 0.0f); utilFilterInit3(controlData.navRollFilter, AQ_INNER_TIMESTEP, 0.125f, 0.0f); controlData.pitchRatePID = pidInit(&p[CTRL_TLT_RTE_P], &p[CTRL_TLT_RTE_I], &p[CTRL_TLT_RTE_D], &p[CTRL_TLT_RTE_F], &p[CTRL_TLT_RTE_PM], &p[CTRL_TLT_RTE_IM], &p[CTRL_TLT_RTE_DM], &p[CTRL_TLT_RTE_OM], 0, 0, 0, 0); controlData.rollRatePID = pidInit(&p[CTRL_TLT_RTE_P], &p[CTRL_TLT_RTE_I], &p[CTRL_TLT_RTE_D], &p[CTRL_TLT_RTE_F], &p[CTRL_TLT_RTE_PM], &p[CTRL_TLT_RTE_IM], &p[CTRL_TLT_RTE_DM], &p[CTRL_TLT_RTE_OM], 0, 0, 0, 0); controlData.yawRatePID = pidInit(&p[CTRL_YAW_RTE_P], &p[CTRL_YAW_RTE_I], &p[CTRL_YAW_RTE_D], &p[CTRL_YAW_RTE_F], &p[CTRL_YAW_RTE_PM], &p[CTRL_YAW_RTE_IM], &p[CTRL_YAW_RTE_DM], &p[CTRL_YAW_RTE_OM], 0, 0, 0, 0); controlData.pitchAnglePID = pidInit(&p[CTRL_TLT_ANG_P], &p[CTRL_TLT_ANG_I], &p[CTRL_TLT_ANG_D], &p[CTRL_TLT_ANG_F], &p[CTRL_TLT_ANG_PM], &p[CTRL_TLT_ANG_IM], &p[CTRL_TLT_ANG_DM], &p[CTRL_TLT_ANG_OM], 0, 0, 0, 0); controlData.rollAnglePID = pidInit(&p[CTRL_TLT_ANG_P], &p[CTRL_TLT_ANG_I], &p[CTRL_TLT_ANG_D], &p[CTRL_TLT_ANG_F], &p[CTRL_TLT_ANG_PM], &p[CTRL_TLT_ANG_IM], &p[CTRL_TLT_ANG_DM], &p[CTRL_TLT_ANG_OM], 0, 0, 0, 0); controlData.yawAnglePID = pidInit(&p[CTRL_YAW_ANG_P], &p[CTRL_YAW_ANG_I], &p[CTRL_YAW_ANG_D], &p[CTRL_YAW_ANG_F], &p[CTRL_YAW_ANG_PM], &p[CTRL_YAW_ANG_IM], &p[CTRL_YAW_ANG_DM], &p[CTRL_YAW_ANG_OM], 0, 0, 0, 0); controlTaskStack = aqStackInit(CONTROL_STACK_SIZE, "CONTROL"); controlData.controlTask = CoCreateTask(controlTaskCode, (void *)0, CONTROL_PRIORITY, &controlTaskStack[CONTROL_STACK_SIZE-1], CONTROL_STACK_SIZE); }
void runInit(void) { float acc[3], mag[3]; float pres; int i; memset((void *)&runData, 0, sizeof(runData)); runData.runFlag = CoCreateFlag(1, 0); // auto reset runTaskStack = aqStackInit(RUN_TASK_SIZE, "RUN"); runData.runTask = CoCreateTask(runTaskCode, (void *)0, RUN_PRIORITY, &runTaskStack[RUN_TASK_SIZE-1], RUN_TASK_SIZE); acc[0] = IMU_ACCX; acc[1] = IMU_ACCY; acc[2] = IMU_ACCZ; mag[0] = IMU_MAGX; mag[1] = IMU_MAGY; mag[2] = IMU_MAGZ; pres = AQ_PRESSURE; // initialize sensor history for (i = 0; i < RUN_SENSOR_HIST; i++) { runData.accHist[0][i] = acc[0]; runData.accHist[1][i] = acc[1]; runData.accHist[2][i] = acc[2]; runData.magHist[0][i] = mag[0]; runData.magHist[1][i] = mag[1]; runData.magHist[2][i] = mag[2]; runData.presHist[i] = pres; runData.sumAcc[0] += acc[0]; runData.sumAcc[1] += acc[1]; runData.sumAcc[2] += acc[2]; runData.sumMag[0] += mag[0]; runData.sumMag[1] += mag[1]; runData.sumMag[2] += mag[2]; runData.sumPres += pres; } runData.sensorHistIndex = 0; runData.bestHacc = 1000.0f; runData.accMask = 1000.0f; // use altUkf altitude & vertical velocity estimates to start with runData.altPos = &ALT_POS; runData.altVel = &ALT_VEL; #ifdef USE_MAVLINK // configure px4flow sensor // mavlinkSendParameter(81, 50, "BFLOW_V_THLD", 2500.0f); // mavlinkSendParameter(81, 50, "BFLOW_F_THLD", 100.0f); mavlinkSendParameter(81, 50, "BFLOW_GYRO_COM", 0.0f); mavlinkSendParameter(81, 50, "USB_SEND_VIDEO", 0.0f); #endif }
void supervisorInit(void) { memset((void *)&supervisorData, 0, sizeof(supervisorData)); supervisorData.readyLed = digitalInit(SUPERVISOR_READY_PORT, SUPERVISOR_READY_PIN, 0); #ifdef SUPERVISOR_DEBUG_PORT supervisorData.debugLed = digitalInit(SUPERVISOR_DEBUG_PORT, SUPERVISOR_DEBUG_PIN, 0); #endif supervisorData.gpsLed = digitalInit(GPS_LED_PORT, GPS_LED_PIN, 0); supervisorData.state = STATE_INITIALIZING; supervisorTaskStack = aqStackInit(SUPERVISOR_STACK_SIZE, "SUPERVISOR"); supervisorData.supervisorTask = CoCreateTask(supervisorTaskCode, (void *)0, SUPERVISOR_PRIORITY, &supervisorTaskStack[SUPERVISOR_STACK_SIZE-1], SUPERVISOR_STACK_SIZE); }
void dIMUInit(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; NVIC_InitTypeDef NVIC_InitStructure; #ifdef DIMU_HAVE_MPU6000 mpu6000PreInit(); #endif #ifdef DIMU_HAVE_MAX21100 max21100PreInit(); #endif #ifdef DIMU_HAVE_EEPROM eepromPreInit(); #endif #ifdef DIMU_HAVE_HMC5983 hmc5983PreInit(); #endif #ifdef DIMU_HAVE_MS5611 ms5611PreInit(); #endif #ifdef DIMU_HAVE_MPU6000 mpu6000Init(); #endif #ifdef DIMU_HAVE_MAX21100 max21100Init(); #endif #ifdef DIMU_HAVE_EEPROM eepromInit(); // dIMUWriteCalib(); dIMUReadCalib(); #endif #ifdef DIMU_HAVE_HMC5983 if (hmc5983Init() == 0) AQ_NOTICE("DIMU: MAG sensor init failed!\n"); #endif #ifdef DIMU_HAVE_MS5611 if (ms5611Init() == 0) AQ_NOTICE("DIMU: PRES sensor init failed!\n"); #endif dIMUTaskStack = aqStackInit(DIMU_STACK_SIZE, "DIMU"); dImuData.flag = CoCreateFlag(1, 0); dImuData.task = CoCreateTask(dIMUTaskCode, (void *)0, DIMU_PRIORITY, &dIMUTaskStack[DIMU_STACK_SIZE-1], DIMU_STACK_SIZE); // setup digital IMU timer DIMU_EN; // Time base configuration for 1MHz (us) TIM_TimeBaseStructure.TIM_Period = 0xffff; TIM_TimeBaseStructure.TIM_Prescaler = (DIMU_CLOCK / 1000000) - 1; TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(DIMU_TIM, &TIM_TimeBaseStructure); // reset TIM_SetCounter(DIMU_TIM, 0); // Output Compare for alarms TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Inactive; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; TIM_OC1Init(DIMU_TIM, &TIM_OCInitStructure); TIM_OC1PreloadConfig(DIMU_TIM, TIM_OCPreload_Disable); TIM_OC2Init(DIMU_TIM, &TIM_OCInitStructure); TIM_OC2PreloadConfig(DIMU_TIM, TIM_OCPreload_Disable); // Enable the global Interrupt NVIC_InitStructure.NVIC_IRQChannel = DIMU_IRQ_CH; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); // reset TIM_SetCounter(DIMU_TIM, 0); dIMUCancelAlarm1(); // go... TIM_Cmd(DIMU_TIM, ENABLE); #ifdef DIMU_HAVE_MPU6000 mpu6000Enable(); #endif #ifdef DIMU_HAVE_MAX21100 max21100Enable(); #endif #ifdef DIMU_HAVE_HMC5983 hmc5983Enable(); #endif #ifdef DIMU_HAVE_MS5611 ms5611Enable(); #endif // setup IMU timestep alarm dImuData.nextPeriod = DIMU_TIM->CCR2 + DIMU_INNER_PERIOD; DIMU_TIM->CCR2 = dImuData.nextPeriod; DIMU_TIM->DIER |= TIM_IT_CC2; #ifdef DIMU_HAVE_MPU6000 mpu6600InitialBias(); #endif #ifdef DIMU_HAVE_MAX21100 max21100InitialBias(); #endif #ifdef DIMU_HAVE_MS5611 ms5611InitialBias(); #endif }
void adcInit(void) { GPIO_InitTypeDef GPIO_InitStructure; DMA_InitTypeDef DMA_InitStructure; ADC_CommonInitTypeDef ADC_CommonInitStructure; ADC_InitTypeDef ADC_InitStructure; NVIC_InitTypeDef NVIC_InitStructure; AQ_NOTICE("ADC init\n"); memset((void *)&adcData, 0, sizeof(adcData)); // energize mag's set/reset circuit adcData.magSetReset = digitalInit(GPIOE, GPIO_Pin_10, 1); // use auto-zero function of gyros adcData.rateAutoZero = digitalInit(GPIOE, GPIO_Pin_8, 0); // bring ACC's SELF TEST line low adcData.accST = digitalInit(GPIOE, GPIO_Pin_12, 0); // bring ACC's SCALE line low (ADXL3X5 requires this line be tied to GND or left floating) adcData.accScale = digitalInit(GPIOC, GPIO_Pin_15, 0); GPIO_StructInit(&GPIO_InitStructure); GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7; GPIO_Init(GPIOA, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_4 | GPIO_Pin_5; GPIO_Init(GPIOC, &GPIO_InitStructure); RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | RCC_APB2Periph_ADC3, ENABLE); adcData.sample = ADC_SAMPLES - 1; // Use STM32F4's Triple Regular Simultaneous Mode capable of ~ 6M samples per second DMA_DeInit(ADC_DMA_STREAM); DMA_InitStructure.DMA_Channel = ADC_DMA_CHANNEL; DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)adcDMAData.adc123Raw1; DMA_InitStructure.DMA_PeripheralBaseAddr = ((uint32_t)0x40012308); DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; DMA_InitStructure.DMA_BufferSize = ADC_CHANNELS * 3 * 2; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh; DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Enable; DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; DMA_Init(ADC_DMA_STREAM, &DMA_InitStructure); DMA_ITConfig(ADC_DMA_STREAM, DMA_IT_HT | DMA_IT_TC, ENABLE); DMA_ClearITPendingBit(ADC_DMA_STREAM, ADC_DMA_FLAGS); DMA_Cmd(ADC_DMA_STREAM, ENABLE); NVIC_InitStructure.NVIC_IRQChannel = ADC_DMA_IRQ; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); // ADC Common Init ADC_CommonStructInit(&ADC_CommonInitStructure); ADC_CommonInitStructure.ADC_Mode = ADC_TripleMode_RegSimult; ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div2; ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_1; ADC_CommonInit(&ADC_CommonInitStructure); // ADC1 configuration ADC_StructInit(&ADC_InitStructure); ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; ADC_InitStructure.ADC_ScanConvMode = ENABLE; ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfConversion = 16; ADC_Init(ADC1, &ADC_InitStructure); ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGX, 1, ADC_SAMPLE_TIME); // magX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGX, 2, ADC_SAMPLE_TIME); // magX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGX, 3, ADC_SAMPLE_TIME); // magX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGX, 4, ADC_SAMPLE_TIME); // magX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGY, 5, ADC_SAMPLE_TIME); // magY ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGY, 6, ADC_SAMPLE_TIME); // magY ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGY, 7, ADC_SAMPLE_TIME); // magY ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGY, 8, ADC_SAMPLE_TIME); // magY ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGZ, 9, ADC_SAMPLE_TIME); // magZ ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGZ, 10, ADC_SAMPLE_TIME); // magZ ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGZ, 11, ADC_SAMPLE_TIME); // magZ ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_MAGZ, 12, ADC_SAMPLE_TIME); // magZ ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_RATEX, 13, ADC_SAMPLE_TIME); // rateX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_RATEX, 14, ADC_SAMPLE_TIME); // rateX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_RATEX, 15, ADC_SAMPLE_TIME); // rateX ADC_RegularChannelConfig(ADC1, ADC_CHANNEL_RATEX, 16, ADC_SAMPLE_TIME); // rateX // Enable ADC1 DMA since ADC1 is the Master ADC_DMACmd(ADC1, ENABLE); // ADC2 configuration ADC_StructInit(&ADC_InitStructure); ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; ADC_InitStructure.ADC_ScanConvMode = ENABLE; ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfConversion = 16; ADC_Init(ADC2, &ADC_InitStructure); ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_RATEY, 1, ADC_SAMPLE_TIME); // rateY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_RATEY, 2, ADC_SAMPLE_TIME); // rateY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_RATEY, 3, ADC_SAMPLE_TIME); // rateY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_RATEY, 4, ADC_SAMPLE_TIME); // rateY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCX, 5, ADC_SAMPLE_TIME); // accX ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCX, 6, ADC_SAMPLE_TIME); // accX ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCX, 7, ADC_SAMPLE_TIME); // accX ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCX, 8, ADC_SAMPLE_TIME); // accX ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCY, 9, ADC_SAMPLE_TIME); // accY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCY, 10, ADC_SAMPLE_TIME); // accY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCY, 11, ADC_SAMPLE_TIME); // accY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCY, 12, ADC_SAMPLE_TIME); // accY ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCZ, 13, ADC_SAMPLE_TIME); // accZ ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCZ, 14, ADC_SAMPLE_TIME); // accZ ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCZ, 15, ADC_SAMPLE_TIME); // accZ ADC_RegularChannelConfig(ADC2, ADC_CHANNEL_ACCZ, 16, ADC_SAMPLE_TIME); // accZ // ADC3 configuration ADC_StructInit(&ADC_InitStructure); ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; ADC_InitStructure.ADC_ScanConvMode = ENABLE; ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfConversion = 16; ADC_Init(ADC3, &ADC_InitStructure); ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_RATEZ, 1, ADC_SAMPLE_TIME); // rateZ ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_RATEZ, 2, ADC_SAMPLE_TIME); // rateZ ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_RATEZ, 3, ADC_SAMPLE_TIME); // rateZ ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_RATEZ, 4, ADC_SAMPLE_TIME); // rateZ ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_TEMP1, 5, ADC_SAMPLE_TIME); // temp1 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_TEMP2, 6, ADC_SAMPLE_TIME); // temp2 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES1, 7, ADC_SAMPLE_TIME); // pressure1 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES1, 8, ADC_SAMPLE_TIME); // pressure1 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES1, 9, ADC_SAMPLE_TIME); // pressure1 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES1, 10, ADC_SAMPLE_TIME); // pressure1 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_VIN, 11, ADC_SAMPLE_TIME); // Vin ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_TEMP3, 12, ADC_SAMPLE_TIME); // temp3 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES2, 13, ADC_SAMPLE_TIME); // pressure2 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES2, 14, ADC_SAMPLE_TIME); // pressure2 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES2, 15, ADC_SAMPLE_TIME); // pressure2 ADC_RegularChannelConfig(ADC3, ADC_CHANNEL_PRES2, 16, ADC_SAMPLE_TIME); // pressure2 // Enable DMA request after last transfer (Multi-ADC mode) ADC_MultiModeDMARequestAfterLastTransferCmd(ENABLE); // Enable ADC_Cmd(ADC1, ENABLE); ADC_Cmd(ADC2, ENABLE); ADC_Cmd(ADC3, ENABLE); adcData.adcFlag = CoCreateFlag(1, 0); adcTaskStack = aqStackInit(ADC_STACK_SIZE, "ADC"); adcData.adcTask = CoCreateTask(adcTaskCode, (void *)0, ADC_PRIORITY, &adcTaskStack[ADC_STACK_SIZE-1], ADC_STACK_SIZE); // Start ADC1 Software Conversion ADC_SoftwareStartConv(ADC1); yield(100); // set initial temperatures adcData.temp1 = adcIDGVoltsToTemp(adcData.voltages[ADC_VOLTS_TEMP1]); adcData.temp2 = adcIDGVoltsToTemp(adcData.voltages[ADC_VOLTS_TEMP2]); adcData.temp3 = adcT1VoltsToTemp(adcData.voltages[ADC_VOLTS_TEMP3]); analogData.vIn = adcVsenseToVin(adcData.voltages[ADC_VOLTS_VIN]); adcCalibOffsets(); }
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); }