From b411c7cb259260464cbdb82e8ef91b984bcae337 Mon Sep 17 00:00:00 2001 From: chris Date: Tue, 15 Jul 2025 21:20:39 -0400 Subject: [PATCH 1/3] boomerang code --- src/main/CMakeLists.txt | 2 + src/main/fc/runtime_config.h | 1 + src/main/fc/settings.yaml | 2 +- src/main/flight/mixer.c | 58 ++++++++++++++++++--- src/main/flight/mixer.h | 3 +- src/main/sensors/initialisation.c | 5 ++ src/main/sensors/rotor.c | 84 ++++++++++++++++++++++++++++++ src/main/sensors/rotor.h | 22 ++++++++ src/main/target/FURYF4OSD/target.h | 3 ++ 9 files changed, 172 insertions(+), 8 deletions(-) create mode 100644 src/main/sensors/rotor.c create mode 100644 src/main/sensors/rotor.h 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..1859ca66135 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; + for (int16_t i = 0; i < motorCount; i++) { + 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 From ce79a36d135f796831db7cb69df95d412c1aee6f Mon Sep 17 00:00:00 2001 From: chris Date: Tue, 15 Jul 2025 21:29:19 -0400 Subject: [PATCH 2/3] fix int --- src/main/flight/mixer.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 1859ca66135..436d4b3640d 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -572,13 +572,13 @@ void FAST_CODE mixTable(void) } // motors for non-servo mixes - for (int16_t i = 0; i < motorCount; i++) { - 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])); + for (int i = 0; i < motorCount; i++) { + 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] = From 12bfde32a9dad57618b1b582e34bb848842dac0c Mon Sep 17 00:00:00 2001 From: chris Date: Tue, 15 Jul 2025 21:31:21 -0400 Subject: [PATCH 3/3] tabs --- src/main/flight/mixer.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 436d4b3640d..0d31a899a1b 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -565,7 +565,7 @@ void FAST_CODE mixTable(void) int16_t rpyMixMax = 0; // assumption: symetrical about zero. int16_t rpyMixMin = 0; - int16_t angle = 0; + int16_t angle = 0; if (STATE(BOOMERANG)) { gvSet(0, rotorRPM()); angle = rotorAngle(); @@ -581,11 +581,11 @@ void FAST_CODE mixTable(void) 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; - } + 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];