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

Merge branch 'submit-simulator-xplane' of https://github.com/RomanLut/inav into submit-gps-fix-estimation

Browse source
This commit is contained in:
Roman Lut 2022-08-21 21:25:51 +03:00
commit 3ba8b4e066
24 changed files with 504 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

11
docs/development/Software In The Loop (HITL) plugin for X-Plane 11.md Normal file
View file

@ -0,0 +1,11 @@
# Software In The Loop (HITL) plugin for X-Plane 11
**Hardware-in-the-loop (HITL) simulation**, is a technique that is used in the development and testing of complex real-time embedded systems.
**X-Plane** is a flight simulation engine series developed and published by Laminar Research https://www.x-plane.com/
**INAV-X-Plane-HITL** is plugin for **X-Plane** for testing and developing flight controllers with **INAV flight controller firmware**
https://github.com/iNavFlight/inav.
HITL technique can be used to test features during development. Please check page above for installation instructions.

13
src/main/drivers/display.c
View file

@ -31,6 +31,7 @@
#include "drivers/time.h"
#include "fc/settings.h"
#include "fc/runtime_config.h"
#define SW_BLINK_CYCLE_MS 200 // 200ms on / 200ms off
@ -176,6 +177,18 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c)
if (instance->maxChar == 0) {
displayUpdateMaxChar(instance);
}
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) {
//some FCs do not power max7456 from USB power
//driver can not read font metadata
//chip assumed to not support second bank of font
//artifical horizon, variometer and home direction are not drawn ( display.c: displayUpdateMaxChar())
//return dummy metadata to let all OSD elements to work in simulator mode
instance->maxChar = 512;
}
#endif
if (c > instance->maxChar) {
return -1;
}

9
src/main/drivers/sound_beeper.c
View file

