Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 09:16:01 +03:00
Code Issues Wiki Activity

Merge pull request #8858 from RomanLut/submit-simulator-sensor-failures

Browse source 
Enable possibility to simulate HW sensor failures in HITL
This commit is contained in:
Paweł Spychalski 2023-06-28 11:03:55 -07:00 • committed by GitHub
commit cbb4b5f311
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 61 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

16
src/main/fc/fc_msp.c
View file

@ -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);
}

23
src/main/fc/runtime_config.h
View file

@ -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;

19
src/main/io/gps.c
View file

@ -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

9
src/main/sensors/diagnostics.c
View file

@ -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;

19
src/main/sensors/pitotmeter.c
View file

@ -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
}