Add joystick mixer class

include/joystick_mixer.h

@@ -0,0 +1,33 @@
+#ifndef JOYSTICK_MIXER_H
+#define JOYSTICK_MIXER_H
+
+#include <stdint.h>
+
+class JoystickMixer
+{
+    private:
+
+    float x, y;
+    float dx, dy;
+
+    uint32_t count;
+
+    int32_t usb_x;
+    int32_t usb_y;
+    int32_t usb_r;
+
+    public:
+
+    JoystickMixer() {}
+    JoystickMixer(int32_t usb_x, int32_t usb_y, int32_t usb_r);
+
+    void init(int32_t usb_x, int32_t usb_y, int32_t usb_r);
+
+    void mix(float x, float y);
+
+    void mix_delta(float dx, float dy);
+
+    void getXY(int32_t &x, int32_t &y);
+};
+
+#endif

lib/touch_controls/include/touch_mouse_joystick.h

@@ -16,7 +16,10 @@ class TouchMouseJoystick : public TouchJoystick
     float dx;
     float dy;
 
+    int32_t usb_r2;
+
     int16_t min_delta;
+    int32_t min_delta2;
 
     float time0;
 

lib/touch_controls/src/touch_mouse_joystick.cpp

@@ -14,7 +14,10 @@ void TouchMouseJoystick::init(int32_t pos_x, int32_t pos_y, int32_t pos_r, int16
     this->trackball_vel_x = 0;
     this->trackball_vel_y = 0;
 
+    this->usb_r2 = usb_r * usb_r;
+
     this->min_delta = 0;
+    this->min_delta2 = 0;
 
     this->time0 = 0;
 }
@@ -41,6 +44,7 @@ void TouchMouseJoystick::setTrackballFriction(float trackball_friction)
 void TouchMouseJoystick::setMinDelta(int16_t min_delta)
 {
     this->min_delta = min_delta;
+    this->min_delta2 = min_delta * min_delta;
 }
 
 TouchMouseJoystick::TouchState TouchMouseJoystick::touch(int8_t fid, int32_t tx, int32_t ty, int32_t tdx, int32_t tdy, uint32_t time)
@@ -77,11 +81,23 @@ TouchMouseJoystick::TouchState TouchMouseJoystick::touch(int8_t fid, int32_t tx,
             dx = sensitivity * -tdx;
             dy = sensitivity * -tdy;
 
-            dx = dx > usb_r? usb_r : (dx < -usb_r? -usb_r : dx);
-            dy = dy > usb_r? usb_r : (dy < -usb_r? -usb_r : dy);
+            float d2 = dx * dx + dy * dy;
+
+            if (d2 < min_delta2)
+            {
+                float factor = min_delta / sqrt(d2);
+
+                dx = dx * factor;
+                dy = dy * factor;
+            }
+            else
+            if (d2 > usb_r2)
+            {
+                float factor = usb_r / sqrt(d2);
 
-            dx = dx > -min_delta && dx < 0? -min_delta : (dx < min_delta && dx > 0? min_delta : dx);
-            dy = dy > -min_delta && dy < 0? -min_delta : (dy < min_delta && dy > 0? min_delta : dy);
+                dx = dx * factor;
+                dy = dy * factor;
+            }
 
             float dt = time - time0;
             time0 = time;
@@ -114,6 +130,7 @@ TouchMouseJoystick::TouchState TouchMouseJoystick::touch(int8_t fid, int32_t tx,
 
 void TouchMouseJoystick::updateTrackball(uint32_t time)
 {
+    // todo change to circular ranges
     if (trackball_friction > 0 && !touching)
     {
         if (trackball_vel_x != 0)

src/joystick_mixer.cpp

@@ -0,0 +1,62 @@
+#include "joystick_mixer.h"
+
+#include <math.h>
+
+JoystickMixer::JoystickMixer(int32_t usb_x, int32_t usb_y, int32_t usb_r)
+{
+    init(usb_x, usb_y, usb_r);
+}
+
+void JoystickMixer::init(int32_t usb_x, int32_t usb_y, int32_t usb_r)
+{
+    this->usb_x = usb_x;
+    this->usb_y = usb_y;
+    this->usb_r = usb_r;
+
+    this->x = 0;
+    this->y = 0;
+    this->dx = 0;
+    this->dy = 0;
+
+    this->count = 0;
+}
+
+void JoystickMixer::mix(float x, float y)
+{
+    this->x += x;
+    this->y += y;
+
+    ++count;
+}
+
+void JoystickMixer::mix_delta(float dx, float dy)
+{
+    this->dx += dx;
+    this->dy += dy;
+}
+
+void JoystickMixer::getXY(int32_t &X, int32_t &Y)
+{
+    if (count > 0)
+    {
+        x = x / count + dx;
+        y = y / count + dy;
+    }
+    else
+    {
+        x = dx;
+        y = dy;
+    }
+
+    float len2 = x * x + y * y;
+    if (len2 > 1)
+    {
+        float len = sqrt(len2);
+
+        x /= len;
+        y /= len;
+    }
+
+    X = usb_x + x * usb_r;
+    Y = usb_y + y * usb_r;
+}

src/main.cpp

@@ -28,8 +28,8 @@ const uint8_t gyro_int = PC14;
 
 #ifndef DISABLE_TRACKPADS
 
-const uint8_t pin_trackpad_data[2]  = {PA2, PB9};
-const uint8_t pin_trackpad_clock[2] = {PA3, PB8};
+const uint8_t pin_trackpad_data[2]  = {PA2, PB9}; // elan 33200v-3600 pad T8
+const uint8_t pin_trackpad_clock[2] = {PA3, PB8}; // elan 33200v-3600 pad T7
 
 TrackPad trackpad[2]; // 0 - left, 1 - right