@ -27,6 +27,7 @@
#include "drivers/pwm_mapping.h"
#include "drivers/pwm_output.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "sound_beeper.h"
@ -45,6 +46,14 @@ void systemBeep(bool onoff)
UNUSED(onoff);
#else
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) {
if (simulatorData.flags & SIMU_MUTE_BEEPER) {
onoff = false;
}
}
#endif
if (beeperConfig()->pwmMode) {
pwmWriteBeeper(onoff);
beeperState = onoff;

13
src/main/fc/fc_core.c
View file

@ -942,6 +942,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
}
//Servos should be filtered or written only when mixer is using servos or special feaures are enabled
#ifdef USE_SMULATOR
if (!ARMING_FLAG(SIMULATOR_MODE)) {
if (isServoOutputEnabled()) {
writeServos();
}
@ -949,6 +952,16 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
if (motorControlEnable) {
writeMotors();
}
}
#else
if (isServoOutputEnabled()) {
writeServos();
}
if (motorControlEnable) {
writeMotors();
}
#endif
// Check if landed, FW and MR
if (STATE(ALTITUDE_CONTROL)) {

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

@ -21,6 +21,7 @@
#include <string.h>
#include <math.h>
#include "common/log.h" //for MSP_SIMULATOR
#include "platform.h"
#include "blackbox/blackbox.h"
@ -92,12 +93,15 @@
#include "io/serial_4way.h"
#include "io/vtx.h"
#include "io/vtx_string.h"
#include "io/gps_private.h" //for MSP_SIMULATOR
#include "msp/msp.h"
#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
#include "navigation/navigation.h"
#include "navigation/navigation_private.h" //for MSP_SIMULATOR
#include "navigation/navigation_pos_estimator_private.h" //for MSP_SIMULATOR
#include "rx/rx.h"
#include "rx/msp.h"
@ -3182,8 +3186,147 @@ static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src)
return true;
}
#ifdef USE_SIMULATOR
bool isOSDTypeSupportedBySimulator(void)
{
displayPort_t *osdDisplayPort = osdGetDisplayPort();
return (osdDisplayPort && osdDisplayPort->cols == 30 && (osdDisplayPort->rows == 13 || osdDisplayPort->rows == 16));
}
void mspWriteSimulatorOSD(sbuf_t *dst)
{
//RLE encoding
//scan displayBuffer iteratively
//no more than 80+3+2 bytes output in single run
//0 and 255 are special symbols
//255 - font bank switch
//0 - font bank switch, blink switch and character repeat
static uint8_t osdPos_y = 0;
static uint8_t osdPos_x = 0;
if (isOSDTypeSupportedBySimulator())
{
displayPort_t *osdDisplayPort = osdGetDisplayPort();
sbufWriteU8(dst, osdPos_y | (osdDisplayPort->rows == 16 ? 128: 0));
sbufWriteU8(dst, osdPos_x);
int bytesCount = 0;
uint16_t c = 0;
textAttributes_t attr = 0;
bool highBank = false;
bool blink = false;
int count = 0;
int processedRows = 16;
while (bytesCount < 80) //whole response should be less 155 bytes at worst.
{
bool blink1;
uint16_t lastChar;
count = 0;
while ( true )
{
displayReadCharWithAttr(osdDisplayPort, osdPos_x, osdPos_y, &c, &attr);
if (c == 0 || c == 255) c = 32;
//REVIEW: displayReadCharWithAttr() should return mode with _TEXT_ATTRIBUTES_BLINK_BIT !
//for max7456 it returns mode with MAX7456_MODE_BLINK instead (wrong)
//because max7456ReadChar() does not decode from MAX7456_MODE_BLINK to _TEXT_ATTRIBUTES_BLINK_BIT
//it should!
//bool blink2 = TEXT_ATTRIBUTES_HAVE_BLINK(attr);
bool blink2 = attr & (1<<4); //MAX7456_MODE_BLINK
if (count == 0)
{
lastChar = c;
blink1 = blink2;
}
else if (lastChar != c || blink2 != blink1 || count == 63)
{
break;
}
count++;
osdPos_x++;
if (osdPos_x == 30)
{
osdPos_x = 0;
osdPos_y++;
processedRows--;
if (osdPos_y == 16)
{
osdPos_y = 0;
}
}
}
uint8_t cmd = 0;
if (blink1 != blink)
{
cmd |= 128;//switch blink attr
blink = blink1;
}
bool highBank1 = lastChar > 255;
if (highBank1 != highBank)
{
cmd |= 64;//switch bank attr
highBank = highBank1;
}
if (count == 1 && cmd == 64)
{
sbufWriteU8(dst, 255); //short command for bank switch
sbufWriteU8(dst, lastChar & 0xff);
bytesCount += 2;
}
else if (count > 2 || cmd !=0 )
{
cmd |= count; //long command for blink/bank switch and symbol repeat
sbufWriteU8(dst, 0);
sbufWriteU8(dst, cmd);
sbufWriteU8(dst, lastChar & 0xff);
bytesCount += 3;
}
else if (count == 2) //cmd == 0 here
{
sbufWriteU8(dst, lastChar & 0xff);
sbufWriteU8(dst, lastChar & 0xff);
bytesCount+=2;
}
else
{
sbufWriteU8(dst, lastChar & 0xff);
bytesCount++;
}
if ( processedRows <= 0 )
{
break;
}
}
sbufWriteU8(dst, 0); //command 0 with length=0 -> stop
sbufWriteU8(dst, 0);
}
else
{
sbufWriteU8(dst, 255);
}
}
#endif
bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResult_e *ret)
{
uint8_t tmp_u8;
const unsigned int dataSize = sbufBytesRemaining(src);
switch (cmdMSP) {
case MSP_WP:
@ -3254,6 +3397,174 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
*ret = mspFcSafeHomeOutCommand(dst, src);
break;
#ifdef USE_SIMULATOR
case MSP_SIMULATOR:
tmp_u8 = sbufReadU8(src); //MSP_SIMULATOR version
if (tmp_u8 != 2) break;
simulatorData.flags = sbufReadU8(src);
if ((simulatorData.flags & SIMU_ENABLE) == 0) {
if (ARMING_FLAG(SIMULATOR_MODE)) { // just once
DISABLE_ARMING_FLAG(SIMULATOR_MODE);
#ifdef USE_BARO
baroStartCalibration();
#endif
#ifdef USE_MAG
DISABLE_STATE(COMPASS_CALIBRATED);
compassInit();
#endif
simulatorData.flags = 0;
//review: many states were affected. reboot?
disarm(DISARM_SWITCH); //disarm to prevent motor output!!!
}
}
else if (!areSensorsCalibrating()) {
if (!ARMING_FLAG(SIMULATOR_MODE)) { // just once
#ifdef USE_BARO
baroStartCalibration();
#endif
#ifdef USE_MAG
if (compassConfig()->mag_hardware != MAG_NONE){
sensorsSet(SENSOR_MAG);
ENABLE_STATE(COMPASS_CALIBRATED);
DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
mag.magADC[X] = 800;
mag.magADC[Y] = 0;
mag.magADC[Z] = 0;
}
#endif
ENABLE_ARMING_FLAG(SIMULATOR_MODE);
LOG_D(SYSTEM, "Simulator enabled");
}
if (dataSize >= 14) {
if (feature(FEATURE_GPS) && ((simulatorData.flags & SIMU_HAS_NEW_GPS_DATA)!=0) ) {
gpsSol.fixType = sbufReadU8(src);
gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
gpsSol.flags.hasNewData = true;
gpsSol.numSat = sbufReadU8(src);
if (gpsSol.fixType != GPS_NO_FIX) {
gpsSol.flags.validVelNE = 1;
gpsSol.flags.validVelD = 1;
gpsSol.flags.validEPE = 1;
gpsSol.llh.lat = sbufReadU32(src);
gpsSol.llh.lon = sbufReadU32(src);
gpsSol.llh.alt = sbufReadU32(src);
gpsSol.groundSpeed = (int16_t)sbufReadU16(src);
gpsSol.groundCourse = (int16_t)sbufReadU16(src);
gpsSol.velNED[X] = (int16_t)sbufReadU16(src);
gpsSol.velNED[Y] = (int16_t)sbufReadU16(src);
gpsSol.velNED[Z] = (int16_t)sbufReadU16(src);
gpsSol.eph = 100;
gpsSol.epv = 100;
ENABLE_STATE(GPS_FIX);
// Feed data to navigation
gpsProcessNewSolutionData();
}
else {
sbufAdvance(src, 4 + 4 + 4 + 2 + 2 + 2 * 3);
DISABLE_STATE(GPS_FIX);
}
}
else {
sbufAdvance(src, 1 + 1 + 4 + 4 + 4 + 2 + 2 + 2 * 3);
}
if ((simulatorData.flags & SIMU_USE_SENSORS) == 0) {
attitude.values.roll = (int16_t)sbufReadU16(src);
attitude.values.pitch = (int16_t)sbufReadU16(src);
attitude.values.yaw = (int16_t)sbufReadU16(src);
}
else
{
sbufAdvance(src, 2*3);
}
acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;// acceleration in 1G units
acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f;
acc.accVibeSq[X] = 0;
acc.accVibeSq[Y] = 0;
acc.accVibeSq[Z] = 0;
gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f;
if (sensors(SENSOR_BARO))
{
baro.baroPressure = (int32_t)sbufReadU32(src);
baro.baroTemperature = 2500;
}
else {
sbufAdvance(src,4);
}
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[Z] = ((int16_t)sbufReadU16(src)) / 20;
}
else {
sbufAdvance(src, 2*3);
}
if (simulatorData.flags & SIMU_EXT_BATTERY_VOLTAGE) {
simulatorData.vbat = sbufReadU8(src);
}
else {
simulatorData.vbat = 126;
}
if (simulatorData.flags & SIMU_AIRSPEED) {
simulatorData.airSpeed = sbufReadU16(src);
}
}
else {
DISABLE_STATE(GPS_FIX);
}
}
sbufWriteU16(dst, (uint16_t)simulatorData.INPUT_STABILIZED_ROLL);
sbufWriteU16(dst, (uint16_t)simulatorData.INPUT_STABILIZED_PITCH);
sbufWriteU16(dst, (uint16_t)simulatorData.INPUT_STABILIZED_YAW);
sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.INPUT_STABILIZED_THROTTLE : -500));
simulatorData.debugIndex++;
if (simulatorData.debugIndex == 8) {
simulatorData.debugIndex = 0;
}
tmp_u8 = simulatorData.debugIndex |
((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) |
(ARMING_FLAG(ARMED) ? 64 : 0) |
(!feature(FEATURE_OSD) ? 32: 0) |
(!isOSDTypeSupportedBySimulator() ? 16: 0);
sbufWriteU8(dst, tmp_u8 );
sbufWriteU32(dst, debug[simulatorData.debugIndex]);
sbufWriteU16(dst, attitude.values.roll);
sbufWriteU16(dst, attitude.values.pitch);
sbufWriteU16(dst, attitude.values.yaw);
mspWriteSimulatorOSD(dst);
*ret = MSP_RESULT_ACK;
break;
#endif
default:
// Not handled
return false;

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

@ -306,7 +306,13 @@ void taskUpdateAux(timeUs_t currentTimeUs)
{
updatePIDCoefficients();
dynamicLpfGyroTask();
#ifdef USE_SIMULATOR
if (!ARMING_FLAG(SIMULATOR_MODE)) {
updateFixedWingLevelTrim(currentTimeUs);
}
#else
updateFixedWingLevelTrim(currentTimeUs);
#endif
}
void fcTasksInit(void)

7
src/main/fc/runtime_config.c
View file

@ -176,3 +176,10 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO;
}
#ifdef USE_SIMULATOR
simulatorData_t simulatorData = {
flags: 0,
debugIndex: 0
};
#endif

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

