void sensorsInit(void) { zeroSensorAccumulators(); // TODO allow user to select hardware if there are multiple choices if(mpu6050Detect(&gyro, &accel, cfg.mpu6050Scale)) { sensorsSet(SENSOR_ACC); } else if(!mpu3050Detect(&gyro)) { failureMode(3); } if(adxl345Detect(&accel)) { sensorsSet(SENSOR_ACC); } // At the moment we will do this after mpu6050 and overrride mpu6050 accel if detected if(mma8452Detect(&accel)) { sensorsSet(SENSOR_ACC); } gyro.init(); if(sensorsGet(SENSOR_ACC)) accel.init(); if(hmc5883Detect(&mag)) { mag.init(); sensorsSet(SENSOR_MAG); } #ifdef SONAR if(feature(FEATURE_PPM);) {
static void gpsFakeGPSUpdate(void) { if (millis() - gpsState.lastMessageMs > 100) { gpsSol.fixType = GPS_FIX_3D; gpsSol.numSat = 6; gpsSol.llh.lat = 509102311; gpsSol.llh.lon = -15349744; gpsSol.llh.alt = 0; gpsSol.groundSpeed = 0; gpsSol.groundCourse = 0; gpsSol.velNED[X] = 0; gpsSol.velNED[Y] = 0; gpsSol.velNED[Z] = 0; gpsSol.flags.validVelNE = 1; gpsSol.flags.validVelD = 1; gpsSol.flags.validEPE = 1; gpsSol.eph = 100; gpsSol.epv = 100; ENABLE_STATE(GPS_FIX); sensorsSet(SENSOR_GPS); onNewGPSData(); gpsState.lastLastMessageMs = gpsState.lastMessageMs; gpsState.lastMessageMs = millis(); gpsSetState(GPS_RECEIVING_DATA); } }
static void gpsHandleProtocol(void) { bool newDataReceived = false; // Call protocol-specific code if (gpsProviders[gpsState.gpsConfig->provider].read) { newDataReceived = gpsProviders[gpsState.gpsConfig->provider].read(); } // Received new update for solution data if (newDataReceived) { // Set GPS fix flag only if we have 3D fix if (gpsSol.fixType == GPS_FIX_3D) { ENABLE_STATE(GPS_FIX); } else { DISABLE_STATE(GPS_FIX); } // Update GPS coordinates etc sensorsSet(SENSOR_GPS); onNewGPSData(); // Update timeout gpsState.lastLastMessageMs = gpsState.lastMessageMs; gpsState.lastMessageMs = millis(); // Update statistics gpsStats.lastMessageDt = gpsState.lastMessageMs - gpsState.lastLastMessageMs; } }
bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) { // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function baroSensor_e baroHardware = baroHardwareToUse; #ifdef USE_BARO_BMP085 const bmp085Config_t *bmp085Config = NULL; #if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO) static const bmp085Config_t defaultBMP085Config = { .xclrIO = IO_TAG(BARO_XCLR_PIN), .eocIO = IO_TAG(BARO_EOC_PIN), }; bmp085Config = &defaultBMP085Config; #endif #endif switch (baroHardware) { case BARO_DEFAULT: ; // fallthough case BARO_BMP085: #ifdef USE_BARO_BMP085 if (bmp085Detect(bmp085Config, dev)) { baroHardware = BARO_BMP085; break; } #endif ; // fallthough case BARO_MS5611: #ifdef USE_BARO_MS5611 if (ms5611Detect(dev)) { baroHardware = BARO_MS5611; break; } #endif ; // fallthough case BARO_BMP280: #if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280) if (bmp280Detect(dev)) { baroHardware = BARO_BMP280; break; } #endif ; // fallthough case BARO_NONE: baroHardware = BARO_NONE; break; } if (baroHardware == BARO_NONE) { return false; } detectedSensors[SENSOR_INDEX_BARO] = baroHardware; sensorsSet(SENSOR_BARO); return true; }
//////////////////////////////////////////////////////////////////////////////////// // *** GPS INIT *** //////////////////////////////////////////////////////////////////////////////////// void gpsInit(uint32_t baudrate) // Called in Main { uint8_t i; uint32_t timeout; GPS_Present = 0; delay(2000); // let it init timeout = millis() + 12000; // 12 sec timeout while (!GPS_Present && millis() < timeout) // Repeat while no GPS Data { uart2Init(baudrate, GPS_NewData, false); // Set up Interrupthandler switch (cfg.gps_type) // GPS_NMEA = 0, GPS_UBLOX = 1, GPS_MTK16 = 2, GPS_MTK19 = 3, GPS_UBLOX_DUMB = 4 { case 0: // GPS_NMEA break; case 1: // GPS_UBLOX UbloxForceBaud(baudrate); for (i = 0; i < sizeof(ubloxInit); i++) { delay(7); uart2Write(ubloxInit[i]); // send ubx init binary } break; case 2: // GPS_MTK16 case 3: // GPS_MTK19 for (i = 0; i < 5; i++) { uart2ChangeBaud(init_speed[i]); delay(200); gpsPrint(MTK_BAUD_RATE_57600); } uart2ChangeBaud(57600); delay(200); gpsPrint(MTK_SET_BINARY); delay(200); gpsPrint(MTK_OUTPUT_5HZ); delay(200); gpsPrint(MTK_SBAS_INTEGRITYMODE); delay(200); gpsPrint(MTK_NAVTHRES_OFF); delay(200); gpsPrint(MTK_SBAS_ON); delay(200); gpsPrint(MTK_WAAS_ON); break; case 4: // GPS_UBLOX_DUMB = 4 break; } delay(1000); } if (GPS_Present) sensorsSet(SENSOR_GPS); // Do we get Data? Is GPS present? }
void sonarInit(const sonarHardware_t *sonarHardware) { sonarRange_t sonarRange; hcsr04_init(sonarHardware, &sonarRange); sensorsSet(SENSOR_SONAR); sonarMaxRangeCm = sonarRange.maxRangeCm; sonarCfAltCm = sonarMaxRangeCm / 2; sonarMaxTiltDeciDegrees = sonarRange.detectionConeExtendedDeciDegrees / 2; sonarMaxTiltCos = cos_approx(sonarMaxTiltDeciDegrees / 10.0f * RAD); sonarMaxAltWithTiltCm = sonarMaxRangeCm * sonarMaxTiltCos; calculatedAltitude = SONAR_OUT_OF_RANGE; }
void GPS_NewData(uint16_t c) { gps.bytes_rx ++; if (!GPS_newFrame(c)) return; gps.frames_rx ++; sensorsSet(SENSOR_GPS); xTimerReset(gpsLostTimer, 10); gps.update = (gps.update == 1 ? 0 : 1); if (flag(FLAG_GPS_FIX) && gps.numSat > 4) xSemaphoreGive(gpsSemaphore); }
/* * Detect which rangefinder is present */ static rangefinderType_e detectRangefinder(void) { rangefinderType_e rangefinderType = RANGEFINDER_NONE; if (feature(FEATURE_SONAR)) { // the user has set the sonar feature, so assume they have an HC-SR04 plugged in, // since there is no way to detect it rangefinderType = RANGEFINDER_HCSR04; } #ifdef USE_SONAR_SRF10 if (srf10_detect()) { // if an SFR10 sonar rangefinder is detected then use it in preference to the assumed HC-SR04 rangefinderType = RANGEFINDER_SRF10; } #endif detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderType; if (rangefinderType != RANGEFINDER_NONE) { sensorsSet(SENSOR_SONAR); } return rangefinderType; }
static void gpsNewData(uint16_t c) { if (!gpsNewFrame(c)) { return; } // new data received and parsed, we're in business gpsData.lastLastMessage = gpsData.lastMessage; gpsData.lastMessage = millis(); sensorsSet(SENSOR_GPS); if (GPS_update == 1) GPS_update = 0; else GPS_update = 1; #if 0 debug[3] = GPS_update; #endif onGpsNewData(); }
static void gpsHandleProtocol(void) { bool newDataReceived = false; // Call protocol-specific code if (gpsProviders[gpsState.gpsConfig->provider].read) { newDataReceived = gpsProviders[gpsState.gpsConfig->provider].read(); } // Received new update for solution data if (newDataReceived) { // Set GPS fix flag only if we have 3D fix if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) { ENABLE_STATE(GPS_FIX); } else { /* When no fix available - reset flags as well */ gpsSol.flags.validVelNE = 0; gpsSol.flags.validVelD = 0; gpsSol.flags.validEPE = 0; DISABLE_STATE(GPS_FIX); } // Update GPS coordinates etc sensorsSet(SENSOR_GPS); onNewGPSData(); // Update timeout gpsState.lastLastMessageMs = gpsState.lastMessageMs; gpsState.lastMessageMs = millis(); // Update statistics gpsStats.lastMessageDt = gpsState.lastMessageMs - gpsState.lastLastMessageMs; } }
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(); } }
STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev) { gyroHardware_e gyroHardware = GYRO_DEFAULT; switch (gyroHardware) { case GYRO_DEFAULT: FALLTHROUGH; #ifdef USE_GYRO_MPU6050 case GYRO_MPU6050: if (mpu6050GyroDetect(dev)) { gyroHardware = GYRO_MPU6050; break; } FALLTHROUGH; #endif #ifdef USE_GYRO_L3G4200D case GYRO_L3G4200D: if (l3g4200dDetect(dev)) { gyroHardware = GYRO_L3G4200D; break; } FALLTHROUGH; #endif #ifdef USE_GYRO_MPU3050 case GYRO_MPU3050: if (mpu3050Detect(dev)) { gyroHardware = GYRO_MPU3050; break; } FALLTHROUGH; #endif #ifdef USE_GYRO_L3GD20 case GYRO_L3GD20: if (l3gd20GyroDetect(dev)) { gyroHardware = GYRO_L3GD20; break; } FALLTHROUGH; #endif #ifdef USE_GYRO_SPI_MPU6000 case GYRO_MPU6000: if (mpu6000SpiGyroDetect(dev)) { gyroHardware = GYRO_MPU6000; break; } FALLTHROUGH; #endif #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) case GYRO_MPU6500: case GYRO_ICM20601: case GYRO_ICM20602: case GYRO_ICM20608G: #ifdef USE_GYRO_SPI_MPU6500 if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) { #else if (mpu6500GyroDetect(dev)) { #endif switch (dev->mpuDetectionResult.sensor) { case MPU_9250_SPI: gyroHardware = GYRO_MPU9250; break; case ICM_20601_SPI: gyroHardware = GYRO_ICM20601; break; case ICM_20602_SPI: gyroHardware = GYRO_ICM20602; break; case ICM_20608_SPI: gyroHardware = GYRO_ICM20608G; break; default: gyroHardware = GYRO_MPU6500; } break; } FALLTHROUGH; #endif #ifdef USE_GYRO_SPI_MPU9250 case GYRO_MPU9250: if (mpu9250SpiGyroDetect(dev)) { gyroHardware = GYRO_MPU9250; break; } FALLTHROUGH; #endif #ifdef USE_GYRO_SPI_ICM20649 case GYRO_ICM20649: if (icm20649SpiGyroDetect(dev)) { gyroHardware = GYRO_ICM20649; break; } FALLTHROUGH; #endif #ifdef USE_GYRO_SPI_ICM20689 case GYRO_ICM20689: if (icm20689SpiGyroDetect(dev)) { gyroHardware = GYRO_ICM20689; break; } FALLTHROUGH; #endif #ifdef USE_ACCGYRO_BMI160 case GYRO_BMI160: if (bmi160SpiGyroDetect(dev)) { gyroHardware = GYRO_BMI160; break; } FALLTHROUGH; #endif #ifdef USE_FAKE_GYRO case GYRO_FAKE: if (fakeGyroDetect(dev)) { gyroHardware = GYRO_FAKE; break; } FALLTHROUGH; #endif default: gyroHardware = GYRO_NONE; } if (gyroHardware != GYRO_NONE) { sensorsSet(SENSOR_GYRO); } return gyroHardware; } static void gyroPreInitSensor(const gyroDeviceConfig_t *config) { #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) mpuPreInit(config); #else UNUSED(config); #endif }
void gpsInit(uint32_t baudrate) { uart2Init(baudrate, GPS_NewData); sensorsSet(SENSOR_GPS); }
static void detectMag(magSensor_e magHardwareToUse) { magSensor_e magHardware; #ifdef USE_MAG_HMC5883 const hmc5883Config_t *hmc5883Config = 0; #ifdef NAZE static const hmc5883Config_t nazeHmc5883Config_v1_v4 = { .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB, .gpioPin = Pin_12, .gpioPort = GPIOB, /* Disabled for v4 needs more work. .exti_port_source = GPIO_PortSourceGPIOB, .exti_pin_source = GPIO_PinSource12, .exti_line = EXTI_Line12, .exti_irqn = EXTI15_10_IRQn */ }; static const hmc5883Config_t nazeHmc5883Config_v5 = { .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC, .gpioPin = Pin_14, .gpioPort = GPIOC, .exti_port_source = GPIO_PortSourceGPIOC, .exti_line = EXTI_Line14, .exti_pin_source = GPIO_PinSource14, .exti_irqn = EXTI15_10_IRQn }; if (hardwareRevision < NAZE32_REV5) { hmc5883Config = &nazeHmc5883Config_v1_v4; } else { hmc5883Config = &nazeHmc5883Config_v5; } #endif #ifdef SPRACINGF3 static const hmc5883Config_t spRacingF3Hmc5883Config = { .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, .gpioPin = Pin_14, .gpioPort = GPIOC, .exti_port_source = EXTI_PortSourceGPIOC, .exti_pin_source = EXTI_PinSource14, .exti_line = EXTI_Line14, .exti_irqn = EXTI15_10_IRQn }; hmc5883Config = &spRacingF3Hmc5883Config; #endif #endif retry: magAlign = ALIGN_DEFAULT; switch(magHardwareToUse) { case MAG_DEFAULT: ; // fallthrough case MAG_HMC5883: #ifdef USE_MAG_HMC5883 if (hmc5883lDetect(&mag, hmc5883Config)) { #ifdef MAG_HMC5883_ALIGN magAlign = MAG_HMC5883_ALIGN; #endif magHardware = MAG_HMC5883; break; } #endif ; // fallthrough case MAG_AK8975: #ifdef USE_MAG_AK8975 if (ak8975detect(&mag)) { #ifdef MAG_AK8975_ALIGN magAlign = MAG_AK8975_ALIGN; #endif magHardware = MAG_AK8975; break; } #endif ; // fallthrough case MAG_NONE: magHardware = MAG_NONE; break; } if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) { // Nothing was found and we have a forced sensor that isn't present. magHardwareToUse = MAG_DEFAULT; goto retry; } if (magHardware == MAG_NONE) { return; } detectedSensors[SENSOR_INDEX_MAG] = magHardware; sensorsSet(SENSOR_MAG); }
static void detectAcc(accelerationSensor_e accHardwareToUse) { accelerationSensor_e accHardware; #ifdef USE_ACC_ADXL345 drv_adxl345_config_t acc_params; #endif retry: accAlign = ALIGN_DEFAULT; switch (accHardwareToUse) { case ACC_DEFAULT: ; // fallthrough case ACC_ADXL345: // ADXL345 #ifdef USE_ACC_ADXL345 acc_params.useFifo = false; acc_params.dataRate = 800; // unused currently #ifdef NAZE if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) { #else if (adxl345Detect(&acc_params, &acc)) { #endif #ifdef ACC_ADXL345_ALIGN accAlign = ACC_ADXL345_ALIGN; #endif accHardware = ACC_ADXL345; break; } #endif ; // fallthrough case ACC_LSM303DLHC: #ifdef USE_ACC_LSM303DLHC if (lsm303dlhcAccDetect(&acc)) { #ifdef ACC_LSM303DLHC_ALIGN accAlign = ACC_LSM303DLHC_ALIGN; #endif accHardware = ACC_LSM303DLHC; break; } #endif ; // fallthrough case ACC_MPU6050: // MPU6050 #ifdef USE_ACC_MPU6050 if (mpu6050AccDetect(&acc)) { #ifdef ACC_MPU6050_ALIGN accAlign = ACC_MPU6050_ALIGN; #endif accHardware = ACC_MPU6050; break; } #endif ; // fallthrough case ACC_MMA8452: // MMA8452 #ifdef USE_ACC_MMA8452 #ifdef NAZE // Not supported with this frequency if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) { #else if (mma8452Detect(&acc)) { #endif #ifdef ACC_MMA8452_ALIGN accAlign = ACC_MMA8452_ALIGN; #endif accHardware = ACC_MMA8452; break; } #endif ; // fallthrough case ACC_BMA280: // BMA280 #ifdef USE_ACC_BMA280 if (bma280Detect(&acc)) { #ifdef ACC_BMA280_ALIGN accAlign = ACC_BMA280_ALIGN; #endif accHardware = ACC_BMA280; break; } #endif ; // fallthrough case ACC_MPU6000: #ifdef USE_ACC_SPI_MPU6000 if (mpu6000SpiAccDetect(&acc)) { #ifdef ACC_MPU6000_ALIGN accAlign = ACC_MPU6000_ALIGN; #endif accHardware = ACC_MPU6000; break; } #endif ; // fallthrough case ACC_MPU6500: #ifdef USE_ACC_MPU6500 #ifdef USE_ACC_SPI_MPU6500 if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc)) #else if (mpu6500AccDetect(&acc)) #endif { #ifdef ACC_MPU6500_ALIGN accAlign = ACC_MPU6500_ALIGN; #endif accHardware = ACC_MPU6500; break; } #endif ; // fallthrough case ACC_FAKE: #ifdef USE_FAKE_ACC if (fakeAccDetect(&acc)) { accHardware = ACC_FAKE; break; } #endif ; // fallthrough case ACC_NONE: // disable ACC accHardware = ACC_NONE; break; } // Found anything? Check if error or ACC is really missing. if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) { // Nothing was found and we have a forced sensor that isn't present. accHardwareToUse = ACC_DEFAULT; goto retry; } if (accHardware == ACC_NONE) { return; } detectedSensors[SENSOR_INDEX_ACC] = accHardware; sensorsSet(SENSOR_ACC); } static void detectBaro(baroSensor_e baroHardwareToUse) { #ifndef BARO UNUSED(baroHardwareToUse); #else // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function baroSensor_e baroHardware = baroHardwareToUse; #ifdef USE_BARO_BMP085 const bmp085Config_t *bmp085Config = NULL; #if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO) static const bmp085Config_t defaultBMP085Config = { .gpioAPB2Peripherals = BARO_APB2_PERIPHERALS, .xclrGpioPin = BARO_XCLR_PIN, .xclrGpioPort = BARO_XCLR_GPIO, .eocGpioPin = BARO_EOC_PIN, .eocGpioPort = BARO_EOC_GPIO }; bmp085Config = &defaultBMP085Config; #endif #ifdef NAZE if (hardwareRevision == NAZE32) { bmp085Disable(bmp085Config); } #endif #endif switch (baroHardware) { case BARO_DEFAULT: ; // fallthough case BARO_MS5611: #ifdef USE_BARO_MS5611 if (ms5611Detect(&baro)) { baroHardware = BARO_MS5611; break; } #endif ; // fallthough case BARO_BMP085: #ifdef USE_BARO_BMP085 if (bmp085Detect(bmp085Config, &baro)) { baroHardware = BARO_BMP085; break; } #endif ; // fallthough case BARO_BMP280: #ifdef USE_BARO_BMP280 if (bmp280Detect(&baro)) { baroHardware = BARO_BMP280; break; } #endif ; // fallthrough case BARO_FAKE: #ifdef USE_FAKE_BARO if (fakeBaroDetect(&baro)) { baroHardware = BARO_FAKE; break; } #endif ; // fallthrough case BARO_NONE: baroHardware = BARO_NONE; break; } if (baroHardware == BARO_NONE) { return; } detectedSensors[SENSOR_INDEX_BARO] = baroHardware; sensorsSet(SENSOR_BARO); #endif } static void detectMag(magSensor_e magHardwareToUse) { magSensor_e magHardware; #ifdef USE_MAG_HMC5883 const hmc5883Config_t *hmc5883Config = 0; #ifdef NAZE static const hmc5883Config_t nazeHmc5883Config_v1_v4 = { .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB, .gpioPin = Pin_12, .gpioPort = GPIOB, /* Disabled for v4 needs more work. .exti_port_source = GPIO_PortSourceGPIOB, .exti_pin_source = GPIO_PinSource12, .exti_line = EXTI_Line12, .exti_irqn = EXTI15_10_IRQn */ }; static const hmc5883Config_t nazeHmc5883Config_v5 = { .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC, .gpioPin = Pin_14, .gpioPort = GPIOC, .exti_port_source = GPIO_PortSourceGPIOC, .exti_line = EXTI_Line14, .exti_pin_source = GPIO_PinSource14, .exti_irqn = EXTI15_10_IRQn }; if (hardwareRevision < NAZE32_REV5) { hmc5883Config = &nazeHmc5883Config_v1_v4; } else { hmc5883Config = &nazeHmc5883Config_v5; } #endif #ifdef SPRACINGF3 static const hmc5883Config_t spRacingF3Hmc5883Config = { .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, .gpioPin = Pin_14, .gpioPort = GPIOC, .exti_port_source = EXTI_PortSourceGPIOC, .exti_pin_source = EXTI_PinSource14, .exti_line = EXTI_Line14, .exti_irqn = EXTI15_10_IRQn }; hmc5883Config = &spRacingF3Hmc5883Config; #endif #endif retry: magAlign = ALIGN_DEFAULT; switch(magHardwareToUse) { case MAG_DEFAULT: ; // fallthrough case MAG_HMC5883: #ifdef USE_MAG_HMC5883 if (hmc5883lDetect(&mag, hmc5883Config)) { #ifdef MAG_HMC5883_ALIGN magAlign = MAG_HMC5883_ALIGN; #endif magHardware = MAG_HMC5883; break; } #endif ; // fallthrough case MAG_AK8975: #ifdef USE_MAG_AK8975 if (ak8975detect(&mag)) { #ifdef MAG_AK8975_ALIGN magAlign = MAG_AK8975_ALIGN; #endif magHardware = MAG_AK8975; break; } #endif ; // fallthrough case MAG_GPS: #ifdef GPS if (gpsMagDetect(&mag)) { #ifdef MAG_GPS_ALIGN magAlign = MAG_GPS_ALIGN; #endif magHardware = MAG_GPS; break; } #endif ; // fallthrough case MAG_MAG3110: #ifdef USE_MAG_MAG3110 if (mag3110detect(&mag)) { #ifdef MAG_MAG3110_ALIGN magAlign = MAG_MAG3110_ALIGN; #endif magHardware = MAG_MAG3110; break; } #endif ; // fallthrough case MAG_FAKE: #ifdef USE_FAKE_MAG if (fakeMagDetect(&mag)) { magHardware = MAG_FAKE; break; } #endif ; // fallthrough case MAG_NONE: magHardware = MAG_NONE; break; } if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) { // Nothing was found and we have a forced sensor that isn't present. magHardwareToUse = MAG_DEFAULT; goto retry; } if (magHardware == MAG_NONE) { return; } detectedSensors[SENSOR_INDEX_MAG] = magHardware; sensorsSet(SENSOR_MAG); }
void Sonar_init(void) { hcsr04_init(sonar_rc78); sensorsSet(SENSOR_SONAR); sonarAlt = 0; }
void init(void) { uint8_t i; drv_pwm_config_t pwm_params; bool sensorsOK = false; initPrintfSupport(); initEEPROM(); ensureEEPROMContainsValidData(); readEEPROM(); #ifdef STM32F303 // start fpu SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2)); #endif #ifdef STM32F303xC SetSysClock(); #endif #ifdef STM32F10X // Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers // Configure the Flash Latency cycles and enable prefetch buffer SetSysClock(masterConfig.emf_avoidance); #endif #ifdef NAZE detectHardwareRevision(); #endif systemInit(); #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 ledInit(); #ifdef BEEPER beeperConfig_t beeperConfig = { .gpioMode = Mode_Out_OD, .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, .gpioPeripheral = BEEP_PERIPHERAL, .isInverted = false }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. beeperConfig.gpioMode = Mode_Out_PP; beeperConfig.isInverted = true; } #endif beeperInit(&beeperConfig); #endif #ifdef INVERTER initInverter(); #endif #ifdef USE_SPI spiInit(SPI1); spiInit(SPI2); #endif #ifdef NAZE updateHardwareRevision(); #endif #ifdef USE_I2C #ifdef NAZE if (hardwareRevision != NAZE32_SP) { i2cInit(I2C_DEVICE); } #else // Configure the rest of the stuff i2cInit(I2C_DEVICE); #endif #endif #if !defined(SPARKY) drv_adc_config_t adc_params; 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 // 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(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination); // 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(); mixerInit(masterConfig.mixerMode, masterConfig.customMixer); #ifdef MAG if (sensors(SENSOR_MAG)) compassInit(); #endif serialInit(&masterConfig.serialConfig); memset(&pwm_params, 0, sizeof(pwm_params)); // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING) pwm_params.airplane = true; else pwm_params.airplane = false; #if defined(SERIAL_PORT_USART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #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); pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.useServos = isMixerUsingServos(); pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below) if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc; pwmRxInit(masterConfig.inputFilteringMode); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); mixerUsePWMOutputConfiguration(pwmOutputConfiguration); failsafe = failsafeInit(&masterConfig.rxConfig); beepcodeInit(failsafe); rxInit(&masterConfig.rxConfig, failsafe); #ifdef GPS if (feature(FEATURE_GPS)) { gpsInit( &masterConfig.serialConfig, &masterConfig.gpsConfig ); navigationInit( ¤tProfile->gpsProfile, ¤tProfile->pidProfile ); } #endif #ifdef SONAR if (feature(FEATURE_SONAR)) { Sonar_init(); } #endif #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors, failsafe); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) initTelemetry(); #endif previousTime = micros(); if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); #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. // Check battery type/voltage if (feature(FEATURE_VBAT)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY displayShowFixedPage(PAGE_GPS); #else displayEnablePageCycling(); #endif } #endif } #ifdef SOFTSERIAL_LOOPBACK void processLoopback(void) { if (loopbackPort) { uint8_t bytesWaiting; while ((bytesWaiting = serialTotalBytesWaiting(loopbackPort))) { uint8_t b = serialRead(loopbackPort); serialWrite(loopbackPort, b); }; } } #else #define processLoopback() #endif int main(void) { init(); while (1) { loop(); processLoopback(); } }
static void detectAcc(accelerationSensor_e accHardwareToUse) { accelerationSensor_e accHardware; #ifdef USE_ACC_ADXL345 drv_adxl345_config_t acc_params; #endif retry: accAlign = ALIGN_DEFAULT; switch (accHardwareToUse) { case ACC_DEFAULT: ; // fallthrough case ACC_ADXL345: // ADXL345 #ifdef USE_ACC_ADXL345 acc_params.useFifo = false; acc_params.dataRate = 800; // unused currently #ifdef NAZE if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) { #else if (adxl345Detect(&acc_params, &acc)) { #endif #ifdef ACC_ADXL345_ALIGN accAlign = ACC_ADXL345_ALIGN; #endif accHardware = ACC_ADXL345; break; } #endif ; // fallthrough case ACC_LSM303DLHC: #ifdef USE_ACC_LSM303DLHC if (lsm303dlhcAccDetect(&acc)) { #ifdef ACC_LSM303DLHC_ALIGN accAlign = ACC_LSM303DLHC_ALIGN; #endif accHardware = ACC_LSM303DLHC; break; } #endif ; // fallthrough case ACC_MPU6050: // MPU6050 #ifdef USE_ACC_MPU6050 if (mpu6050AccDetect(&acc)) { #ifdef ACC_MPU6050_ALIGN accAlign = ACC_MPU6050_ALIGN; #endif accHardware = ACC_MPU6050; break; } #endif ; // fallthrough case ACC_MMA8452: // MMA8452 #ifdef USE_ACC_MMA8452 #ifdef NAZE // Not supported with this frequency if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) { #else if (mma8452Detect(&acc)) { #endif #ifdef ACC_MMA8452_ALIGN accAlign = ACC_MMA8452_ALIGN; #endif accHardware = ACC_MMA8452; break; } #endif ; // fallthrough case ACC_BMA280: // BMA280 #ifdef USE_ACC_BMA280 if (bma280Detect(&acc)) { #ifdef ACC_BMA280_ALIGN accAlign = ACC_BMA280_ALIGN; #endif accHardware = ACC_BMA280; break; } #endif ; // fallthrough case ACC_MPU6000: #ifdef USE_ACC_SPI_MPU6000 if (mpu6000SpiAccDetect(&acc)) { #ifdef ACC_MPU6000_ALIGN accAlign = ACC_MPU6000_ALIGN; #endif accHardware = ACC_MPU6000; break; } #endif ; // fallthrough case ACC_MPU6500: #if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) #ifdef USE_ACC_SPI_MPU6500 if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc)) #else if (mpu6500AccDetect(&acc)) #endif { #ifdef ACC_MPU6500_ALIGN accAlign = ACC_MPU6500_ALIGN; #endif accHardware = ACC_MPU6500; break; } #endif ; // fallthrough case ACC_MPU9250: #ifdef USE_ACC_SPI_MPU9250 if (mpu9250SpiAccDetect(&acc)) { accHardware = ACC_MPU9250; #ifdef ACC_MPU9250_ALIGN accAlign = ACC_MPU9250_ALIGN; #endif break; } #endif ; // fallthrough case ACC_FAKE: #ifdef USE_FAKE_ACC if (fakeAccDetect(&acc)) { accHardware = ACC_FAKE; break; } #endif ; // fallthrough case ACC_NONE: // disable ACC accHardware = ACC_NONE; break; } // Found anything? Check if error or ACC is really missing. if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) { // Nothing was found and we have a forced sensor that isn't present. accHardwareToUse = ACC_DEFAULT; goto retry; } if (accHardware == ACC_NONE) { return; } detectedSensors[SENSOR_INDEX_ACC] = accHardware; sensorsSet(SENSOR_ACC); } static void detectBaro(baroSensor_e baroHardwareToUse) { #ifndef BARO UNUSED(baroHardwareToUse); #else // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function baroSensor_e baroHardware = baroHardwareToUse; #ifdef USE_BARO_BMP085 const bmp085Config_t *bmp085Config = NULL; #if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO) static const bmp085Config_t defaultBMP085Config = { .xclrIO = IO_TAG(BARO_XCLR_PIN), .eocIO = IO_TAG(BARO_EOC_PIN), }; bmp085Config = &defaultBMP085Config; #endif #ifdef NAZE if (hardwareRevision == NAZE32) { bmp085Disable(bmp085Config); } #endif #endif switch (baroHardware) { case BARO_DEFAULT: ; // fallthough case BARO_BMP085: #ifdef USE_BARO_BMP085 if (bmp085Detect(bmp085Config, &baro)) { baroHardware = BARO_BMP085; break; } #endif ; // fallthough case BARO_MS5611: #ifdef USE_BARO_MS5611 if (ms5611Detect(&baro)) { baroHardware = BARO_MS5611; break; } #endif ; // fallthough case BARO_BMP280: #ifdef USE_BARO_BMP280 if (bmp280Detect(&baro)) { baroHardware = BARO_BMP280; break; } #endif ; // fallthough case BARO_NONE: baroHardware = BARO_NONE; break; } if (baroHardware == BARO_NONE) { return; } detectedSensors[SENSOR_INDEX_BARO] = baroHardware; sensorsSet(SENSOR_BARO); #endif } static void detectMag(magSensor_e magHardwareToUse) { magSensor_e magHardware; #ifdef USE_MAG_HMC5883 const hmc5883Config_t *hmc5883Config = 0; #ifdef NAZE // TODO remove this target specific define static const hmc5883Config_t nazeHmc5883Config_v1_v4 = { .intTag = IO_TAG(PB12) /* perhaps disabled? */ }; static const hmc5883Config_t nazeHmc5883Config_v5 = { .intTag = IO_TAG(MAG_INT_EXTI) }; if (hardwareRevision < NAZE32_REV5) { hmc5883Config = &nazeHmc5883Config_v1_v4; } else { hmc5883Config = &nazeHmc5883Config_v5; } #endif #ifdef MAG_INT_EXTI static const hmc5883Config_t extiHmc5883Config = { .intTag = IO_TAG(MAG_INT_EXTI) }; hmc5883Config = &extiHmc5883Config; #endif #endif retry: magAlign = ALIGN_DEFAULT; switch(magHardwareToUse) { case MAG_DEFAULT: ; // fallthrough case MAG_HMC5883: #ifdef USE_MAG_HMC5883 if (hmc5883lDetect(&mag, hmc5883Config)) { #ifdef MAG_HMC5883_ALIGN magAlign = MAG_HMC5883_ALIGN; #endif magHardware = MAG_HMC5883; break; } #endif ; // fallthrough case MAG_AK8975: #ifdef USE_MAG_AK8975 if (ak8975Detect(&mag)) { #ifdef MAG_AK8975_ALIGN magAlign = MAG_AK8975_ALIGN; #endif magHardware = MAG_AK8975; break; } #endif ; // fallthrough case MAG_AK8963: #ifdef USE_MAG_AK8963 if (ak8963Detect(&mag)) { #ifdef MAG_AK8963_ALIGN magAlign = MAG_AK8963_ALIGN; #endif magHardware = MAG_AK8963; break; } #endif ; // fallthrough case MAG_NONE: magHardware = MAG_NONE; break; } if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) { // Nothing was found and we have a forced sensor that isn't present. magHardwareToUse = MAG_DEFAULT; goto retry; } if (magHardware == MAG_NONE) { return; } detectedSensors[SENSOR_INDEX_MAG] = magHardware; sensorsSet(SENSOR_MAG); }
static void detectAcc(accelerationSensor_e accHardwareToUse) { accelerationSensor_e accHardware; #ifdef USE_ACC_ADXL345 drv_adxl345_config_t acc_params; #endif retry: accAlign = ALIGN_DEFAULT; switch (accHardwareToUse) { case ACC_DEFAULT: ; // fallthrough case ACC_ADXL345: // ADXL345 #ifdef USE_ACC_ADXL345 acc_params.useFifo = false; acc_params.dataRate = 800; // unused currently #ifdef NAZE if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) { #else if (adxl345Detect(&acc_params, &acc)) { #endif #ifdef ACC_ADXL345_ALIGN accAlign = ACC_ADXL345_ALIGN; #endif accHardware = ACC_ADXL345; break; } #endif ; // fallthrough case ACC_LSM303DLHC: #ifdef USE_ACC_LSM303DLHC if (lsm303dlhcAccDetect(&acc)) { #ifdef ACC_LSM303DLHC_ALIGN accAlign = ACC_LSM303DLHC_ALIGN; #endif accHardware = ACC_LSM303DLHC; break; } #endif ; // fallthrough case ACC_MPU6050: // MPU6050 #ifdef USE_ACC_MPU6050 if (mpu6050AccDetect(selectMPU6050Config(), &acc)) { #ifdef ACC_MPU6050_ALIGN accAlign = ACC_MPU6050_ALIGN; #endif accHardware = ACC_MPU6050; break; } #endif ; // fallthrough case ACC_MMA8452: // MMA8452 #ifdef USE_ACC_MMA8452 #ifdef NAZE // Not supported with this frequency if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) { #else if (mma8452Detect(&acc)) { #endif #ifdef ACC_MMA8452_ALIGN accAlign = ACC_MMA8452_ALIGN; #endif accHardware = ACC_MMA8452; break; } #endif ; // fallthrough case ACC_BMA280: // BMA280 #ifdef USE_ACC_BMA280 if (bma280Detect(&acc)) { #ifdef ACC_BMA280_ALIGN accAlign = ACC_BMA280_ALIGN; #endif accHardware = ACC_BMA280; break; } #endif ; // fallthrough case ACC_SPI_MPU6000: #ifdef USE_ACC_SPI_MPU6000 if (mpu6000SpiAccDetect(&acc)) { #ifdef ACC_SPI_MPU6000_ALIGN accAlign = ACC_SPI_MPU6000_ALIGN; #endif accHardware = ACC_SPI_MPU6000; break; } #endif ; // fallthrough case ACC_SPI_MPU6500: #ifdef USE_ACC_SPI_MPU6500 #ifdef NAZE if (hardwareRevision == NAZE32_SP && mpu6500SpiAccDetect(&acc)) { #else if (mpu6500SpiAccDetect(&acc)) { #endif #ifdef ACC_SPI_MPU6500_ALIGN accAlign = ACC_SPI_MPU6500_ALIGN; #endif accHardware = ACC_SPI_MPU6500; break; } #endif ; // fallthrough case ACC_FAKE: #ifdef USE_FAKE_ACC if (fakeAccDetect(&acc)) { accHardware = ACC_FAKE; break; } #endif ; // fallthrough case ACC_NONE: // disable ACC accHardware = ACC_NONE; break; } // Found anything? Check if error or ACC is really missing. if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) { // Nothing was found and we have a forced sensor that isn't present. accHardwareToUse = ACC_DEFAULT; goto retry; } if (accHardware == ACC_NONE) { return; } detectedSensors[SENSOR_INDEX_ACC] = accHardware; sensorsSet(SENSOR_ACC); } static void detectBaro(baroSensor_e baroHardwareToUse) { #ifdef BARO // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function baroSensor_e baroHardware = baroHardwareToUse; #ifdef USE_BARO_BMP085 const bmp085Config_t *bmp085Config = NULL; #if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO) static const bmp085Config_t defaultBMP085Config = { .gpioAPB2Peripherals = BARO_APB2_PERIPHERALS, .xclrGpioPin = BARO_XCLR_PIN, .xclrGpioPort = BARO_XCLR_GPIO, .eocGpioPin = BARO_EOC_PIN, .eocGpioPort = BARO_EOC_GPIO }; bmp085Config = &defaultBMP085Config; #endif #ifdef NAZE if (hardwareRevision == NAZE32) { bmp085Disable(bmp085Config); } #endif #endif switch (baroHardware) { case BARO_DEFAULT: ; // fallthough case BARO_MS5611: #ifdef USE_BARO_MS5611 if (ms5611Detect(&baro)) { baroHardware = BARO_MS5611; break; } #endif ; // fallthough case BARO_BMP085: #ifdef USE_BARO_BMP085 if (bmp085Detect(bmp085Config, &baro)) { baroHardware = BARO_BMP085; break; } #endif case BARO_NONE: baroHardware = BARO_NONE; break; } if (baroHardware == BARO_NONE) { return; } detectedSensors[SENSOR_INDEX_BARO] = baroHardware; sensorsSet(SENSOR_BARO); #endif } static void detectMag(magSensor_e magHardwareToUse) { magSensor_e magHardware; #ifdef USE_MAG_HMC5883 const hmc5883Config_t *hmc5883Config = 0; #ifdef NAZE static const hmc5883Config_t nazeHmc5883Config_v1_v4 = { .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB, .gpioPin = Pin_12, .gpioPort = GPIOB, /* Disabled for v4 needs more work. .exti_port_source = GPIO_PortSourceGPIOB, .exti_pin_source = GPIO_PinSource12, .exti_line = EXTI_Line12, .exti_irqn = EXTI15_10_IRQn */ }; static const hmc5883Config_t nazeHmc5883Config_v5 = { .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC, .gpioPin = Pin_14, .gpioPort = GPIOC, .exti_port_source = GPIO_PortSourceGPIOC, .exti_line = EXTI_Line14, .exti_pin_source = GPIO_PinSource14, .exti_irqn = EXTI15_10_IRQn }; if (hardwareRevision < NAZE32_REV5) { hmc5883Config = &nazeHmc5883Config_v1_v4; } else { hmc5883Config = &nazeHmc5883Config_v5; } #endif #ifdef SPRACINGF3 static const hmc5883Config_t spRacingF3Hmc5883Config = { .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, .gpioPin = Pin_14, .gpioPort = GPIOC, .exti_port_source = EXTI_PortSourceGPIOC, .exti_pin_source = EXTI_PinSource14, .exti_line = EXTI_Line14, .exti_irqn = EXTI15_10_IRQn }; hmc5883Config = &spRacingF3Hmc5883Config; #endif #endif retry: magAlign = ALIGN_DEFAULT; switch(magHardwareToUse) { case MAG_DEFAULT: ; // fallthrough case MAG_HMC5883: #ifdef USE_MAG_HMC5883 if (hmc5883lDetect(&mag, hmc5883Config)) { #ifdef MAG_HMC5883_ALIGN magAlign = MAG_HMC5883_ALIGN; #endif magHardware = MAG_HMC5883; break; } #endif ; // fallthrough case MAG_AK8975: #ifdef USE_MAG_AK8975 if (ak8975detect(&mag)) { #ifdef MAG_AK8975_ALIGN magAlign = MAG_AK8975_ALIGN; #endif magHardware = MAG_AK8975; break; } #endif ; // fallthrough case MAG_NONE: magHardware = MAG_NONE; break; } if (magHardware == MAG_NONE && magHardwareToUse != MAG_DEFAULT && magHardwareToUse != MAG_NONE) { // Nothing was found and we have a forced sensor that isn't present. magHardwareToUse = MAG_DEFAULT; goto retry; } if (magHardware == MAG_NONE) { return; } detectedSensors[SENSOR_INDEX_MAG] = magHardware; sensorsSet(SENSOR_MAG); } void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) { if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) { gyroAlign = sensorAlignmentConfig->gyro_align; } if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) { accAlign = sensorAlignmentConfig->acc_align; } if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) { magAlign = sensorAlignmentConfig->mag_align; } } bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig) { int16_t deg, min; memset(&acc, 0, sizeof(acc)); memset(&gyro, 0, sizeof(gyro)); if (!detectGyro(gyroLpf)) { return false; } detectAcc(accHardwareToUse); detectBaro(baroHardwareToUse); // Now time to init things, acc first if (sensors(SENSOR_ACC)) acc.init(); // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. gyro.init(); detectMag(magHardwareToUse); reconfigureAlignment(sensorAlignmentConfig); // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c if (sensors(SENSOR_MAG)) { // calculate magnetic declination deg = magDeclinationFromConfig / 100; min = magDeclinationFromConfig % 100; magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units } else { magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. } return true; }
bool detectGyro(uint16_t gyroLpf) { gyroSensor_e gyroHardware = GYRO_DEFAULT; gyroAlign = ALIGN_DEFAULT; switch(gyroHardware) { case GYRO_DEFAULT: ; // fallthrough case GYRO_MPU6050: #ifdef USE_GYRO_MPU6050 if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) { #ifdef GYRO_MPU6050_ALIGN gyroHardware = GYRO_MPU6050; gyroAlign = GYRO_MPU6050_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_L3G4200D: #ifdef USE_GYRO_L3G4200D if (l3g4200dDetect(&gyro, gyroLpf)) { #ifdef GYRO_L3G4200D_ALIGN gyroHardware = GYRO_L3G4200D; gyroAlign = GYRO_L3G4200D_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_MPU3050: #ifdef USE_GYRO_MPU3050 if (mpu3050Detect(&gyro, gyroLpf)) { #ifdef GYRO_MPU3050_ALIGN gyroHardware = GYRO_MPU3050; gyroAlign = GYRO_MPU3050_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_L3GD20: #ifdef USE_GYRO_L3GD20 if (l3gd20Detect(&gyro, gyroLpf)) { #ifdef GYRO_L3GD20_ALIGN gyroHardware = GYRO_L3GD20; gyroAlign = GYRO_L3GD20_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_SPI_MPU6000: #ifdef USE_GYRO_SPI_MPU6000 if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) { #ifdef GYRO_SPI_MPU6000_ALIGN gyroHardware = GYRO_SPI_MPU6000; gyroAlign = GYRO_SPI_MPU6000_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_SPI_MPU6500: #ifdef USE_GYRO_SPI_MPU6500 #ifdef USE_HARDWARE_REVISION_DETECTION spiBusInit(); #endif #ifdef NAZE if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) { #ifdef GYRO_SPI_MPU6500_ALIGN gyroHardware = GYRO_SPI_MPU6500; gyroAlign = GYRO_SPI_MPU6500_ALIGN; #endif break; } #else if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) { #ifdef GYRO_SPI_MPU6500_ALIGN gyroHardware = GYRO_SPI_MPU6500; gyroAlign = GYRO_SPI_MPU6500_ALIGN; #endif break; } #endif #endif ; // fallthrough case GYRO_FAKE: #ifdef USE_FAKE_GYRO if (fakeGyroDetect(&gyro, gyroLpf)) { gyroHardware = GYRO_FAKE; break; } #endif ; // fallthrough case GYRO_NONE: gyroHardware = GYRO_NONE; } if (gyroHardware == GYRO_NONE) { return false; } detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; sensorsSet(SENSOR_GYRO); return true; }
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(); } }
void SensorDetectAndINI(void) // "enabledSensors" is "0" in config.c so all sensors disabled per default { int16_t deg, min; uint8_t sig = 0; bool ack = false; bool haveMpu6k = false; GyroScale16 = (16.0f / 16.4f) * RADX; // GYRO part. RAD per SECOND, take into account that gyrodata are div by X if (mpu6050Detect(&acc, &gyro)) // Autodetect gyro hardware. We have MPU3050 or MPU6050. { haveMpu6k = true; // this filled up acc.* struct with init values } else if (l3g4200dDetect(&gyro)) { havel3g4200d = true; GyroScale16 = (16.0f / 14.2857f) * RADX; // GYRO part. RAD per SECOND, take into account that gyrodata are div by X } else if (!mpu3050Detect(&gyro)) { failureMode(3); // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error. } sensorsSet(SENSOR_ACC); // ACC part. Will be cleared if not available retry: switch (cfg.acc_hdw) { case 0: // autodetect case 1: // ADXL345 if (adxl345Detect(&acc)) accHardware = ACC_ADXL345; if (cfg.acc_hdw == ACC_ADXL345) break; case 2: // MPU6050 if (haveMpu6k) { mpu6050Detect(&acc, &gyro); // yes, i'm rerunning it again. re-fill acc struct accHardware = ACC_MPU6050; if (cfg.acc_hdw == ACC_MPU6050) break; } case 3: // MMA8452 if (mma8452Detect(&acc)) { accHardware = ACC_MMA8452; if (cfg.acc_hdw == ACC_MMA8452) break; } } if (accHardware == ACC_DEFAULT) // Found anything? Check if user f****d up or ACC is really missing. { if (cfg.acc_hdw > ACC_DEFAULT) { cfg.acc_hdw = ACC_DEFAULT; // Nothing was found and we have a forced sensor type. User probably chose a sensor that isn't present. goto retry; } else sensorsClear(SENSOR_ACC); // We're really screwed } if (sensors(SENSOR_ACC)) acc.init(); if (haveMpu6k && accHardware == ACC_MPU6050) MpuSpecial = true; else MpuSpecial = false; if (feature(FEATURE_PASS)) return; // Stop here we need just ACC for Vibrationmonitoring if present if (feature(FEATURE_GPS) && !SerialRCRX) gpsInit(cfg.gps_baudrate);// SerialRX and GPS can not coexist. gyro.init(); // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. if (havel3g4200d) l3g4200dConfig(); else if (!haveMpu6k) mpu3050Config(); Gyro_Calibrate(); // Do Gyrocalibration here (is blocking), provides nice Warmuptime for the following rest! #ifdef MAG if (hmc5883lDetect()) { sensorsSet(SENSOR_MAG); hmc5883lInit(magCal); // Crashpilot: Calculate Gains / Scale deg = cfg.mag_dec / 100; // calculate magnetic declination min = cfg.mag_dec % 100; magneticDeclination = ((float)deg + ((float)min / 60.0f));// heading is in decimaldeg units NO 0.1 deg shit here } #endif #ifdef BARO // No delay necessary since Gyrocal blocked a lot already ack = i2cRead(0x77, 0x77, 1, &sig); // Check Baroadr.(MS & BMP) BMP will say hello here, MS not if ( ack) ack = bmp085Detect(&baro); // Are we really dealing with BMP? if (!ack) ack = ms5611Detect(&baro); // No, Check for MS Baro if (ack) sensorsSet(SENSOR_BARO); if(cfg.esc_nfly) ESCnoFlyThrottle = constrain_int(cfg.esc_nfly, cfg.esc_min, cfg.esc_max); // Set the ESC PWM signal threshold for not flyable RPM else ESCnoFlyThrottle = cfg.esc_min + (((cfg.esc_max - cfg.esc_min) * 5) / 100); // If not configured, take 5% above esc_min #endif #ifdef SONAR if (feature(FEATURE_SONAR)) Sonar_init(); // Initialize Sonars here depending on Rc configuration. SonarLandWanted = cfg.snr_land; // Variable may be overwritten by failsave #endif MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D }
bool compassDetect(magDev_t *dev) { magSensor_e magHardware = MAG_NONE; busDevice_t *busdev = &dev->busdev; #ifdef USE_MAG_DATA_READY_SIGNAL dev->magIntExtiTag = compassConfig()->interruptTag; #endif switch (compassConfig()->mag_bustype) { #ifdef USE_I2C case BUSTYPE_I2C: busdev->bustype = BUSTYPE_I2C; busdev->busdev_u.i2c.device = I2C_CFG_TO_DEV(compassConfig()->mag_i2c_device); busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; #endif break; #ifdef USE_SPI case BUSTYPE_SPI: busdev->bustype = BUSTYPE_SPI; spiBusSetInstance(busdev, spiInstanceByDevice(SPI_CFG_TO_DEV(compassConfig()->mag_spi_device))); busdev->busdev_u.spi.csnPin = IOGetByTag(compassConfig()->mag_spi_csn); #endif break; #if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)) case BUSTYPE_MPU_SLAVE: { if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) { busdev->bustype = BUSTYPE_MPU_SLAVE; busdev->busdev_u.mpuSlave.master = gyroSensorBus(); busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address; } else { return false; } } #endif break; default: return false; } dev->magAlign = ALIGN_DEFAULT; switch (compassConfig()->mag_hardware) { case MAG_DEFAULT: FALLTHROUGH; case MAG_HMC5883: #if defined(USE_MAG_HMC5883) || defined(USE_MAG_SPI_HMC5883) if (busdev->bustype == BUSTYPE_I2C) { busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; } if (hmc5883lDetect(dev)) { #ifdef MAG_HMC5883_ALIGN dev->magAlign = MAG_HMC5883_ALIGN; #endif magHardware = MAG_HMC5883; break; } #endif FALLTHROUGH; case MAG_AK8975: #ifdef USE_MAG_AK8975 if (busdev->bustype == BUSTYPE_I2C) { busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; } if (ak8975Detect(dev)) { #ifdef MAG_AK8975_ALIGN dev->magAlign = MAG_AK8975_ALIGN; #endif magHardware = MAG_AK8975; break; } #endif FALLTHROUGH; case MAG_AK8963: #if defined(USE_MAG_AK8963) || defined(USE_MAG_SPI_AK8963) if (busdev->bustype == BUSTYPE_I2C) { busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address; } if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) { dev->busdev.bustype = BUSTYPE_MPU_SLAVE; busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address; dev->busdev.busdev_u.mpuSlave.master = gyroSensorBus(); } if (ak8963Detect(dev)) { #ifdef MAG_AK8963_ALIGN dev->magAlign = MAG_AK8963_ALIGN; #endif magHardware = MAG_AK8963; break; } #endif FALLTHROUGH; case MAG_NONE: magHardware = MAG_NONE; break; } if (magHardware == MAG_NONE) { return false; } detectedSensors[SENSOR_INDEX_MAG] = magHardware; sensorsSet(SENSOR_MAG); return true; }
bool detectGyro(void) { bool sensorDetected; UNUSED(sensorDetected); // avoid unused-variable warning on some targets. gyroSensor_e gyroHardware = GYRO_DEFAULT; gyroAlign = ALIGN_DEFAULT; switch(gyroHardware) { case GYRO_DEFAULT: ; // fallthrough case GYRO_MPU6050: #ifdef USE_GYRO_MPU6050 if (mpu6050GyroDetect(&gyro)) { #ifdef GYRO_MPU6050_ALIGN gyroHardware = GYRO_MPU6050; gyroAlign = GYRO_MPU6050_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_L3G4200D: #ifdef USE_GYRO_L3G4200D if (l3g4200dDetect(&gyro)) { #ifdef GYRO_L3G4200D_ALIGN gyroHardware = GYRO_L3G4200D; gyroAlign = GYRO_L3G4200D_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_MPU3050: #ifdef USE_GYRO_MPU3050 if (mpu3050Detect(&gyro)) { #ifdef GYRO_MPU3050_ALIGN gyroHardware = GYRO_MPU3050; gyroAlign = GYRO_MPU3050_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_L3GD20: #ifdef USE_GYRO_L3GD20 if (l3gd20Detect(&gyro)) { #ifdef GYRO_L3GD20_ALIGN gyroHardware = GYRO_L3GD20; gyroAlign = GYRO_L3GD20_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_MPU6000: #ifdef USE_GYRO_SPI_MPU6000 if (mpu6000SpiGyroDetect(&gyro)) { #ifdef GYRO_MPU6000_ALIGN gyroHardware = GYRO_MPU6000; gyroAlign = GYRO_MPU6000_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_MPU6500: #ifdef USE_GYRO_MPU6500 #ifdef USE_GYRO_SPI_MPU6500 sensorDetected = mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro); #else sensorDetected = mpu6500GyroDetect(&gyro); #endif if (sensorDetected) { gyroHardware = GYRO_MPU6500; #ifdef GYRO_MPU6500_ALIGN gyroAlign = GYRO_MPU6500_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_FAKE: #ifdef USE_FAKE_GYRO if (fakeGyroDetect(&gyro)) { gyroHardware = GYRO_FAKE; break; } #endif ; // fallthrough case GYRO_NONE: gyroHardware = GYRO_NONE; } if (gyroHardware == GYRO_NONE) { return false; } detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; sensorsSet(SENSOR_GYRO); return true; }
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 } }
static void detectAcc(accelerationSensor_e accHardwareToUse) { bool sensorDetected; UNUSED(sensorDetected); // avoid unused-variable warning on some targets. accelerationSensor_e accHardware; #ifdef USE_ACC_ADXL345 drv_adxl345_config_t acc_params; #endif retry: accAlign = ALIGN_DEFAULT; switch (accHardwareToUse) { case ACC_DEFAULT: ; // fallthrough case ACC_ADXL345: // ADXL345 #ifdef USE_ACC_ADXL345 acc_params.useFifo = false; acc_params.dataRate = 800; // unused currently #ifdef NAZE sensorDetected = (hardwareRevision < NAZE32_REV5) && adxl345Detect(&acc_params, &acc); #else sensorDetected = adxl345Detect(&acc_params, &acc); #endif if (sensorDetected) { #ifdef ACC_ADXL345_ALIGN accAlign = ACC_ADXL345_ALIGN; #endif accHardware = ACC_ADXL345; break; } #endif ; // fallthrough case ACC_LSM303DLHC: #ifdef USE_ACC_LSM303DLHC if (lsm303dlhcAccDetect(&acc)) { #ifdef ACC_LSM303DLHC_ALIGN accAlign = ACC_LSM303DLHC_ALIGN; #endif accHardware = ACC_LSM303DLHC; break; } #endif ; // fallthrough case ACC_MPU6050: // MPU6050 #ifdef USE_ACC_MPU6050 if (mpu6050AccDetect(&acc)) { #ifdef ACC_MPU6050_ALIGN accAlign = ACC_MPU6050_ALIGN; #endif accHardware = ACC_MPU6050; break; } #endif ; // fallthrough case ACC_MMA8452: // MMA8452 #ifdef USE_ACC_MMA8452 #ifdef NAZE sensorDetected = (hardwareRevision < NAZE32_REV5) && mma8452Detect(&acc); #else sensorDetected = mma8452Detect(&acc); #endif if (sensorDetected) { #ifdef ACC_MMA8452_ALIGN accAlign = ACC_MMA8452_ALIGN; #endif accHardware = ACC_MMA8452; break; } #endif ; // fallthrough case ACC_BMA280: // BMA280 #ifdef USE_ACC_BMA280 if (bma280Detect(&acc)) { #ifdef ACC_BMA280_ALIGN accAlign = ACC_BMA280_ALIGN; #endif accHardware = ACC_BMA280; break; } #endif ; // fallthrough case ACC_MPU6000: #ifdef USE_ACC_SPI_MPU6000 if (mpu6000SpiAccDetect(&acc)) { #ifdef ACC_MPU6000_ALIGN accAlign = ACC_MPU6000_ALIGN; #endif accHardware = ACC_MPU6000; break; } #endif ; // fallthrough case ACC_MPU6500: #ifdef USE_ACC_MPU6500 #ifdef USE_ACC_SPI_MPU6500 sensorDetected = mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc); #else sensorDetected = mpu6500AccDetect(&acc); #endif if (sensorDetected) { #ifdef ACC_MPU6500_ALIGN accAlign = ACC_MPU6500_ALIGN; #endif accHardware = ACC_MPU6500; break; } #endif ; // fallthrough case ACC_FAKE: #ifdef USE_FAKE_ACC if (fakeAccDetect(&acc)) { accHardware = ACC_FAKE; break; } #endif ; // fallthrough case ACC_NONE: // disable ACC accHardware = ACC_NONE; break; } // Found anything? Check if error or ACC is really missing. if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) { // Nothing was found and we have a forced sensor that isn't present. accHardwareToUse = ACC_DEFAULT; goto retry; } if (accHardware == ACC_NONE) { return; } detectedSensors[SENSOR_INDEX_ACC] = accHardware; sensorsSet(SENSOR_ACC); }
static void gpsNewData(uint16_t c) { int axis; static uint32_t nav_loopTimer; int32_t dist; int32_t dir; int16_t speed; if (gpsNewFrame(c)) { // new data received and parsed, we're in business gpsData.lastLastMessage = gpsData.lastMessage; gpsData.lastMessage = millis(); sensorsSet(SENSOR_GPS); if (GPS_update == 1) GPS_update = 0; else GPS_update = 1; if (f.GPS_FIX && GPS_numSat >= 5) { if (!f.ARMED) f.GPS_FIX_HOME = 0; if (!f.GPS_FIX_HOME && f.ARMED) GPS_reset_home_position(); // Apply moving average filter to GPS data #if defined(GPS_FILTERING) GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH; for (axis = 0; axis < 2; axis++) { GPS_read[axis] = GPS_coord[axis]; // latest unfiltered data is in GPS_latitude and GPS_longitude GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t // How close we are to a degree line ? its the first three digits from the fractions of degree // later we use it to Check if we are close to a degree line, if yes, disable averaging, fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000; GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index]; GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000); GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index]; GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000); if (nav_mode == NAV_MODE_POSHOLD) { // we use gps averaging only in poshold mode... if (fraction3[axis] > 1 && fraction3[axis] < 999) GPS_coord[axis] = GPS_filtered[axis]; } } #endif // dTnav calculation // Time for calculating x,y speed and navigation pids dTnav = (float)(millis() - nav_loopTimer) / 1000.0f; nav_loopTimer = millis(); // prevent runup from bad GPS dTnav = min(dTnav, 1.0f); // calculate distance and bearings for gui and other stuff continously - From home to copter GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_home[LAT], &GPS_home[LON], &dist, &dir); GPS_distanceToHome = dist / 100; GPS_directionToHome = dir / 100; if (!f.GPS_FIX_HOME) { // If we don't have home set, do not display anything GPS_distanceToHome = 0; GPS_directionToHome = 0; } // calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating GPS_calc_velocity(); if (f.GPS_HOLD_MODE || f.GPS_HOME_MODE) { // ok we are navigating // do gps nav calculations here, these are common for nav and poshold GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing); GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]); switch (nav_mode) { case NAV_MODE_POSHOLD: // Desired output is in nav_lat and nav_lon where 1deg inclination is 100 GPS_calc_poshold(); break; case NAV_MODE_WP: speed = GPS_calc_desired_speed(cfg.nav_speed_max, NAV_SLOW_NAV); // slow navigation // use error as the desired rate towards the target // Desired output is in nav_lat and nav_lon where 1deg inclination is 100 GPS_calc_nav_rate(speed); // Tail control if (cfg.nav_controls_heading) { if (NAV_TAIL_FIRST) { magHold = wrap_18000(nav_bearing - 18000) / 100; } else { magHold = nav_bearing / 100; } } // Are we there yet ?(within x meters of the destination) if ((wp_distance <= cfg.gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode nav_mode = NAV_MODE_POSHOLD; if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; } } break; } } //end of gps calcs } } }
static void detectBaro(baroSensor_e baroHardwareToUse) { #ifndef BARO UNUSED(baroHardwareToUse); #else // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function baroSensor_e baroHardware = baroHardwareToUse; #ifdef USE_BARO_BMP085 const bmp085Config_t *bmp085Config = NULL; #if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO) static const bmp085Config_t defaultBMP085Config = { .gpioAPB2Peripherals = BARO_APB2_PERIPHERALS, .xclrGpioPin = BARO_XCLR_PIN, .xclrGpioPort = BARO_XCLR_GPIO, .eocGpioPin = BARO_EOC_PIN, .eocGpioPort = BARO_EOC_GPIO }; bmp085Config = &defaultBMP085Config; #endif #ifdef NAZE if (hardwareRevision == NAZE32) { bmp085Disable(bmp085Config); } #endif #endif switch (baroHardware) { case BARO_DEFAULT: ; // fallthough case BARO_MS5611: #ifdef USE_BARO_MS5611 if (ms5611Detect(&baro)) { baroHardware = BARO_MS5611; break; } #endif ; // fallthough case BARO_BMP085: #ifdef USE_BARO_BMP085 if (bmp085Detect(bmp085Config, &baro)) { baroHardware = BARO_BMP085; break; } #endif ; // fallthough case BARO_BMP280: #ifdef USE_BARO_BMP280 if (bmp280Detect(&baro)) { baroHardware = BARO_BMP280; break; } #endif case BARO_NONE: baroHardware = BARO_NONE; break; } if (baroHardware == BARO_NONE) { return; } detectedSensors[SENSOR_INDEX_BARO] = baroHardware; sensorsSet(SENSOR_BARO); #endif }
bool detectGyro(void) { gyroSensor_e gyroHardware = GYRO_DEFAULT; gyroAlign = ALIGN_DEFAULT; switch(gyroHardware) { case GYRO_DEFAULT: ; // fallthrough case GYRO_MPU6050: #ifdef USE_GYRO_MPU6050 if (mpu6050GyroDetect(&gyro)) { #ifdef GYRO_MPU6050_ALIGN gyroHardware = GYRO_MPU6050; gyroAlign = GYRO_MPU6050_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_L3G4200D: #ifdef USE_GYRO_L3G4200D if (l3g4200dDetect(&gyro)) { #ifdef GYRO_L3G4200D_ALIGN gyroHardware = GYRO_L3G4200D; gyroAlign = GYRO_L3G4200D_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_MPU3050: #ifdef USE_GYRO_MPU3050 if (mpu3050Detect(&gyro)) { #ifdef GYRO_MPU3050_ALIGN gyroHardware = GYRO_MPU3050; gyroAlign = GYRO_MPU3050_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_L3GD20: #ifdef USE_GYRO_L3GD20 if (l3gd20Detect(&gyro)) { #ifdef GYRO_L3GD20_ALIGN gyroHardware = GYRO_L3GD20; gyroAlign = GYRO_L3GD20_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_MPU6000: #ifdef USE_GYRO_SPI_MPU6000 if (mpu6000SpiGyroDetect(&gyro)) { #ifdef GYRO_MPU6000_ALIGN gyroHardware = GYRO_MPU6000; gyroAlign = GYRO_MPU6000_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_MPU6500: #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) #ifdef USE_GYRO_SPI_MPU6500 if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro)) #else if (mpu6500GyroDetect(&gyro)) #endif { gyroHardware = GYRO_MPU6500; #ifdef GYRO_MPU6500_ALIGN gyroAlign = GYRO_MPU6500_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_MPU9250: #ifdef USE_GYRO_SPI_MPU9250 if (mpu9250SpiGyroDetect(&gyro)) { gyroHardware = GYRO_MPU9250; #ifdef GYRO_MPU9250_ALIGN gyroAlign = GYRO_MPU9250_ALIGN; #endif break; } #endif ; // fallthrough case GYRO_FAKE: #ifdef USE_FAKE_GYRO if (fakeGyroDetect(&gyro)) { gyroHardware = GYRO_FAKE; break; } #endif ; // fallthrough case GYRO_NONE: gyroHardware = GYRO_NONE; } if (gyroHardware == GYRO_NONE) { return false; } detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; sensorsSet(SENSOR_GYRO); return true; }
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 } }