From b5af1b765fb10a968d290b6ee17889f4fc426e5d Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 27 Feb 2023 00:03:01 +0100 Subject: [PATCH 1/4] enable possibility to simulate GPS sensor timeout, mag failure and pitot failure with HITL --- src/main/fc/fc_msp.c | 6 +++++- src/main/fc/runtime_config.h | 5 ++++- src/main/io/gps.c | 19 +++++++++++++++---- src/main/sensors/diagnostics.c | 9 +++++++-- src/main/sensors/pitotmeter.c | 9 ++++++++- 5 files changed, 39 insertions(+), 9 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ee5c8f0397..a5a634b4fa 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3545,7 +3545,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); @@ -3560,6 +3560,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { simulatorData.airSpeed = 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 9333fd62c1..4ccca2a5ef 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -180,7 +180,10 @@ typedef enum { 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_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 { diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 042286c238..2778e0997a 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -349,10 +349,21 @@ bool gpsUpdate(void) #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE)) { - 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 #ifdef USE_FAKE_GPS diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index 0702f78991..d0ae8324fb 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) && 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 eb2cad13f0..7c8fb7ad29 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -193,6 +193,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(); @@ -236,7 +243,7 @@ STATIC_PROTOTHREAD(pitotThread) } #ifdef USE_SIMULATOR if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - pitot.airSpeed = simulatorData.airSpeed; + pitot.airSpeed = simulatorData.airSpeed; } #endif } From ae1e55409b6a4e20af68319a099b75032b242ac7 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 19:43:29 +0100 Subject: [PATCH 2/4] enable possibility to simulate GPS sensor timeout, mag failure and pitot failure with HITL --- src/main/fc/fc_msp.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index a5a634b4fa..626af7df95 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2495,6 +2495,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); @@ -3491,6 +3492,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); From 2c5e80ec9713a7b2096fcd602c4cee3ce2e33036 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 6 Mar 2023 19:53:36 +0100 Subject: [PATCH 3/4] replaced tabs with spaces --- src/main/fc/fc_msp.c | 2 +- src/main/fc/runtime_config.h | 18 +++++++++--------- src/main/sensors/diagnostics.c | 2 +- src/main/sensors/pitotmeter.c | 2 +- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 626af7df95..b2ebac8343 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2495,7 +2495,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.flags.validTime = false; gpsSol.numSat = sbufReadU8(src); gpsSol.llh.lat = sbufReadU32(src); gpsSol.llh.lon = sbufReadU32(src); diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 4ccca2a5ef..33a3f4899a 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -174,12 +174,12 @@ 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_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), @@ -187,10 +187,10 @@ typedef enum { } 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/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index d0ae8324fb..351d5c6b5f 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -65,7 +65,7 @@ hardwareSensorStatus_e getHwCompassStatus(void) #if defined(USE_MAG) #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE) && sensors(SENSOR_MAG)) { - if (compassIsHealthy()) { + if (compassIsHealthy()) { return HW_SENSOR_OK; } else { diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 7c8fb7ad29..ad3b1f4400 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -243,7 +243,7 @@ STATIC_PROTOTHREAD(pitotThread) } #ifdef USE_SIMULATOR if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - pitot.airSpeed = simulatorData.airSpeed; + pitot.airSpeed = simulatorData.airSpeed; } #endif } From 1bc18880060d5208585c1044540e6a65a05f3e8a Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 28 Apr 2023 22:33:18 +0200 Subject: [PATCH 4/4] fixed HITL HW sensors failure simulation --- src/main/fc/fc_msp.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b2ebac8343..c3f238c54c 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3560,8 +3560,12 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu } 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;