diff --git a/radio/src/gui/128x64/radio_diaganas.cpp b/radio/src/gui/128x64/radio_diaganas.cpp index 83571e31d..c224ccf98 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.outputs[0], RIGHT); + lcdDrawNumber(x+10*FW-1, y, gyro.scaled_outputs[0], 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.outputs[1], RIGHT); + lcdDrawNumber(x+10*FW-1, y, gyro.scaled_outputs[1], RIGHT); #endif // RAS diff --git a/radio/src/gyro.cpp b/radio/src/gyro.cpp index 5fa2594ff..84b57b80d 100644 --- a/radio/src/gyro.cpp +++ b/radio/src/gyro.cpp @@ -66,5 +66,8 @@ 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 1f14d7261..7b3240d15 100644 --- a/radio/src/gyro.h +++ b/radio/src/gyro.h @@ -44,6 +44,7 @@ class Gyro { public: int16_t outputs[2]; + int16_t scaled_outputs[2]; void wakeup(); }; diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index d3285bd0c..73cc61713 100644 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -317,7 +317,7 @@ getvalue_t getValue(mixsrc_t i) #if defined(GYRO) else if (i <= MIXSRC_GYRO2) { - return gyro.outputs[i - MIXSRC_GYRO1]; + return gyro.scaled_outputs[i - MIXSRC_GYRO1]; } #endif diff --git a/radio/src/targets/taranis/board.h b/radio/src/targets/taranis/board.h index f0d85ce12..978be7d96 100644 --- a/radio/src/targets/taranis/board.h +++ b/radio/src/targets/taranis/board.h @@ -813,7 +813,7 @@ int gyroInit(); int gyroRead(uint8_t buffer[GYRO_BUFFER_LENGTH]); #define GYRO_MAX_DEFAULT 30 #define GYRO_MAX_RANGE 60 -#define GYRO_OFFSET_MIN 10 -#define GYRO_OFFSET_MAX 90 +#define GYRO_OFFSET_MIN -30 +#define GYRO_OFFSET_MAX 10 #endif // _BOARD_H_