@ -21,6 +21,7 @@
typedef enum {
ARMED = (1 << 2),
WAS_EVER_ARMED = (1 << 3),
SIMULATOR_MODE = (1 << 4),
ARMING_DISABLED_FAILSAFE_SYSTEM = (1 << 7),
ARMING_DISABLED_NOT_LEVEL = (1 << 8),
@ -164,6 +165,32 @@ typedef enum {
flightModeForTelemetry_e getFlightModeForTelemetry(void);
#ifdef USE_SIMULATOR
typedef enum {
SIMU_ENABLE = (1 << 0),
SIMU_SIMULATE_BATTERY = (1 << 1),
SIMU_MUTE_BEEPER = (1 << 2),
SIMU_USE_SENSORS = (1 << 3),
SIMU_HAS_NEW_GPS_DATA = (1 << 4),
SIMU_EXT_BATTERY_VOLTAGE = (1 << 5),//extend MSP_SIMULATOR format 2
SIMU_AIRSPEED = (1 << 6)
} simulatorFlags_t;
typedef struct {
simulatorFlags_t flags;
uint8_t debugIndex;
uint8_t vbat; //126 ->12.6V
uint16_t airSpeed; //cm/s
int16_t INPUT_STABILIZED_ROLL;
int16_t INPUT_STABILIZED_PITCH;
int16_t INPUT_STABILIZED_YAW;
int16_t INPUT_STABILIZED_THROTTLE;
} simulatorData_t;
extern simulatorData_t simulatorData;
#endif
uint32_t enableFlightMode(flightModeFlags_e mask);
uint32_t disableFlightMode(flightModeFlags_e mask);

15
src/main/flight/imu.c
View file

@ -327,6 +327,12 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
// Normalize to unit vector
vectorNormalize(&vMag, &vMag);
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) {
imuSetMagneticDeclination(0);
}
#endif
// Reference mag field vector heading is Magnetic North in EF. We compute that by rotating True North vector by declination and assuming Z-component is zero
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
vectorCrossProduct(&vErr, &vMag, &vCorrectedMagNorth);
@ -455,10 +461,19 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
{
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE) && ((simulatorData.flags & SIMU_USE_SENSORS) == 0)) {
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw);
imuComputeRotationMatrix();
}
else
#endif
{
/* Compute pitch/roll angles */
attitude.values.roll = RADIANS_TO_DECIDEGREES(atan2_approx(rMat[2][1], rMat[2][2]));
attitude.values.pitch = RADIANS_TO_DECIDEGREES((0.5f * M_PIf) - acos_approx(-rMat[2][0]));
attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0]));
}
if (attitude.values.yaw < 0)
attitude.values.yaw += 3600;

