From c1cc55264594b51c54324178c40d846ae8bd1e82 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Mon, 27 Jun 2022 19:02:02 +0300 Subject: [PATCH] adjusted sensors usage in simulator mode --- src/main/drivers/display.c | 6 +- src/main/fc/fc_msp.c | 127 +++++++++++++++++++++++++++++++------------ src/main/fc/runtime_config.h | 3 +- src/main/flight/imu.c | 3 +- src/main/io/gps.c | 3 +- 5 files changed, 99 insertions(+), 43 deletions(-) diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 13b3f2679..0cabce055 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -180,10 +180,10 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c) if (ARMING_FLAG(SIMULATOR_MODE)) { //some FCs do not power max7456 from USB power - //driver can not read front metadata + //driver can not read font metadata //chip assumed to not support second bank of font - //artifical horizon, variometer and home direction are not drawn ( displat.c: displayUpdateMaxChar()) - //return dummy metadata so OSD work in simulator mode + //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; } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 00200167a..07057ccff 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3420,45 +3420,91 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu tmp_u8 = sbufReadU8(src); //MSP_SIMULATOR version if (tmp_u8 != 1) break; - tmp_u8 = sbufReadU8(src); - if ((tmp_u8 & SIMU_ENABLE) == 0) { - DISABLE_ARMING_FLAG(SIMULATOR_MODE); - baroStartCalibration(); // just once - simulatorData.flags = 0; + simulatorData.flags = sbufReadU8(src); + if ((simulatorData.flags & SIMU_ENABLE) == 0) { + + if (ARMING_FLAG(SIMULATOR_MODE)) { // just once + DISABLE_ARMING_FLAG(SIMULATOR_MODE); + + baroStartCalibration(); + + simulatorData.flags = 0; + //review: many states were affected. reboot? + } } - else { - simulatorData.flags = tmp_u8; + else if (!areSensorsCalibrating()) { + if (!ARMING_FLAG(SIMULATOR_MODE)) { // just once + baroStartCalibration(); + + //todo: magnetometer emulation support + sensorsClear(SENSOR_MAG); + mag.magADC[X] = 0; + mag.magADC[Y] = 0; + mag.magADC[Z] = 0; - if (!ARMING_FLAG(SIMULATOR_MODE)) { - baroStartCalibration(); // just once ENABLE_ARMING_FLAG(SIMULATOR_MODE); LOG_D(SYSTEM, "Simulator enabled"); } - if (dataSize >= 14 && isImuReady()) { - gpsSol.fixType = sbufReadU8(src); - gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; - gpsSol.flags.hasNewData = true; - gpsSol.flags.validVelNE = 0; - gpsSol.flags.validVelD = 0; - gpsSol.flags.validEPE = 0; - gpsSol.numSat = sbufReadU8(src); - 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] = gpsSol.groundSpeed * cos_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse)); - gpsSol.velNED[Y] = gpsSol.groundSpeed * sin_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse)); - gpsSol.velNED[Z] = 0; - gpsSol.eph = 100; - gpsSol.epv = 100; - // Feed data to navigation - gpsProcessNewSolutionData(); - - attitude.values.roll = (int16_t)sbufReadU16(src); - attitude.values.pitch = (int16_t)sbufReadU16(src); - attitude.values.yaw = (int16_t)sbufReadU16(src); + if (dataSize >= 14) { + + if (feature(FEATURE_GPS)) { + gpsSol.fixType = sbufReadU8(src); + gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; + gpsSol.flags.hasNewData = true; + gpsSol.flags.validVelNE = 0; + gpsSol.flags.validVelD = 0; + gpsSol.flags.validEPE = 0; + gpsSol.numSat = sbufReadU8(src); + + if (gpsSol.fixType != GPS_NO_FIX) { + 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] = gpsSol.groundSpeed * cos_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse)); + gpsSol.velNED[Y] = gpsSol.groundSpeed * sin_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse)); + gpsSol.velNED[Z] = 0; + gpsSol.eph = 100; + gpsSol.epv = 100; + + ENABLE_STATE(GPS_FIX); + + // Feed data to navigation + gpsProcessNewSolutionData(); + } + else { + sbufReadU32(src); + sbufReadU32(src); + sbufReadU32(src); + sbufReadU16(src); + sbufReadU16(src); + + DISABLE_STATE(GPS_FIX); + } + } + else { + sbufReadU8(src); + sbufReadU8(src); + sbufReadU32(src); + sbufReadU32(src); + sbufReadU32(src); + sbufReadU16(src); + sbufReadU16(src); + } + + 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 + { + sbufReadU16(src); + sbufReadU16(src); + sbufReadU16(src); + } acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;// acceleration in 1G units acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f; @@ -3471,8 +3517,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f; gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f; - baro.baroPressure = sbufReadU32(src); - baro.baroTemperature = 2500; + if (sensors(SENSOR_BARO)) + { + baro.baroPressure = sbufReadU32(src); + baro.baroTemperature = 2500; + } + else { + baro.baroPressure = sbufReadU32(src); + } + } else { DISABLE_STATE(GPS_FIX); @@ -3482,12 +3535,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_ROLL]); sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_PITCH]); sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_YAW]); - sbufWriteU16(dst, (uint16_t)input[INPUT_STABILIZED_THROTTLE]); + sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? input[INPUT_STABILIZED_THROTTLE] : -500)); simulatorData.debugIndex++; if (simulatorData.debugIndex == 8){ simulatorData.debugIndex = 0; } + + simulatorData.debugIndex |= (mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0; sbufWriteU8(dst, simulatorData.debugIndex); sbufWriteU32(dst, debug[simulatorData.debugIndex]); diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index c105e56f1..800145bf5 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -169,7 +169,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void); typedef enum { SIMU_ENABLE = (1 << 0), SIMU_SIMULATE_BATTERY = (1 << 1), - SIMU_MUTE_BEEPER = (1 << 2) + SIMU_MUTE_BEEPER = (1 << 2), + SIMU_USE_SENSORS = (1 << 3) } simulatorFlags_t; typedef struct { diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 46e2b10fa..456f97d62 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -573,8 +573,7 @@ static void imuCalculateEstimatedAttitude(float dT) const float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeight(dT); const bool useAcc = (accWeight > 0.001f); - if (ARMING_FLAG(SIMULATOR_MODE)) { - // todo: after imuMahonyAHRSupdate, ground course is divided by 10 =( + if (ARMING_FLAG(SIMULATOR_MODE) && ((simulatorData.flags & SIMU_USE_SENSORS) == 0 )) { return; } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index ca71eedeb..1e6c24214 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -362,7 +362,8 @@ bool gpsUpdate(void) if (ARMING_FLAG(SIMULATOR_MODE)) { gpsUpdateTime(); gpsSetState(GPS_RUNNING); - return gpsSol.flags.hasNewData; + sensorsSet(SENSOR_GPS); + return gpsSol.flags.hasNewData; } #ifdef USE_FAKE_GPS return gpsFakeGPSUpdate(); -- 2.11.4.GIT