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

fixed attitude

Browse source
This commit is contained in:
Roman Lut 2022-06-26 09:19:31 +03:00
parent 0906f4241b
commit dd7df4c1bd
1 changed files with 11 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -3446,8 +3446,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
gpsSol.llh.lat = sbufReadU32(src);
gpsSol.llh.lon = sbufReadU32(src);
gpsSol.llh.alt = sbufReadU32(src);
gpsSol.groundSpeed = sbufReadU16(src);
gpsSol.groundCourse = sbufReadU16(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;
@ -3456,17 +3456,17 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
// Feed data to navigation
gpsProcessNewSolutionData();
attitude.values.roll = sbufReadU16(src) - 1800;
attitude.values.pitch = sbufReadU16(src) - 1800;
attitude.values.yaw = sbufReadU16(src);
attitude.values.roll = (int16_t)sbufReadU16(src);
attitude.values.pitch = (int16_t)sbufReadU16(src);
attitude.values.yaw = (int16_t)sbufReadU16(src);
acc.accADCf[X] = (float)sbufReadU16(src) / 1000 - 32;// / GRAVITY_CMSS;
acc.accADCf[Y] = (float)sbufReadU16(src) / 1000 - 32;// / GRAVITY_CMSS;
acc.accADCf[Z] = (float)sbufReadU16(src) / 1000 - 32;// / GRAVITY_CMSS;
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;
gyro.gyroADCf[X] = (float)sbufReadU16(src) / 100 - 320;
gyro.gyroADCf[Y] = (float)sbufReadU16(src) / 100 - 320;
gyro.gyroADCf[Z] = (float)sbufReadU16(src) / 100 - 320;
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;
baro.baroPressure = sbufReadU32(src);
}