From 0f70b2d5fb604d618e6743d2a79f5d386cda506e Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sat, 2 Jul 2022 02:49:01 +0300 Subject: [PATCH] simulation GPS rate 5Hz simulation GPS provide velocity simulation return ARMED flag --- src/main/fc/fc_msp.c | 74 +++++++++++++++++++----------- src/main/fc/runtime_config.h | 3 +- src/main/flight/imu.c | 19 ++++---- src/main/target/GRAVITYF7/target.h | 2 + 4 files changed, 63 insertions(+), 35 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 37138cc941..86d765a984 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3448,50 +3448,65 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (dataSize >= 14) { - if (feature(FEATURE_GPS)) { + 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.flags.validVelNE = 0; - gpsSol.flags.validVelD = 0; - gpsSol.flags.validEPE = 0; 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] = 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.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.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; + /* + //------------------------ + gpsSol.flags.validVelNE = 0; + gpsSol.flags.validVelD = 0; + gpsSol.flags.validEPE = 0; + + debug[0] = gpsSol.velNED[X]; + debug[1] = gpsSol.velNED[Y]; + debug[2] = gpsSol.velNED[Z]; + + debug[3] = posEstimator.gps.vel.x; + debug[4] = posEstimator.gps.vel.y; + debug[5] = posEstimator.gps.vel.z; + + debug[6] = gpsSol.groundSpeed * cos_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse)); + debug[7] = gpsSol.groundSpeed * sin_approx(DECIDEGREES_TO_RADIANS(gpsSol.groundCourse)); + //------------------------ + */ + ENABLE_STATE(GPS_FIX); // Feed data to navigation gpsProcessNewSolutionData(); } else { - sbufReadU32(src); - sbufReadU32(src); - sbufReadU32(src); - sbufReadU16(src); - sbufReadU16(src); - + sbufAdvance(src, 4 + 4 + 4 + 2 + 2 + 2 * 3); DISABLE_STATE(GPS_FIX); } } else { - sbufReadU8(src); - sbufReadU8(src); - sbufReadU32(src); - sbufReadU32(src); - sbufReadU32(src); - sbufReadU16(src); - sbufReadU16(src); + sbufAdvance(src, 1 + 1 + 4 + 4 + 4 + 2 + 2 + 2 * 3); } if ((simulatorData.flags & SIMU_USE_SENSORS) == 0) { @@ -3501,9 +3516,17 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu } else { - sbufReadU16(src); - sbufReadU16(src); - sbufReadU16(src); + sbufAdvance(src, 2*3); + + /* + debug[0] = (int16_t)sbufReadU16(src); + debug[1] = (int16_t)sbufReadU16(src); + debug[2] = (int16_t)sbufReadU16(src); + + debug[3] = attitude.values.roll; + debug[4] = attitude.values.pitch; + debug[5] = attitude.values.yaw; + */ } acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;// acceleration in 1G units @@ -3523,9 +3546,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu baro.baroTemperature = 2500; } else { - baro.baroPressure = sbufReadU32(src); + sbufAdvance(src,4); } - } else { DISABLE_STATE(GPS_FIX); @@ -3542,7 +3564,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu simulatorData.debugIndex = 0; } - tmp_u8 = simulatorData.debugIndex | ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0); + tmp_u8 = simulatorData.debugIndex | ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) | (ARMING_FLAG(ARMED) ? 64 : 0); sbufWriteU8(dst, tmp_u8 ); sbufWriteU32(dst, debug[simulatorData.debugIndex]); diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 800145bf54..8ce3f5890b 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -170,7 +170,8 @@ typedef enum { SIMU_ENABLE = (1 << 0), SIMU_SIMULATE_BATTERY = (1 << 1), SIMU_MUTE_BEEPER = (1 << 2), - SIMU_USE_SENSORS = (1 << 3) + SIMU_USE_SENSORS = (1 << 3), + SIMU_HAS_NEW_GPS_DATA = (1 << 4) } simulatorFlags_t; typedef struct { diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 456f97d62e..42e7580273 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -455,10 +455,17 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) { - /* 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 (ARMING_FLAG(SIMULATOR_MODE) && ((simulatorData.flags & SIMU_USE_SENSORS) == 0)) { + imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw); + imuComputeRotationMatrix(); + } + else + { + /* 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; @@ -573,10 +580,6 @@ static void imuCalculateEstimatedAttitude(float dT) const float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeight(dT); const bool useAcc = (accWeight > 0.001f); - if (ARMING_FLAG(SIMULATOR_MODE) && ((simulatorData.flags & SIMU_USE_SENSORS) == 0 )) { - return; - } - imuMahonyAHRSupdate(dT, &imuMeasuredRotationBF, useAcc ? &imuMeasuredAccelBF : NULL, useMag ? &measuredMagBF : NULL, diff --git a/src/main/target/GRAVITYF7/target.h b/src/main/target/GRAVITYF7/target.h index 0891865587..a770fbc861 100644 --- a/src/main/target/GRAVITYF7/target.h +++ b/src/main/target/GRAVITYF7/target.h @@ -31,6 +31,8 @@ #define BEEPER PC13 #define BEEPER_INVERTED +#define USE_PITOT_VIRTUAL + // MPU6000 #define USE_IMU_MPU6000