void gpsThread(void) { switch (gpsData.state) { case GPS_UNKNOWN: break; case GPS_INITIALIZING: case GPS_INITDONE: gpsInitHardware(); break; case GPS_LOSTCOMMS: gpsData.errors++; // try another rate gpsData.baudrateIndex++; gpsData.baudrateIndex %= GPS_INIT_ENTRIES; gpsData.lastMessage = millis(); // TODO - move some / all of these into gpsData GPS_numSat = 0; f.GPS_FIX = 0; gpsSetState(GPS_INITIALIZING); break; case GPS_RECEIVINGDATA: // check for no data/gps timeout/cable disconnection etc if (millis() - gpsData.lastMessage > GPS_TIMEOUT) { // remove GPS from capability sensorsClear(SENSOR_GPS); gpsSetState(GPS_LOSTCOMMS); } break; } }
void DoChkGPSDeadin50HzLoop(void) // Check this in a 50Hz loop in mainprogram { static uint8_t cnt = 0; static uint32_t LastGPSTS = 0; if (!LastGPSTS) LastGPSTS = TimestampNewGPSdata; // if TimestampNewGPSdata is 0 we will be here again soon else { if (TimestampNewGPSdata == LastGPSTS) cnt++; else { cnt = 0; LastGPSTS = TimestampNewGPSdata; } if (cnt == 75) sensorsClear(SENSOR_GPS); // No Data for 1.5 secs? } }
bool sensorsAutodetect(void) { memset(&acc, 0, sizeof(acc)); memset(&gyro, 0, sizeof(gyro)); #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) const extiConfig_t *extiConfig = selectMPUIntExtiConfig(); mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig); UNUSED(mpuDetectionResult); #endif if (!detectGyro()) { return false; } gyro.sampleFrequencyHz = gyroConfig()->gyro_sample_hz; // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. gyro.init(&gyro, gyroConfig()->gyro_lpf); gyroInit(); // the combination of LPF and GYRO_SAMPLE_HZ may be invalid for the gyro, update the configuration to use the sample frequency that was determined for the desired LPF. gyroConfig()->gyro_sample_hz = gyro.sampleFrequencyHz; if (detectAcc(sensorSelectionConfig()->acc_hardware)) { acc.acc_1G = 256; // set default acc.init(&acc); } #ifdef BARO detectBaro(sensorSelectionConfig()->baro_hardware); #endif #ifdef MAG if (detectMag(sensorSelectionConfig()->mag_hardware)) { if (!compassInit()) { sensorsClear(SENSOR_MAG); } } #endif reconfigureAlignment(sensorAlignmentConfig()); return true; }
static void detectBaro() { #ifdef BARO #ifdef USE_BARO_MS5611 // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function if (ms5611Detect(&baro)) { return; } #endif #ifdef USE_BARO_BMP085 // ms5611 disables BMP085, and tries to initialize + check PROM crc. if this works, we have a baro if (bmp085Detect(&baro)) { return; } #endif sensorsClear(SENSOR_BARO); #endif }
void gpsThread(void) { // read out available GPS bytes if (gpsPort) { while (serialRxBytesWaiting(gpsPort)) gpsNewData(serialRead(gpsPort)); } switch (gpsData.state) { case GPS_UNKNOWN: break; case GPS_INITIALIZING: case GPS_CHANGE_BAUD: case GPS_CONFIGURE: gpsInitHardware(); break; case GPS_LOST_COMMUNICATION: gpsData.timeouts++; if (gpsConfig()->autoBaud) { // try another rate gpsData.baudrateIndex++; gpsData.baudrateIndex %= GPS_INIT_ENTRIES; } gpsData.lastMessage = millis(); // TODO - move some / all of these into gpsData GPS_numSat = 0; DISABLE_STATE(GPS_FIX); gpsSetState(GPS_INITIALIZING); break; case GPS_RECEIVING_DATA: // check for no data/gps timeout/cable disconnection etc if (millis() - gpsData.lastMessage > GPS_TIMEOUT) { // remove GPS from capability sensorsClear(SENSOR_GPS); gpsSetState(GPS_LOST_COMMUNICATION); } break; } }
bool sensorsAutodetect(void) { int16_t deg, min; if (!mpu6000DetectSpi(&acc, &gyro, mcfg.gyro_lpf)) { sensorsClear(SENSOR_GYRO); sensorsClear(SENSOR_ACC); } #ifdef BARO if (!ms5611DetectSpi(&baro)) sensorsClear(SENSOR_BARO); #else sensorsClear(SENSOR_BARO); #endif if (feature(FEATURE_I2C)){ #ifdef MAG if (!hmc5883lDetect(&mag)) sensorsClear(SENSOR_MAG); #else sensorsClear(SENSOR_MAG); #endif } else sensorsClear(SENSOR_MAG); // Now time to init things, acc first if (sensors(SENSOR_ACC)) acc.init(mcfg.acc_align); if (sensors(SENSOR_GYRO)) gyro.init(mcfg.gyro_align); // calculate magnetic declination deg = cfg.mag_declination / 100; min = cfg.mag_declination % 100; if (sensors(SENSOR_MAG)) magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units else magneticDeclination = 0.0f; return true; }
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig) { int16_t deg, min; memset(&acc, sizeof(acc), 0); memset(&gyro, sizeof(gyro), 0); if (!detectGyro(gyroLpf)) { return false; } detectAcc(accHardwareToUse); detectBaro(); reconfigureAlignment(sensorAlignmentConfig); // 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(); #ifdef MAG if (hmc5883lDetect()) { magAlign = CW180_DEG; // default NAZE alignment } else { sensorsClear(SENSOR_MAG); } #endif // 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 sensorsAutodetect(void) { memset(&acc, 0, sizeof(acc)); memset(&gyro, 0, sizeof(gyro)); #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) const extiConfig_t *extiConfig = selectMPUIntExtiConfig(); mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig); UNUSED(mpuDetectionResult); #endif if (!detectGyro()) { return false; } // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. gyro.init(gyroConfig()->gyro_lpf); if (detectAcc(sensorSelectionConfig()->acc_hardware)) { acc.acc_1G = 256; // set default acc.init(&acc); } #ifdef BARO detectBaro(sensorSelectionConfig()->baro_hardware); #endif #ifdef MAG if (detectMag(sensorSelectionConfig()->mag_hardware)) { if (!compassInit()) { sensorsClear(SENSOR_MAG); } } #endif reconfigureAlignment(sensorAlignmentConfig()); return true; }
/* GPS signal lost timer callback function */ void gpsLostTimerCallback(xTimerHandle pxTimer) { sensorsClear(SENSOR_GPS); flagClear(FLAG_GPS_FIX); }
static void detectAcc(uint8_t accHardwareToUse) { #ifdef USE_ACC_ADXL345 drv_adxl345_config_t acc_params; #endif retry: accAlign = ALIGN_DEFAULT; switch (accHardwareToUse) { #ifdef USE_FAKE_ACC default: if (fakeAccDetect(&acc)) { accHardware = ACC_FAKE; if (accHardwareToUse == ACC_FAKE) break; } #endif case ACC_NONE: // disable ACC sensorsClear(SENSOR_ACC); break; case ACC_DEFAULT: // autodetect #ifdef USE_ACC_ADXL345 case ACC_ADXL345: // ADXL345 acc_params.useFifo = false; acc_params.dataRate = 800; // unused currently if (adxl345Detect(&acc_params, &acc)) { accHardware = ACC_ADXL345; #ifdef NAZE accAlign = CW270_DEG; #endif } if (accHardwareToUse == ACC_ADXL345) break; ; // fallthrough #endif #ifdef USE_ACC_MPU6050 case ACC_MPU6050: // MPU6050 if (mpu6050AccDetect(&acc)) { accHardware = ACC_MPU6050; #ifdef NAZE accAlign = CW0_DEG; #endif if (accHardwareToUse == ACC_MPU6050) break; } ; // fallthrough #endif #ifdef USE_ACC_MMA8452 case ACC_MMA8452: // MMA8452 if (mma8452Detect(&acc)) { accHardware = ACC_MMA8452; #ifdef NAZE accAlign = CW90_DEG; #endif if (accHardwareToUse == ACC_MMA8452) break; } ; // fallthrough #endif #ifdef USE_ACC_BMA280 case ACC_BMA280: // BMA280 if (bma280Detect(&acc)) { accHardware = ACC_BMA280; #ifdef NAZE accAlign = CW0_DEG; #endif if (accHardwareToUse == ACC_BMA280) break; } ; // fallthrough #endif #ifdef USE_ACC_LSM303DLHC case ACC_LSM303DLHC: if (lsm303dlhcAccDetect(&acc)) { accHardware = ACC_LSM303DLHC; if (accHardwareToUse == ACC_LSM303DLHC) break; } ; // fallthrough #endif #ifdef USE_GYRO_SPI_MPU6000 case ACC_SPI_MPU6000: if (mpu6000SpiAccDetect(&acc)) { accHardware = ACC_SPI_MPU6000; #ifdef CC3D accAlign = CW270_DEG; #endif if (accHardwareToUse == ACC_SPI_MPU6000) break; } ; // fallthrough #endif ; // prevent compiler error } // Found anything? Check if user f****d up or ACC is really missing. if (accHardware == ACC_DEFAULT) { if (accHardwareToUse > ACC_DEFAULT) { // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present. accHardwareToUse = ACC_DEFAULT; goto retry; } else { // No ACC was detected sensorsClear(SENSOR_ACC); } } }
bool sensorsAutodetect(void) { int16_t deg, min; drv_adxl345_config_t acc_params; bool haveMpu6k = false; // Autodetect gyro hardware. We have MPU3050 or MPU6050. if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale)) { // this filled up acc.* struct with init values haveMpu6k = true; } else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) { // well, we found our gyro ; } else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf)) { // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error. return false; } // Accelerometer. F**k it. Let user break shit. retry: switch (mcfg.acc_hardware) { case ACC_NONE: // disable ACC sensorsClear(SENSOR_ACC); break; case ACC_DEFAULT: // autodetect case ACC_ADXL345: // ADXL345 acc_params.useFifo = false; acc_params.dataRate = 800; // unused currently if (adxl345Detect(&acc_params, &acc)) accHardware = ACC_ADXL345; if (mcfg.acc_hardware == ACC_ADXL345) break; ; // fallthrough case ACC_MPU6050: // MPU6050 if (haveMpu6k) { mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct accHardware = ACC_MPU6050; if (mcfg.acc_hardware == ACC_MPU6050) break; } ; // fallthrough #ifndef OLIMEXINO case ACC_MMA8452: // MMA8452 if (mma8452Detect(&acc)) { accHardware = ACC_MMA8452; if (mcfg.acc_hardware == ACC_MMA8452) break; } ; // fallthrough case ACC_BMA280: // BMA280 if (bma280Detect(&acc)) { accHardware = ACC_BMA280; if (mcfg.acc_hardware == ACC_BMA280) break; } #endif } // Found anything? Check if user f****d up or ACC is really missing. if (accHardware == ACC_DEFAULT) { if (mcfg.acc_hardware > ACC_DEFAULT) { // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present. mcfg.acc_hardware = ACC_DEFAULT; goto retry; } else { // We're really screwed sensorsClear(SENSOR_ACC); } } #ifdef BARO // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function if (!bmp085Detect(&baro)) { // ms5611 disables BMP085, and tries to initialize + check PROM crc. // moved 5611 init here because there have been some reports that calibration data in BMP180 // has been "passing" ms5611 PROM crc check if (!ms5611Detect(&baro)) { // if both failed, we don't have anything sensorsClear(SENSOR_BARO); } } #endif // Now time to init things, acc first if (sensors(SENSOR_ACC)) acc.init(mcfg.acc_align); // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. gyro.init(mcfg.gyro_align); #ifdef MAG if (!hmc5883lDetect(mcfg.mag_align)) sensorsClear(SENSOR_MAG); #endif // calculate magnetic declination deg = cfg.mag_declination / 100; min = cfg.mag_declination % 100; if (sensors(SENSOR_MAG)) magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units else magneticDeclination = 0.0f; return true; }
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 sensorsAutodetect(void) { int16_t deg, min; mpu_params_t mpu_config; bool haveMpu = false; #ifndef CJMCU drv_adxl345_config_t acc_params; #endif // mpu driver parameters mpu_config.lpf = mcfg.gyro_lpf; // Autodetect Invensense acc/gyro hardware haveMpu = mpuDetect(&acc, &gyro, &mpu_config); // MPU6500 on I2C bus if (hse_value == 12000000 && mpu_config.deviceType == MPU_65xx_I2C) hw_revision = NAZE32_REV6; #ifndef CJMCU if (!haveMpu) { // Try some other gyros or bail out if fail if (!l3g4200dDetect(&gyro, mcfg.gyro_lpf)) return false; } #endif // Accelerometer. F**k it. Let user break shit. retry: switch (mcfg.acc_hardware) { case ACC_NONE: // disable ACC sensorsClear(SENSOR_ACC); break; case ACC_DEFAULT: // autodetect #ifndef CJMCU case ACC_ADXL345: // ADXL345 acc_params.useFifo = false; acc_params.dataRate = 800; // unused currently if (adxl345Detect(&acc_params, &acc)) accHardware = ACC_ADXL345; if (mcfg.acc_hardware == ACC_ADXL345) break; ; // fallthrough #endif case ACC_MPU6050: // MPU6050 if (haveMpu && mpu_config.deviceType == MPU_60x0) { accHardware = ACC_MPU6050; if (mcfg.acc_hardware == ACC_MPU6050) break; } ; // fallthrough #ifdef NAZE case ACC_MPU6500: // MPU6500 if (haveMpu && (mpu_config.deviceType >= MPU_65xx_I2C)) { accHardware = ACC_MPU6500; if (mcfg.acc_hardware == ACC_MPU6500) break; } ; // fallthrough case ACC_MMA8452: // MMA8452 if (mma8452Detect(&acc)) { accHardware = ACC_MMA8452; if (mcfg.acc_hardware == ACC_MMA8452) break; } ; // fallthrough case ACC_BMA280: // BMA280 if (bma280Detect(&acc)) { accHardware = ACC_BMA280; if (mcfg.acc_hardware == ACC_BMA280) break; } #endif } // Found anything? Check if user f****d up or ACC is really missing. if (accHardware == ACC_DEFAULT) { if (mcfg.acc_hardware > ACC_DEFAULT && mcfg.acc_hardware < ACC_NONE) { // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present. mcfg.acc_hardware = ACC_DEFAULT; goto retry; } else { // We're really screwed sensorsClear(SENSOR_ACC); } } #ifdef BARO // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function if (!bmp280Detect(&baro)) { if (!bmp085Detect(&baro)) { // ms5611 disables BMP085, and tries to initialize + check PROM crc. // moved 5611 init here because there have been some reports that calibration data in BMP180 // has been "passing" ms5611 PROM crc check if (!ms5611Detect(&baro)) { // if both failed, we don't have anything sensorsClear(SENSOR_BARO); } } } #endif // Now time to init things, acc first if (sensors(SENSOR_ACC)) acc.init(mcfg.acc_align); // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. gyro.init(mcfg.gyro_align); #ifdef MAG retryMag: switch (mcfg.mag_hardware) { case MAG_NONE: // disable MAG sensorsClear(SENSOR_MAG); break; case MAG_DEFAULT: // autodetect case MAG_HMC5883L: if (hmc5883lDetect(&mag)) { magHardware = MAG_HMC5883L; if (mcfg.mag_hardware == MAG_HMC5883L) break; } ; // fallthrough #ifdef NAZE case MAG_AK8975: if (ak8975detect(&mag)) { magHardware = MAG_AK8975; if (mcfg.mag_hardware == MAG_AK8975) break; } #endif } // Found anything? Check if user f****d up or mag is really missing. if (magHardware == MAG_DEFAULT) { if (mcfg.mag_hardware > MAG_DEFAULT && mcfg.mag_hardware < MAG_NONE) { // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present. mcfg.mag_hardware = MAG_DEFAULT; goto retryMag; } else { // No mag present sensorsClear(SENSOR_MAG); } } #endif // calculate magnetic declination deg = cfg.mag_declination / 100; min = cfg.mag_declination % 100; if (sensors(SENSOR_MAG)) magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units else magneticDeclination = 0.0f; return true; }
bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse) { pitotSensor_e pitotHardware = PITOT_NONE; requestedSensors[SENSOR_INDEX_PITOT] = pitotHardwareToUse; switch (pitotHardwareToUse) { case PITOT_AUTODETECT: case PITOT_MS4525: #ifdef USE_PITOT_MS4525 if (ms4525Detect(dev)) { pitotHardware = PITOT_MS4525; break; } #endif /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ if (pitotHardwareToUse != PITOT_AUTODETECT) { break; } case PITOT_ADC: #if defined(USE_PITOT_ADC) && defined(AIRSPEED_ADC_PIN) if (adcPitotDetect(dev)) { pitotHardware = PITOT_ADC; break; } #endif /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ if (pitotHardwareToUse != PITOT_AUTODETECT) { break; } case PITOT_VIRTUAL: #if defined(USE_PITOT_VIRTUAL) /* if (adcPitotDetect(&pitot)) { pitotHardware = PITOT_ADC; break; } */ #endif /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ if (pitotHardwareToUse != PITOT_AUTODETECT) { break; } case PITOT_FAKE: #ifdef USE_PITOT_FAKE if (fakePitotDetect(dev)) { pitotHardware = PITOT_FAKE; break; } #endif /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ if (pitotHardwareToUse != PITOT_AUTODETECT) { break; } case PITOT_NONE: pitotHardware = PITOT_NONE; break; } addBootlogEvent6(BOOT_EVENT_PITOT_DETECTION, BOOT_EVENT_FLAGS_NONE, pitotHardware, 0, 0, 0); if (pitotHardware == PITOT_NONE) { sensorsClear(SENSOR_PITOT); return false; } detectedSensors[SENSOR_INDEX_PITOT] = pitotHardware; sensorsSet(SENSOR_PITOT); return true; }
void gpsThread(void) { /* Extra delay for at least 2 seconds after booting to give GPS time to initialise */ if (!isMPUSoftReset() && (millis() < GPS_BOOT_DELAY)) { sensorsClear(SENSOR_GPS); DISABLE_STATE(GPS_FIX); return; } #ifdef USE_FAKE_GPS gpsFakeGPSUpdate(); #else // Serial-based GPS if ((gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_SERIAL) && (gpsState.gpsPort != NULL)) { switch (gpsState.state) { default: case GPS_INITIALIZING: if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) { // Reset internals DISABLE_STATE(GPS_FIX); gpsSol.fixType = GPS_NO_FIX; gpsState.hwVersion = 0; gpsState.autoConfigStep = 0; gpsState.autoConfigPosition = 0; gpsState.autoBaudrateIndex = 0; // Reset solution gpsResetSolution(); // Call protocol handler - switch to next state is done there gpsHandleProtocol(); } break; case GPS_CHANGE_BAUD: // Call protocol handler - switch to next state is done there gpsHandleProtocol(); break; case GPS_CHECK_VERSION: case GPS_CONFIGURE: case GPS_RECEIVING_DATA: gpsHandleProtocol(); if ((millis() - gpsState.lastMessageMs) > GPS_TIMEOUT) { // Check for GPS timeout sensorsClear(SENSOR_GPS); DISABLE_STATE(GPS_FIX); gpsSol.fixType = GPS_NO_FIX; gpsSetState(GPS_LOST_COMMUNICATION); } break; case GPS_LOST_COMMUNICATION: gpsStats.timeouts++; // Handle autobaud - switch to next port baud rate if (gpsState.gpsConfig->autoBaud != GPS_AUTOBAUD_OFF) { gpsState.baudrateIndex++; gpsState.baudrateIndex %= GPS_BAUDRATE_COUNT; } gpsSetState(GPS_INITIALIZING); break; } } // Driver-based GPS (I2C) else if (gpsProviders[gpsState.gpsConfig->provider].type == GPS_TYPE_BUS) { switch (gpsState.state) { default: case GPS_INITIALIZING: // Detect GPS unit if ((millis() - gpsState.lastStateSwitchMs) >= GPS_BUS_INIT_DELAY) { gpsResetSolution(); if (gpsProviders[gpsState.gpsConfig->provider].detect && gpsProviders[gpsState.gpsConfig->provider].detect()) { gpsState.hwVersion = 0; gpsState.autoConfigStep = 0; gpsState.autoConfigPosition = 0; gpsState.lastMessageMs = millis(); sensorsSet(SENSOR_GPS); gpsSetState(GPS_CHANGE_BAUD); } else { sensorsClear(SENSOR_GPS); } } break; case GPS_CHANGE_BAUD: case GPS_CHECK_VERSION: case GPS_CONFIGURE: case GPS_RECEIVING_DATA: gpsHandleProtocol(); if (millis() - gpsState.lastMessageMs > GPS_TIMEOUT) { // remove GPS from capability gpsSetState(GPS_LOST_COMMUNICATION); } break; case GPS_LOST_COMMUNICATION: // No valid data from GPS unit, cause re-init and re-detection gpsStats.timeouts++; DISABLE_STATE(GPS_FIX); gpsSol.fixType = GPS_NO_FIX; gpsSetState(GPS_INITIALIZING); break; } } else { // GPS_TYPE_NA } #endif }