12
src/main/flight/servos.c
View file

@ -320,6 +320,13 @@ void servoMixer(float dT)
input[INPUT_RC_CH16] = GET_RX_CHANNEL_INPUT(AUX12);
#undef GET_RX_CHANNEL_INPUT
#ifdef USE_SIMULATOR
simulatorData.INPUT_STABILIZED_ROLL = input[INPUT_STABILIZED_ROLL];
simulatorData.INPUT_STABILIZED_PITCH = input[INPUT_STABILIZED_PITCH];
simulatorData.INPUT_STABILIZED_YAW = input[INPUT_STABILIZED_YAW];
simulatorData.INPUT_STABILIZED_THROTTLE = input[INPUT_STABILIZED_THROTTLE];
#endif
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = 0;
}
@ -565,6 +572,11 @@ void processContinuousServoAutotrim(const float dT)
}
void processServoAutotrim(const float dT) {
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) {
return;
}
#endif
if (feature(FEATURE_FW_AUTOTRIM)) {
processContinuousServoAutotrim(dT);
} else {

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

@ -344,6 +344,14 @@ bool gpsUpdate(void)
return false;
}
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) {
gpsUpdateTime();
gpsSetState(GPS_RUNNING);
sensorsSet(SENSOR_GPS);
return gpsSol.flags.hasNewData;
}
#endif
#ifdef USE_FAKE_GPS
return gpsFakeGPSUpdate();
#else

