FPV Boomerang

src/main/CMakeLists.txt

@@ -463,6 +463,8 @@ main_sources(COMMON_SRC
     sensors/sensors.c
     sensors/temperature.c
     sensors/temperature.h
+    sensors/rotor.c
+    sensors/rotor.h
 
     blackbox/blackbox.c
     blackbox/blackbox.h

src/main/fc/runtime_config.h

@@ -148,6 +148,7 @@ typedef enum {
     LANDING_DETECTED                    = (1 << 26),
     IN_FLIGHT_EMERG_REARM               = (1 << 27),
     TAILSITTER                          = (1 << 28), //offset the pitch angle by 90 degrees
+    BOOMERANG                           = (1 << 29),
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

src/main/fc/settings.yaml

@@ -107,7 +107,7 @@ tables:
     values: ["PERCENT", "MAH", "MWH"]
     enum: smartportFuelUnit_e
   - name: platform_type
-    values: ["MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT"]
+    values: ["MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT", "BOOMERANG"]
   - name: tz_automatic_dst
     values: ["OFF", "EU", "USA"]
     enum: tz_automatic_dst_e

src/main/flight/mixer.c

@@ -56,6 +56,9 @@
 #include "rx/rx.h"
 
 #include "sensors/battery.h"
+#include "sensors/rotor.h"
+
+#include "programming/global_variables.h"
 
 #define MAX_THROTTLE 2000
 #define MAX_THROTTLE_ROVER 1850
@@ -164,6 +167,7 @@ void mixerUpdateStateFlags(void)
     DISABLE_STATE(AIRPLANE);
     DISABLE_STATE(MOVE_FORWARD_ONLY);
     DISABLE_STATE(TAILSITTER);
+    DISABLE_STATE(BOOMERANG);
 
     if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) {
         ENABLE_STATE(FIXED_WING_LEGACY);
@@ -187,6 +191,9 @@ void mixerUpdateStateFlags(void)
     } else if (currentMixerConfig.platformType == PLATFORM_HELICOPTER) {
         ENABLE_STATE(MULTIROTOR);
         ENABLE_STATE(ALTITUDE_CONTROL);
+    } else if (currentMixerConfig.platformType == PLATFORM_BOOMERANG) {
+        ENABLE_STATE(BOOMERANG);
+        ENABLE_STATE(ALTITUDE_CONTROL);
     }
 
     if (currentMixerConfig.tailsitterOrientationOffset) {
@@ -493,6 +500,30 @@ static int getReversibleMotorsThrottleDeadband(void)
     return ifMotorstopFeatureEnabled() ? reversibleMotorsConfig()->neutral : directionValue;
 }
 
+const uint8_t sinTab[90] = {0, 4, 8, 13, 17, 22, 26, 31, 35, 40, 44, 48, 53,
+  57, 61, 66, 70, 74, 79, 83, 87, 91, 95, 100, 104, 108, 112, 116, 120, 124,
+  127, 131, 135, 139, 143, 146, 150, 154, 157, 161, 164, 167, 171, 174, 177,
+  181, 184, 187, 190, 193, 196, 198, 201, 204, 207, 209, 212, 214, 217, 219,
+  221, 223, 226, 228, 230, 232, 233, 235, 237, 238, 240, 242, 243, 244, 246,
+  247, 248, 249, 250, 251, 252, 252, 253, 254, 254, 255, 255, 255, 255, 255, };
+
+int32_t mulSinDegApprox(int32_t deg, int32_t v) {
+  if (deg >= 360) deg = deg % 360;
+  else if (deg < 0) deg = (deg % 360) + 360;
+  if (deg == 90) return v;
+  if (deg == 270) return -v;
+  int32_t w;
+  if (deg < 90)       w = +(int32_t)sinTab[deg];
+  else if (deg < 180) w = +(int32_t)sinTab[180 - deg];
+  else if (deg < 270) w = -(int32_t)sinTab[deg - 180];
+  else                w = -(int32_t)sinTab[360 - deg];
+  return v * w / 256;
+}
+
+int32_t mulCosDegApprox(int32_t deg, int32_t v) {
+  return mulSinDegApprox(deg + 90, v);
+}
+
 void FAST_CODE mixTable(void)
 {
 #ifdef USE_DSHOT
@@ -534,12 +565,27 @@ void FAST_CODE mixTable(void)
     int16_t rpyMixMax = 0; // assumption: symetrical about zero.
     int16_t rpyMixMin = 0;
 
+	int16_t angle = 0;
+    if (STATE(BOOMERANG)) {
+        gvSet(0, rotorRPM());
+        angle = rotorAngle();
+    }
+
     // motors for non-servo mixes
     for (int i = 0; i < motorCount; i++) {
-        rpyMix[i] =
-            (input[PITCH] * currentMixer[i].pitch +
-            input[ROLL] * currentMixer[i].roll +
-            -motorYawMultiplier * input[YAW] * currentMixer[i].yaw) * mixerScale;
+        if (STATE(BOOMERANG)) {
+            // Note: yaw always stabilized; pitch/roll always raw. 
+            rpyMix[i] = currentMixer[i].yaw * axisPID[YAW] +
+                        currentMixer[i].pitch * (
+                -mulCosDegApprox(angle + 360 * currentMixer[i].roll, rcCommand[PITCH]) +
+                mulSinDegApprox(angle + 360 * currentMixer[i].roll, rcCommand[ROLL]));
+        } 
+        else {
+			rpyMix[i] =
+				(input[PITCH] * currentMixer[i].pitch +
+				input[ROLL] * currentMixer[i].roll +
+				-motorYawMultiplier * input[YAW] * currentMixer[i].yaw) * mixerScale;
+		}
 
         if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i];
         if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i];
@@ -741,4 +787,4 @@ uint16_t getMaxThrottle(void) {
     }
 
     return throttle;
-}
+}

