@@ -11,65 +11,65 @@ namespace FGamepadSensors
1111 FGamepadCalibration& OutCalibration)
1212 {
1313 auto GetLE16 = [](const std::uint8_t * Data) -> std::int16_t {
14- return static_cast <std::int16_t >(Data[0 ] | (Data[1 ] << 8 ));
15- };
16-
17- // Todos agora são int16_t
18- const std::int16_t GyroPitchBias = GetLE16 (&Buffer[1 ]);
19- const std::int16_t GyroYawBias = GetLE16 (&Buffer[3 ]);
20- const std::int16_t GyroRollBias = GetLE16 (&Buffer[5 ]);
21-
22- const std::int16_t GyroPitchPlus = GetLE16 (&Buffer[7 ]);
23- const std::int16_t GyroPitchMinus = GetLE16 (&Buffer[9 ]);
24- const std::int16_t GyroYawPlus = GetLE16 (&Buffer[11 ]);
25- const std::int16_t GyroYawMinus = GetLE16 (&Buffer[13 ]);
26- const std::int16_t GyroRollPlus = GetLE16 (&Buffer[15 ]);
27- const std::int16_t GyroRollMinus = GetLE16 (&Buffer[17 ]);
28-
29- const std::int16_t GyroSpeedPlus = GetLE16 (&Buffer[19 ]);
30- const std::int16_t GyroSpeedMinus = GetLE16 (&Buffer[21 ]);
31-
32- const std::int16_t AccelXPlus = GetLE16 (&Buffer[23 ]);
33- const std::int16_t AccelXMinus = GetLE16 (&Buffer[25 ]);
34- const std::int16_t AccelYPlus = GetLE16 (&Buffer[27 ]);
35- const std::int16_t AccelYMinus = GetLE16 (&Buffer[29 ]);
36- const std::int16_t AccelZPlus = GetLE16 (&Buffer[31 ]);
37- const std::int16_t AccelZMinus = GetLE16 (&Buffer[33 ]);
38-
39- // Gyro Biases (Cuidado: Você tinha código duplicado aqui, limpei)
40- OutCalibration.GyroBiasX = static_cast <float >(GyroPitchBias);
41- OutCalibration.GyroBiasY = static_cast <float >(GyroYawBias);
42- OutCalibration.GyroBiasZ = static_cast <float >(GyroRollBias);
43-
44- // Fatores do Gyro
45- // Usamos float para garantir precisão na soma antes da divisão
46- const float Speed2x = static_cast <float >(GyroSpeedPlus + GyroSpeedMinus);
47-
48- // A logica ABS funciona melhor agora que são int16
49- float DenomX = static_cast <float >(std::abs (GyroPitchPlus - GyroPitchBias) + std::abs (GyroPitchMinus - GyroPitchBias));
50- OutCalibration.GyroFactorX = (DenomX != 0 .0f ) ? (Speed2x / DenomX) : 1 .0f ;
51-
52- float DenomY = static_cast <float >(std::abs (GyroYawPlus - GyroYawBias) + std::abs (GyroYawMinus - GyroYawBias));
53- OutCalibration.GyroFactorY = (DenomY != 0 .0f ) ? (Speed2x / DenomY) : 1 .0f ;
54-
55- float DenomZ = static_cast <float >(std::abs (GyroRollPlus - GyroRollBias) + std::abs (GyroRollMinus - GyroRollBias));
56- OutCalibration.GyroFactorZ = (DenomZ != 0 .0f ) ? (Speed2x / DenomZ) : 1 .0f ;
57-
58- // Acc X
59- // IMPORTANTE: Agora (XPlus - XMinus) fará a conta correta: ex: 8192 - (-8192) = 16384
60- const float RangeX = static_cast <float >(AccelXPlus - AccelXMinus);
61- OutCalibration.AccelBiasX = (AccelXPlus + AccelXMinus) / 2 .0f ; // Bias agora será próximo de 0
62- OutCalibration.AccelFactorX = (RangeX != 0 .0f ) ? (2 .0f / RangeX) : 1 .0f ;
63-
64- // Acc Y
65- const float RangeY = static_cast <float >(AccelYPlus - AccelYMinus);
66- OutCalibration.AccelBiasY = (AccelYPlus + AccelYMinus) / 2 .0f ;
67- OutCalibration.AccelFactorY = (RangeY != 0 .0f ) ? (2 .0f / RangeY) : 1 .0f ;
68-
69- // Acc Z
70- const float RangeZ = static_cast <float >(AccelZPlus - AccelZMinus);
71- OutCalibration.AccelBiasZ = (AccelZPlus + AccelZMinus) / 2 .0f ;
72- OutCalibration.AccelFactorZ = (RangeZ != 0 .0f ) ? (2 .0f / RangeZ) : 1 .0f ;
14+ return static_cast <std::int16_t >(Data[0 ] | (Data[1 ] << 8 ));
15+ };
16+
17+ // Todos agora são int16_t
18+ const std::int16_t GyroPitchBias = GetLE16 (&Buffer[1 ]);
19+ const std::int16_t GyroYawBias = GetLE16 (&Buffer[3 ]);
20+ const std::int16_t GyroRollBias = GetLE16 (&Buffer[5 ]);
21+
22+ const std::int16_t GyroPitchPlus = GetLE16 (&Buffer[7 ]);
23+ const std::int16_t GyroPitchMinus = GetLE16 (&Buffer[9 ]);
24+ const std::int16_t GyroYawPlus = GetLE16 (&Buffer[11 ]);
25+ const std::int16_t GyroYawMinus = GetLE16 (&Buffer[13 ]);
26+ const std::int16_t GyroRollPlus = GetLE16 (&Buffer[15 ]);
27+ const std::int16_t GyroRollMinus = GetLE16 (&Buffer[17 ]);
28+
29+ const std::int16_t GyroSpeedPlus = GetLE16 (&Buffer[19 ]);
30+ const std::int16_t GyroSpeedMinus = GetLE16 (&Buffer[21 ]);
31+
32+ const std::int16_t AccelXPlus = GetLE16 (&Buffer[23 ]);
33+ const std::int16_t AccelXMinus = GetLE16 (&Buffer[25 ]);
34+ const std::int16_t AccelYPlus = GetLE16 (&Buffer[27 ]);
35+ const std::int16_t AccelYMinus = GetLE16 (&Buffer[29 ]);
36+ const std::int16_t AccelZPlus = GetLE16 (&Buffer[31 ]);
37+ const std::int16_t AccelZMinus = GetLE16 (&Buffer[33 ]);
38+
39+ // Gyro Biases (Cuidado: Você tinha código duplicado aqui, limpei)
40+ OutCalibration.GyroBiasX = static_cast <float >(GyroPitchBias);
41+ OutCalibration.GyroBiasY = static_cast <float >(GyroYawBias);
42+ OutCalibration.GyroBiasZ = static_cast <float >(GyroRollBias);
43+
44+ // Fatores do Gyro
45+ // Usamos float para garantir precisão na soma antes da divisão
46+ const float Speed2x = static_cast <float >(GyroSpeedPlus + GyroSpeedMinus);
47+
48+ // A logica ABS funciona melhor agora que são int16
49+ float DenomX = static_cast <float >(std::abs (GyroPitchPlus - GyroPitchBias) + std::abs (GyroPitchMinus - GyroPitchBias));
50+ OutCalibration.GyroFactorX = (DenomX != 0 .0f ) ? (Speed2x / DenomX) : 1 .0f ;
51+
52+ float DenomY = static_cast <float >(std::abs (GyroYawPlus - GyroYawBias) + std::abs (GyroYawMinus - GyroYawBias));
53+ OutCalibration.GyroFactorY = (DenomY != 0 .0f ) ? (Speed2x / DenomY) : 1 .0f ;
54+
55+ float DenomZ = static_cast <float >(std::abs (GyroRollPlus - GyroRollBias) + std::abs (GyroRollMinus - GyroRollBias));
56+ OutCalibration.GyroFactorZ = (DenomZ != 0 .0f ) ? (Speed2x / DenomZ) : 1 .0f ;
57+
58+ // Acc X
59+ // IMPORTANTE: Agora (XPlus - XMinus) fará a conta correta: ex: 8192 - (-8192) = 16384
60+ const float RangeX = static_cast <float >(AccelXPlus - AccelXMinus);
61+ OutCalibration.AccelBiasX = (AccelXPlus + AccelXMinus) / 2 .0f ; // Bias agora será próximo de 0
62+ OutCalibration.AccelFactorX = (RangeX != 0 .0f ) ? (2 .0f / RangeX) : 1 .0f ;
63+
64+ // Acc Y
65+ const float RangeY = static_cast <float >(AccelYPlus - AccelYMinus);
66+ OutCalibration.AccelBiasY = (AccelYPlus + AccelYMinus) / 2 .0f ;
67+ OutCalibration.AccelFactorY = (RangeY != 0 .0f ) ? (2 .0f / RangeY) : 1 .0f ;
68+
69+ // Acc Z
70+ const float RangeZ = static_cast <float >(AccelZPlus - AccelZMinus);
71+ OutCalibration.AccelBiasZ = (AccelZPlus + AccelZMinus) / 2 .0f ;
72+ OutCalibration.AccelFactorZ = (RangeZ != 0 .0f ) ? (2 .0f / RangeZ) : 1 .0f ;
7373 }
7474
7575 inline void ProcessMotionData (const std::uint8_t * Buffer,
0 commit comments