Add Horizon mode inverted self-leveling

Co-authored-by: Ken Imhof <[email protected]>

src/main/common/maths.h

@@ -128,6 +128,9 @@ static inline float tan_approx(float x)
     __typeof__ (x) _x = (x); \
     _x > 0 ? _x : -_x; })
 
+#define SIGN(x) \
+  __extension__ ({ __typeof__ (x) _x = (x); \
+  (_x > 0) - (_x < 0); })
 
 /*
  * Basic math operations

src/main/flight/imu.c

@@ -634,3 +634,12 @@ bool isUpright(void)
     return true;
 #endif
 }
+
+bool isUpsidedown(void)
+{
+#ifdef USE_ACC
+    return sensors(SENSOR_ACC) && attitudeIsEstablished && getCosTiltAngle() < 0;
+#else
+    return false;
+#endif
+}

src/main/flight/imu.h

@@ -83,3 +83,4 @@ void imuSetHasNewData(uint32_t dt);
 
 bool shouldInitializeGPSHeading(void);
 bool isUpright(void);
+bool isUpsidedown(void);

src/main/flight/leveling.c

@@ -100,7 +100,20 @@ static float calcLevelErrorAngle(int axis)
 #endif
     angle = constrainf(angle, -level.AngleLimit, level.AngleLimit);
 
-    float error = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
+    float currentAngle = ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f);
+
+    if (isUpsidedown() && FLIGHT_MODE(HORIZON_MODE)) {
+        switch (axis) {
+            case FD_PITCH:
+                currentAngle = -currentAngle;
+                break;
+            case FD_ROLL:
+                currentAngle -= SIGN(currentAngle) * 180;
+                break;
+        }
+    }
+
+    float error = angle - currentAngle;
 
     return error;
 }