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

simulation GPS rate 5Hz

Browse source 
simulation GPS provide velocity
simulation return ARMED flag
This commit is contained in:
Roman Lut 2022-07-02 02:49:01 +03:00
parent 9b257b0948
commit 0f70b2d5fb
4 changed files with 63 additions and 35 deletions
Download patch file Download diff file Expand all files Collapse all files

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

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

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

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

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

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

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

@ -31,6 +31,8 @@
#define BEEPER PC13
#define BEEPER_INVERTED
#define USE_PITOT_VIRTUAL
// MPU6000
#define USE_IMU_MPU6000