src/main/flight/mixer.h

@@ -40,7 +40,8 @@ typedef enum {
     PLATFORM_HELICOPTER     = 2,
     PLATFORM_TRICOPTER      = 3,
     PLATFORM_ROVER          = 4,
-    PLATFORM_BOAT           = 5
+    PLATFORM_BOAT           = 5,
+    PLATFORM_BOOMERANG      = 6
 } flyingPlatformType_e;
 
 

src/main/sensors/initialisation.c

@@ -39,6 +39,7 @@
 #include "sensors/sensors.h"
 #include "sensors/temperature.h"
 #include "sensors/temperature.h"
+#include "sensors/rotor.h"
 #include "rx/rx.h"
 
 uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE };
@@ -108,6 +109,10 @@ bool sensorsAutodetect(void)
     irlockInit();
 #endif
 
+#ifdef USE_ROTOR
+    rotorInit();
+#endif
+
     if (eepromUpdatePending) {
         suspendRxSignal();
         writeEEPROM();

src/main/sensors/rotor.c

@@ -0,0 +1,84 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "sensors/rotor.h"
+#include "stm32f4xx_exti.h"
+// drivers/exti.h needs this?
+#include <stdbool.h>
+#include "drivers/exti.h"
+#include "drivers/io.h"
+#include "drivers/time.h"
+#include "target.h"
+
+typedef struct rotor_s {
+  extiCallbackRec_t callback;
+  IO_t io;
+  timeDelta_t fall, rise, width, interval, bigtick;
+  int currentTick, circleTicks;
+  int rpm;
+} rotor_t;
+
+#define US_PER_MINUTE 60000000
+void rotorCallback(rotor_t *r) {
+  timeDelta_t t = micros();
+  if (!IORead(r->io)) {
+    r->rise = t;
+  } else {
+    timeDelta_t width = t - r->rise;
+    r->currentTick++;
+    // Was big tick?
+    if (width * 2 > r->width * 3) {
+      r->circleTicks = r->currentTick;
+      r->currentTick = 0;
+      r->rpm = US_PER_MINUTE / (t - r->bigtick);
+      r->bigtick = t;
+    }
+    r->width = width;
+    r->interval = t - r->fall;
+    r->fall = t;
+  }
+}
+
+static void rotorInitX(IO_t io, rotor_t* r) {
+  EXTIHandlerInit(&r->callback, (extiHandlerCallback*) rotorCallback); 
+  EXTIConfig(io, &r->callback, 1, EXTI_Trigger_Rising_Falling);
+  EXTIEnable(io, 1);
+  r->io = io;
+  r->circleTicks = 2;
+}
+
+static int rotorAngleX(rotor_t *r) {
+  timeDelta_t interval = micros() - r->fall;
+  if (interval > r->interval) {
+    // expected another tick by now.  assume it's exactly there.
+    return ((r->currentTick + 1) % r->circleTicks)
+           * 360 / r->circleTicks;
+  } else {
+    return r->currentTick * 360 / r->circleTicks +
+           interval * 360 / r->circleTicks / r->interval;
+  }
+}
+
+static int rotorRPMX(rotor_t* r) {
+  return r->rpm;
+}
+
+static rotor_t rotor;
+
+void rotorInit(void) { rotorInitX(IOGetByTag(IO_TAG(ROTOR_PIN)), &rotor); }
+int rotorRPM(void) { return rotorRPMX(&rotor); }
+int rotorAngle(void) { return rotorAngleX(&rotor); }

src/main/sensors/rotor.h

@@ -0,0 +1,22 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+void rotorInit(void);
+int rotorRPM(void);
+int rotorAngle(void);

src/main/target/FURYF4OSD/target.h

@@ -156,6 +156,9 @@
 #define USE_LED_STRIP
 #define WS2811_PIN                  PA0
 
+#define USE_ROTOR
+#define ROTOR_PIN PA0
+
 //#define USE_SPEKTRUM_BIND
 //#define BIND_PIN                PA3 //  RX2