From dd7df4c1bdee08f8e6582a07b740b13fd16fc939 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 26 Jun 2022 09:19:31 +0300 Subject: [PATCH] fixed attitude --- src/main/fc/fc_msp.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 63d59742c9..2b5094bc06 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -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); }