Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

enable possibility to simulate GPS sensor timeout, mag failure and pitot failure with HITL

Browse source
This commit is contained in:
Roman Lut 2023-02-27 00:03:01 +01:00
parent 9c32094f5e
commit b5af1b765f
5 changed files with 39 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

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

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

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

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

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

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

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) && 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;

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

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