Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Enable possibility to simulate HW sensor failures in HITL #8858

Merged
merged 6 commits into from Jun 28, 2023
Merged

Enable possibility to simulate HW sensor failures in HITL #8858

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
b5af1b7
enable possibility to simulate GPS sensor timeout, mag failure and pi…
RomanLut Feb 26, 2023
ae1e554
enable possibility to simulate GPS sensor timeout, mag failure and pi…
RomanLut Mar 6, 2023
0962804
Merge branch 'master' of https://github.com/RomanLut/inav into submit…
RomanLut Mar 6, 2023
2c5e80e
replaced tabs with spaces
RomanLut Mar 6, 2023
1bc1888
fixed HITL HW sensors failure simulation
RomanLut Apr 28, 2023
0c39f8b
Merge branch 'master' of https://github.com/RomanLut/inav into submit…
RomanLut May 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
16 changes: 13 additions & 3 deletions src/main/fc/fc_msp.c
View file
Open in desktop
Expand Up @@ -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);
Expand Down Expand Up @@ -3526,6 +3527,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);
Expand Down Expand Up @@ -3580,7 +3582,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);
Expand All @@ -3597,8 +3599,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);
}
Expand Down
23 changes: 13 additions & 10 deletions src/main/fc/runtime_config.h
View file
Open in desktop
Expand Up @@ -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;

Expand Down
19 changes: 15 additions & 4 deletions src/main/io/gps.c
View file
Open in desktop
Expand Up @@ -286,10 +286,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

Expand Down
9 changes: 7 additions & 2 deletions src/main/sensors/diagnostics.c
View file
Open in desktop
Expand Up @@ -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;
Expand Down
19 changes: 13 additions & 6 deletions src/main/sensors/pitotmeter.c
View file
Open in desktop
Expand Up @@ -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();
Expand All @@ -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) {
Expand Down Expand Up @@ -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
}

Expand Down