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
}