diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0f803f99eb..f92d088cb8 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2526,6 +2526,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) gpsSol.flags.validVelNE = false; gpsSol.flags.validVelD = false; gpsSol.flags.validEPE = false; + gpsSol.flags.validTime = false; gpsSol.numSat = sbufReadU8(src); gpsSol.llh.lat = sbufReadU32(src); gpsSol.llh.lon = sbufReadU32(src); @@ -3533,6 +3534,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu gpsSol.flags.validVelNE = true; gpsSol.flags.validVelD = true; gpsSol.flags.validEPE = true; + gpsSol.flags.validTime = false; gpsSol.llh.lat = sbufReadU32(src); gpsSol.llh.lon = sbufReadU32(src); @@ -3587,7 +3589,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (sensors(SENSOR_MAG)) { mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT - mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; + mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20; } else { sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); @@ -3604,8 +3606,16 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu #endif if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - simulatorData.airSpeed = sbufReadU16(src); - } + simulatorData.airSpeed = sbufReadU16(src); + } else { + if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { + sbufReadU16(src); + } + } + + if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { + simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8; + } } else { DISABLE_STATE(GPS_FIX); } diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index a4e613ed72..16d1b411df 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -175,20 +175,23 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void); typedef enum { HITL_RESET_FLAGS = (0 << 0), - HITL_ENABLE = (1 << 0), - HITL_SIMULATE_BATTERY = (1 << 1), - HITL_MUTE_BEEPER = (1 << 2), - HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV) - HITL_HAS_NEW_GPS_DATA = (1 << 4), - HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2 - HITL_AIRSPEED = (1 << 6) + HITL_ENABLE = (1 << 0), + HITL_SIMULATE_BATTERY = (1 << 1), + HITL_MUTE_BEEPER = (1 << 2), + HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV) + HITL_HAS_NEW_GPS_DATA = (1 << 4), + HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2 + HITL_AIRSPEED = (1 << 6), + HITL_EXTENDED_FLAGS = (1 << 7), // Extend MSP_SIMULATOR format 2 + HITL_GPS_TIMEOUT = (1 << 8), + HITL_PITOT_FAILURE = (1 << 9) } simulatorFlags_t; typedef struct { - simulatorFlags_t flags; - uint8_t debugIndex; + simulatorFlags_t flags; + uint8_t debugIndex; uint8_t vbat; // 126 -> 12.6V - uint16_t airSpeed; // cm/s + uint16_t airSpeed; // cm/s int16_t input[4]; } simulatorData_t; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 92e0ce6178..f8312069af 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -343,10 +343,21 @@ bool gpsUpdate(void) #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { - gpsUpdateTime(); - gpsSetState(GPS_RUNNING); - sensorsSet(SENSOR_GPS); - return gpsSol.flags.hasNewData; + if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT)) + { + gpsSetState(GPS_LOST_COMMUNICATION); + sensorsClear(SENSOR_GPS); + gpsStats.timeouts = 5; + return false; + } + else + { + gpsSetState(GPS_RUNNING); + sensorsSet(SENSOR_GPS); + bool res = gpsSol.flags.hasNewData; + gpsSol.flags.hasNewData = false; + return res; + } } #endif diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index a455a49466..b3d7a1ad4e 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -62,12 +62,17 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void) hardwareSensorStatus_e getHwCompassStatus(void) { +#if defined(USE_MAG) #ifdef USE_SIMULATOR if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) { - return HW_SENSOR_OK; + if (compassIsHealthy()) { + return HW_SENSOR_OK; + } + else { + return HW_SENSOR_UNHEALTHY; + } } #endif -#if defined(USE_MAG) if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { if (compassIsHealthy()) { return HW_SENSOR_OK; diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index d41b5e4cd3..8f10206839 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -194,6 +194,13 @@ STATIC_PROTOTHREAD(pitotThread) pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f); while(1) { +#ifdef USE_SIMULATOR + while (SIMULATOR_HAS_OPTION(HITL_AIRSPEED) && SIMULATOR_HAS_OPTION(HITL_PITOT_FAILURE)) + { + ptDelayUs(10000); + } +#endif + // Start measurement if (pitot.dev.start(&pitot.dev)) { pitot.lastSeenHealthyMs = millis(); @@ -208,9 +215,9 @@ STATIC_PROTOTHREAD(pitotThread) pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL); #ifdef USE_SIMULATOR - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; - } + if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { + pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; + } #endif #if defined(USE_PITOT_FAKE) if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { @@ -241,14 +248,14 @@ STATIC_PROTOTHREAD(pitotThread) pitot.airSpeed = 0.0f; } #ifdef USE_SIMULATOR - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { + if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { pitot.airSpeed = simulatorData.airSpeed; - } + } #endif #if defined(USE_PITOT_FAKE) if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { pitot.airSpeed = fakePitotGetAirspeed(); - } + } #endif }