2
src/main/io/osd.c
View file

@ -846,6 +846,8 @@ static const char * osdArmingDisabledReasonMessage(void)
FALLTHROUGH;
case ARMED:
FALLTHROUGH;
case SIMULATOR_MODE:
FALLTHROUGH;
case WAS_EVER_ARMED:
break;
}

2
src/main/io/osd_dji_hd.c
View file

@ -537,6 +537,8 @@ static char * osdArmingDisabledReasonMessage(void)
FALLTHROUGH;
case ARMED:
FALLTHROUGH;
case SIMULATOR_MODE:
FALLTHROUGH;
case WAS_EVER_ARMED:
break;
}

2
src/main/msp/msp_protocol_v2_inav.h
View file

@ -53,6 +53,8 @@
#define MSP2_INAV_SET_TEMP_SENSOR_CONFIG 0x201D
#define MSP2_INAV_TEMPERATURES 0x201E
#define MSP_SIMULATOR 0x201F
#define MSP2_INAV_SERVO_MIXER 0x2020
#define MSP2_INAV_SET_SERVO_MIXER 0x2021
#define MSP2_INAV_LOGIC_CONDITIONS 0x2022

5
src/main/navigation/navigation_pos_estimator.c
View file

@ -448,6 +448,11 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
/* If calibration is incomplete - report zero acceleration */
if (gravityCalibrationComplete()) {
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) {
posEstimator.imu.calibratedGravityCMSS = GRAVITY_CMSS;
}
#endif
posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
}
else {

2
src/main/navigation/navigation_pos_estimator_private.h
View file

@ -183,6 +183,8 @@ typedef struct {
fpVector3_t accBiasCorr;
} estimationContext_t;
extern navigationPosEstimator_t posEstimator;
extern float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w);
extern void estimationCalculateAGL(estimationContext_t * ctx);
extern bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx);

