diff --git a/radio/src/gui/128x64/radio_diaganas.cpp b/radio/src/gui/128x64/radio_diaganas.cpp index c224ccf98..42c2a6b85 100644 --- a/radio/src/gui/128x64/radio_diaganas.cpp +++ b/radio/src/gui/128x64/radio_diaganas.cpp @@ -60,12 +60,12 @@ void menuRadioDiagAnalogs(event_t event) lcdDrawText(x, y, "X:"); lcdDrawNumber(x+3*FW-1, y, gyro.outputs[0] * 180 / 1024); lcdDrawChar(lcdNextPos, y, '@'); - lcdDrawNumber(x+10*FW-1, y, gyro.scaled_outputs[0], RIGHT); + lcdDrawNumber(x+10*FW-1, y, gyro.scaledX(), RIGHT); x = LCD_W/2 + INDENT_WIDTH; lcdDrawText(x, y, "Y:"); lcdDrawNumber(x+3*FW-1, y, gyro.outputs[1] * 180 / 1024); lcdDrawChar(lcdNextPos, y, '@'); - lcdDrawNumber(x+10*FW-1, y, gyro.scaled_outputs[1], RIGHT); + lcdDrawNumber(x+10*FW-1, y, gyro.scaledY(), RIGHT); #endif // RAS diff --git a/radio/src/gyro.cpp b/radio/src/gyro.cpp index c61e06af7..5822fc76b 100644 --- a/radio/src/gyro.cpp +++ b/radio/src/gyro.cpp @@ -67,8 +67,5 @@ void Gyro::wakeup() outputs[0] = rad2RESX(atan2f(accValues[1], accValues[2])); outputs[1] = rad2RESX(atan2f(-accValues[0], accValues[2])); - scaled_outputs[0] = limit(-RESX, (int)(outputs[0] - g_eeGeneral.gyroOffset * RESX/180) * (180 / (GYRO_MAX_DEFAULT + g_eeGeneral.gyroMax)), RESX); - scaled_outputs[1] = limit(-RESX, outputs[1] * (180 / (GYRO_MAX_DEFAULT + g_eeGeneral.gyroMax)), RESX); - // TRACE("%d %d", values[0], values[1]); } diff --git a/radio/src/gyro.h b/radio/src/gyro.h index 7b3240d15..fec951d12 100644 --- a/radio/src/gyro.h +++ b/radio/src/gyro.h @@ -44,8 +44,17 @@ class Gyro { public: int16_t outputs[2]; - int16_t scaled_outputs[2]; void wakeup(); + + int16_t scaledX() + { + return limit(-RESX, (int)(outputs[0] - g_eeGeneral.gyroOffset * RESX/180) * (180 / (GYRO_MAX_DEFAULT + g_eeGeneral.gyroMax)), RESX); + } + + int16_t scaledY() + { + return limit(-RESX, outputs[1] * (180 / (GYRO_MAX_DEFAULT + g_eeGeneral.gyroMax)), RESX); + } }; extern Gyro gyro; diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index 73cc61713..d1297af6b 100644 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -316,8 +316,11 @@ getvalue_t getValue(mixsrc_t i) } #if defined(GYRO) - else if (i <= MIXSRC_GYRO2) { - return gyro.scaled_outputs[i - MIXSRC_GYRO1]; + else if (i == MIXSRC_GYRO1) { + return gyro.scaledX(); + } + else if (i == MIXSRC_GYRO2) { + return gyro.scaledY(); } #endif