diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index f1966c45f60..e38dfcb03a9 100755
--- a/src/main/CMakeLists.txt
+++ b/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
diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
index 452256a6b64..9c1ec70d5be 100644
--- a/src/main/fc/runtime_config.h
+++ b/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))
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 811269afa92..532dda12a58 100644
--- a/src/main/fc/settings.yaml
+++ b/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
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 716b48d7aa4..0d31a899a1b 100644
--- a/src/main/flight/mixer.c
+++ b/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;
-}
\ No newline at end of file
+}
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 12688bd2c09..29c9ad732f1 100644
--- a/src/main/flight/mixer.h
+++ b/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;
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 047c9e80b15..619b71c9fef 100755
--- a/src/main/sensors/initialisation.c
+++ b/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();
diff --git a/src/main/sensors/rotor.c b/src/main/sensors/rotor.c
new file mode 100644
index 00000000000..a06c03dde8a
--- /dev/null
+++ b/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 .
+ */
+
+#include "sensors/rotor.h"
+#include "stm32f4xx_exti.h"
+// drivers/exti.h needs this?
+#include
+#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); }
diff --git a/src/main/sensors/rotor.h b/src/main/sensors/rotor.h
new file mode 100644
index 00000000000..ca09394e387
--- /dev/null
+++ b/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 .
+ */
+
+#pragma once
+
+void rotorInit(void);
+int rotorRPM(void);
+int rotorAngle(void);
diff --git a/src/main/target/FURYF4OSD/target.h b/src/main/target/FURYF4OSD/target.h
index 71abc9a8c68..d6d5f3a38c3 100644
--- a/src/main/target/FURYF4OSD/target.h
+++ b/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