7
src/main/sensors/acceleration.c
View file

@ -503,6 +503,13 @@ float accGetMeasuredMaxG(void)
void accUpdate(void)
{
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) {
//output: acc.accADCf
//unused: acc.dev.ADCRaw[], acc.accClipCount, acc.accVibeSq[]
return;
}
#endif
if (!acc.dev.readFn(&acc.dev)) {
return;
}

7
src/main/sensors/barometer.c
View file

@ -276,7 +276,14 @@ uint32_t baroUpdate(void)
if (baro.dev.start_ut) {
baro.dev.start_ut(&baro.dev);
}
#ifdef USE_SIMULATOR
if (!ARMING_FLAG(SIMULATOR_MODE)) {
//output: baro.baroPressure, baro.baroTemperature
baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature);
}
#else
baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature);
#endif
state = BAROMETER_NEEDS_SAMPLES;
return baro.dev.ut_delay;
break;

11
src/main/sensors/battery.c
View file

@ -290,7 +290,16 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
vbat = 0;
break;
}
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) {
if (simulatorData.flags & SIMU_SIMULATE_BATTERY) {
vbat = ((uint16_t)simulatorData.vbat)*10;
batteryFullVoltage = 1260;
batteryWarningVoltage = 1020;
batteryCriticalVoltage = 960;
}
}
#endif
if (justConnected) {
pt1FilterReset(&vbatFilterState, vbat);
} else {

6
src/main/sensors/compass.c
View file

@ -356,6 +356,12 @@ bool compassIsCalibrationComplete(void)
void compassUpdate(timeUs_t currentTimeUs)
{
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) {
magUpdatedAtLeastOnce = 1;
return;
}
#endif
static sensorCalibrationState_t calState;
static timeUs_t calStartedAt = 0;
static int16_t magPrev[XYZ_AXIS_COUNT];

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

@ -63,6 +63,11 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void)
hardwareSensorStatus_e getHwCompassStatus(void)
{
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE) && sensors(SENSOR_MAG)) {
return HW_SENSOR_OK;
}
#endif
#if defined(USE_MAG)
if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
if (compassIsHealthy()) {

7
src/main/sensors/gyro.c
View file

@ -499,6 +499,13 @@ void FAST_CODE NOINLINE gyroFilter()
void FAST_CODE NOINLINE gyroUpdate()
{
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE)) {
//output: gyro.gyroADCf[axis]
//unused: dev->gyroADCRaw[], dev->gyroZero[];
return;
}
#endif
if (!gyro.initialized) {
return;
}

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

@ -205,6 +205,11 @@ STATIC_PROTOTHREAD(pitotThread)
}
pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL);
#ifdef USE_SIMULATOR
if (simulatorData.flags & SIMU_AIRSPEED) {
pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
}
#endif
ptYield();
// Filter pressure
@ -228,6 +233,11 @@ STATIC_PROTOTHREAD(pitotThread)
performPitotCalibrationCycle();
pitot.airSpeed = 0;
}
#ifdef USE_SIMULATOR
if (simulatorData.flags & SIMU_AIRSPEED) {
pitot.airSpeed = simulatorData.airSpeed;
}
#endif
}
ptEnd(0);

2
src/main/target/common.h
View file

@ -184,6 +184,8 @@
// Wind estimator
#define USE_WIND_ESTIMATOR
#define USE_SIMULATOR
//Designed to free space of F722 and F411 MCUs
#if (MCU_FLASH_SIZE > 512)