diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md
index e6e878bc2b..91a1003693 100644
--- a/docs/Programming Framework.md
+++ b/docs/Programming Framework.md
@@ -74,9 +74,9 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` |
| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
| 40 | MOD | Divide `Operand A` by `Operand B` and returns the remainder |
+| 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. |
| 42 | SET_PROFILE | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change |
-
### Operands
| Operand Type | Name | Notes |
@@ -126,9 +126,10 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph |
| 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
-| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
-| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
-| 35 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
+| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
+| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
+| 35 | LOITER_RADIUS | The current loiter radius in cm. |
+| 36 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
#### ACTIVE_WAYPOINT_ACTION
diff --git a/docs/Settings.md b/docs/Settings.md
index 2e10ed8548..6ad0e17a50 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -788,17 +788,17 @@ Enable/disable dynamic gyro notch also known as Matrix Filter
| Default | Min | Max |
| --- | --- | --- |
-| OFF | | |
+| ON | | |
---
### dynamic_gyro_notch_min_hz
-Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70`
+Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirotors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70`
| Default | Min | Max |
| --- | --- | --- |
-| 150 | 30 | 1000 |
+| 50 | 30 | 1000 |
---
@@ -812,16 +812,6 @@ Q factor for dynamic notches
---
-### dynamic_gyro_notch_range
-
-Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers
-
-| Default | Min | Max |
-| --- | --- | --- |
-| MEDIUM | | |
-
----
-
### eleres_freq
_// TODO_
@@ -4092,6 +4082,16 @@ Value above which to make the OSD distance from home indicator blink (meters)
---
+### osd_esc_rpm_precision
+
+Number of characters used to display the RPM value.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 3 | 3 | 6 |
+
+---
+
### osd_esc_temp_alarm_max
Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade)
diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 0eb60439c2..47edcaeee4 100644
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -161,6 +161,8 @@ main_sources(COMMON_SRC
drivers/compass/compass_rm3100.h
drivers/compass/compass_vcm5883.c
drivers/compass/compass_vcm5883.h
+ drivers/compass/compass_mlx90393.c
+ drivers/compass/compass_mlx90393.h
drivers/compass/compass_msp.c
drivers/compass/compass_msp.h
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index ebdec54d68..68f22eadef 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1809,7 +1809,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_main_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_main_lpf_type);
#ifdef USE_DYNAMIC_FILTERS
- BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d", gyroConfig()->dynamicGyroNotchRange);
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz);
#endif
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index e031f4ef4a..bc44825251 100755
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -118,6 +118,7 @@ typedef enum {
DEVHW_LIS3MDL,
DEVHW_RM3100,
DEVHW_VCM5883,
+ DEVHW_MLX90393,
/* Temp sensor chips */
DEVHW_LM75_0,
diff --git a/src/main/drivers/compass/compass_mlx90393.c b/src/main/drivers/compass/compass_mlx90393.c
new file mode 100644
index 0000000000..c2b5e40114
--- /dev/null
+++ b/src/main/drivers/compass/compass_mlx90393.c
@@ -0,0 +1,176 @@
+/*
+ * 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 "platform.h"
+
+#ifdef USE_MAG_MLX90393
+
+#include
+#include
+
+#include
+
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "drivers/time.h"
+#include "drivers/io.h"
+#include "drivers/bus.h"
+
+#include "drivers/sensor.h"
+#include "drivers/compass/compass.h"
+
+#define MLX90393_MEASURE_3D 0b00001110
+
+#define MLX90393_NOP 0b00000000
+#define MLX90393_START_BURST_MODE 0b00010000 //uses with zyxt flags
+#define MLX90393_START_WAKE_UP_ON_CHANGE_MODE 0b00100000 //uses with zyxt flags
+#define MLX90393_START_SINGLE_MEASUREMENT_MODE 0b00110000 //uses with zyxt flags
+#define MLX90393_READ_MEASUREMENT 0b01000000 //uses with zyxt flags
+#define MLX90393_READ_REGISTER 0b01010000
+#define MLX90393_WRITE_REGISTER 0b01100000
+#define MLX90393_EXIT_MODE 0b10000000
+#define MLX90393_MEMORY_RECALL 0b11010000
+#define MLX90393_MEMORY_STORE 0b11100000
+#define MLX90393_RESET 0b11110000
+
+#define MLX90393_BURST_MODE_FLAG 0b10000000
+#define MLX90393_WAKE_UP_ON_CHANGE_MODE_FLAG 0b01000000
+#define MLX90393_SINGLE_MEASUREMENT_MODE_FLAG 0b00100000
+#define MLX90393_ERROR_FLAG 0b00010000
+#define MLX90393_SINGLE_ERROR_DETECTION_FLAG 0b00001000
+#define MLX90393_RESET_FLAG 0b00000100
+#define MLX90393_D1_FLAG 0b00000010
+#define MLX90393_D0_FLAG 0b00000001
+
+#define DETECTION_MAX_RETRY_COUNT 5
+
+#define REG_BUF_LEN 3
+
+// Register 1
+#define GAIN_SEL_HALLCONF_REG 0x0 //from datasheet 0x0 << 2 = 0x0
+// GAIN - 0b111
+// Hall_conf - 0xC
+#define GAIN_SEL_HALLCONF_REG_VALUE 0x007C
+// Register 2
+#define TCMP_EN_REG 0x4 //from datasheet 0x1 << 2 = 0x4
+// Burst Data Rate 0b000100
+#define TCMP_EN_REG_VALUE 0x62C4
+// Register 3
+#define RES_XYZ_OSR_FLT_REG 0x8 //from datasheet 0x2 << 2 = 0x8
+// Oversampling 0b01
+// Filtering 0b111
+#define RES_XYZ_OSR_FLT_REG_VALUE 0x001D
+
+
+static void mlx90393WriteRegister(magDev_t * mag, uint8_t reg_val, uint16_t value) {
+
+ uint8_t buf[REG_BUF_LEN] = {0};
+
+ buf[0] = (value >> 8) & 0xFF;
+ buf[1] = (value >> 0) & 0xFF;
+ buf[2] = reg_val;
+
+ busWriteBuf(mag->busDev, MLX90393_WRITE_REGISTER, buf, REG_BUF_LEN);
+
+ // PAUSE
+ delay(20);
+ // To skip ACK FLAG of Write
+ uint8_t sig = 0;
+ busRead(mag->busDev, MLX90393_NOP, &sig);
+
+ return;
+}
+
+// =======================================================================================
+static bool mlx90393Read(magDev_t * mag)
+{
+
+ uint8_t buf[7] = {0};
+
+ busReadBuf(mag->busDev, MLX90393_READ_MEASUREMENT | MLX90393_MEASURE_3D, buf, 7);
+
+ mag->magADCRaw[X] = ((short)(buf[1] << 8 | buf[2]));
+ mag->magADCRaw[Y] = ((short)(buf[3] << 8 | buf[4]));
+ mag->magADCRaw[Z] = ((short)(buf[5] << 8 | buf[6]));
+
+ return true;
+
+}
+
+static bool deviceDetect(magDev_t * mag)
+{
+ for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
+ uint8_t sig = 0;
+
+ bool ack = busRead(mag->busDev, MLX90393_RESET, &sig);
+ delay(20);
+
+ if (ack && ((sig & MLX90393_RESET_FLAG) == MLX90393_RESET_FLAG)) { // Check Reset Flag -> MLX90393
+ return true;
+ }
+ }
+ return false;
+}
+
+//--------------------------------------------------
+static bool mlx90393Init(magDev_t * mag)
+{
+ uint8_t sig = 0;
+
+ delay(20);
+ // Remove reset flag
+ busRead(mag->busDev, MLX90393_NOP, &sig);
+ // Exit if already in burst mode. (For example when external i2c source power the bus.)
+ busRead(mag->busDev, MLX90393_EXIT_MODE, &sig);
+
+ // Writing Registers
+ mlx90393WriteRegister(mag, GAIN_SEL_HALLCONF_REG, GAIN_SEL_HALLCONF_REG_VALUE);
+ mlx90393WriteRegister(mag, TCMP_EN_REG, TCMP_EN_REG_VALUE);
+ mlx90393WriteRegister(mag, RES_XYZ_OSR_FLT_REG, RES_XYZ_OSR_FLT_REG_VALUE);
+
+ // Start burst mode
+ busRead(mag->busDev, MLX90393_START_BURST_MODE | MLX90393_MEASURE_3D, &sig);
+
+ return true;
+}
+
+// ==========================================================================
+
+bool mlx90393Detect(magDev_t * mag)
+{
+ mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MLX90393, mag->magSensorToUse, OWNER_COMPASS);
+ if (mag->busDev == NULL) {
+ return false;
+ }
+
+ if (!deviceDetect(mag)) {
+ busDeviceDeInit(mag->busDev);
+ return false;
+ }
+
+ mag->init = mlx90393Init;
+ mag->read = mlx90393Read;
+
+ return true;
+}
+
+
+#endif
diff --git a/src/main/drivers/compass/compass_mlx90393.h b/src/main/drivers/compass/compass_mlx90393.h
new file mode 100644
index 0000000000..5d73607294
--- /dev/null
+++ b/src/main/drivers/compass/compass_mlx90393.h
@@ -0,0 +1,20 @@
+/*
+ * 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
+
+bool mlx90393Detect(magDev_t* mag);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 876bd16648..67ab20d579 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -13,7 +13,7 @@ tables:
values: ["NONE", "BNO055", "BNO055_SERIAL"]
enum: secondaryImuType_e
- name: mag_hardware
- values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "FAKE"]
+ values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]
enum: magSensor_e
- name: opflow_hardware
values: ["NONE", "CXOF", "MSP", "FAKE"]
@@ -144,9 +144,6 @@ tables:
- name: rssi_source
values: ["NONE", "AUTO", "ADC", "CHANNEL", "PROTOCOL", "MSP"]
enum: rssiSource_e
- - name: dynamicFilterRangeTable
- values: ["HIGH", "MEDIUM", "LOW"]
- enum: dynamicFilterRange_e
- name: pidTypeTable
values: ["NONE", "PID", "PIFF", "AUTO"]
enum: pidType_e
@@ -280,16 +277,10 @@ groups:
max: 10
- name: dynamic_gyro_notch_enabled
description: "Enable/disable dynamic gyro notch also known as Matrix Filter"
- default_value: OFF
+ default_value: ON
field: dynamicGyroNotchEnabled
condition: USE_DYNAMIC_FILTERS
type: bool
- - name: dynamic_gyro_notch_range
- description: "Range for dynamic gyro notches. `MEDIUM` for 5\", `HIGH` for 3\" and `MEDIUM`/`LOW` for 7\" and bigger propellers"
- default_value: "MEDIUM"
- field: dynamicGyroNotchRange
- condition: USE_DYNAMIC_FILTERS
- table: dynamicFilterRangeTable
- name: dynamic_gyro_notch_q
description: "Q factor for dynamic notches"
default_value: 120
@@ -298,8 +289,8 @@ groups:
min: 1
max: 1000
- name: dynamic_gyro_notch_min_hz
- description: "Minimum frequency for dynamic notches. Default value of `150` works best with 5\" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7\" drones. 10\" can go down to `60` - `70`"
- default_value: 150
+ description: "Minimum frequency for dynamic notches. Default value of `150` works best with 5\" multirotors. Should be lowered with increased size of propellers. Values around `100` work fine on 7\" drones. 10\" can go down to `60` - `70`"
+ default_value: 50
field: dynamicGyroNotchMinHz
condition: USE_DYNAMIC_FILTERS
min: 30
@@ -3319,7 +3310,6 @@ groups:
description: "How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2)"
default_value: 0
field: hud_wp_disp
-
min: 0
max: 3
- name: osd_left_sidebar_scroll
@@ -3455,6 +3445,13 @@ groups:
min: -36
max: 36
+ - name: osd_esc_rpm_precision
+ description: Number of characters used to display the RPM value.
+ field: esc_rpm_precision
+ min: 3
+ max: 6
+ default_value: 3
+
- name: PG_OSD_COMMON_CONFIG
type: osdCommonConfig_t
headers: ["io/osd_common.h"]
diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c
index b85d538a15..7cffd9c0b6 100644
--- a/src/main/flight/gyroanalyse.c
+++ b/src/main/flight/gyroanalyse.c
@@ -60,25 +60,11 @@ FILE_COMPILE_FOR_SPEED
void gyroDataAnalyseStateInit(
gyroAnalyseState_t *state,
uint16_t minFrequency,
- uint8_t range,
uint32_t targetLooptimeUs
) {
- state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
state->minFrequency = minFrequency;
- if (range == DYN_NOTCH_RANGE_HIGH) {
- state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
- }
- else if (range == DYN_NOTCH_RANGE_MEDIUM) {
- state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
- }
-
- // If we get at least 3 samples then use the default FFT sample frequency
- // otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K)
- const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
-
- state->fftSamplingRateHz = MIN((gyroLoopRateHz / 3), state->fftSamplingRateHz);
-
+ state->fftSamplingRateHz = lrintf(1e6f / targetLooptimeUs / 3); // Looptime divided by 3
state->fftResolution = (float)state->fftSamplingRateHz / FFT_WINDOW_SIZE;
state->fftStartBin = state->minFrequency / lrintf(state->fftResolution);
diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h
index 54b5c37eff..a3f3d7f8a7 100644
--- a/src/main/flight/gyroanalyse.h
+++ b/src/main/flight/gyroanalyse.h
@@ -68,7 +68,6 @@ STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlyi
void gyroDataAnalyseStateInit(
gyroAnalyseState_t *state,
uint16_t minFrequency,
- uint8_t range,
uint32_t targetLooptimeUs
);
void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index a5bfdf53ed..0ed029705e 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1037,17 +1037,37 @@ static void osdFormatRpm(char *buff, uint32_t rpm)
{
buff[0] = SYM_RPM;
if (rpm) {
- if (rpm >= 1000) {
- osdFormatCentiNumber(buff + 1, rpm / 10, 0, 1, 1, 2);
- buff[3] = 'K';
- buff[4] = '\0';
+ if ( digitCount(rpm) > osdConfig()->esc_rpm_precision) {
+ uint8_t rpmMaxDecimals = (osdConfig()->esc_rpm_precision - 3);
+ osdFormatCentiNumber(buff + 1, rpm / 10, 0, rpmMaxDecimals, rpmMaxDecimals, osdConfig()->esc_rpm_precision-1);
+ buff[osdConfig()->esc_rpm_precision] = 'K';
+ buff[osdConfig()->esc_rpm_precision+1] = '\0';
}
else {
- tfp_sprintf(buff + 1, "%3lu", rpm);
+ switch(osdConfig()->esc_rpm_precision) {
+ case 6:
+ tfp_sprintf(buff + 1, "%6lu", rpm);
+ break;
+ case 5:
+ tfp_sprintf(buff + 1, "%5lu", rpm);
+ break;
+ case 4:
+ tfp_sprintf(buff + 1, "%4lu", rpm);
+ break;
+ case 3:
+ default:
+ tfp_sprintf(buff + 1, "%3lu", rpm);
+ break;
+ }
+
+
}
}
else {
- strcpy(buff + 1, "---");
+ uint8_t buffPos = 1;
+ while (buffPos <=( osdConfig()->esc_rpm_precision)) {
+ strcpy(buff + buffPos++, "-");
+ }
}
}
#endif
@@ -3153,6 +3173,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
.osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT,
.pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT,
.pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT,
+ .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT,
.units = SETTING_OSD_UNITS_DEFAULT,
.main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT,
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 2c9e27d6b4..5e150858cb 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -385,6 +385,7 @@ typedef struct osdConfig_s {
uint8_t crsf_lq_format;
uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows
uint8_t telemetry; // use telemetry on displayed pixel line 0
+ uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers.
} osdConfig_t;
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 9845bde147..e711877c60 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -49,6 +49,8 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
+#include "programming/logic_condition.h"
+
#include "rx/rx.h"
#include "sensors/battery.h"
@@ -271,10 +273,12 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// Limit minimum forward velocity to 1 m/s
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
+ uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius);
+
// If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
#define TAN_15DEG 0.26795f
bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
- && (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
+ && (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG))
&& (distanceToActualTarget > 50.0f)
&& !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
@@ -283,8 +287,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// We are closing in on a waypoint, calculate circular loiter
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f);
- float loiterTargetX = posControl.desiredState.pos.x + navConfig()->fw.loiter_radius * cos_approx(loiterAngle);
- float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle);
+ float loiterTargetX = posControl.desiredState.pos.x + navLoiterRadius * cos_approx(loiterAngle);
+ float loiterTargetY = posControl.desiredState.pos.y + navLoiterRadius * sin_approx(loiterAngle);
// We have temporary loiter target. Recalculate distance and position error
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c
index 36777fc4a9..9ad94591e8 100644
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@@ -83,10 +83,10 @@ void pgResetFn_logicConditions(logicCondition_t *instance)
logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
static int logicConditionCompute(
- int currentVaue,
+ int32_t currentVaue,
logicOperation_e operation,
- int operandA,
- int operandB
+ int32_t operandA,
+ int32_t operandB
) {
int temporaryValue;
vtxDeviceCapability_t vtxDeviceCapability;
@@ -177,20 +177,20 @@ static int logicConditionCompute(
break;
case LOGIC_CONDITION_ADD:
- return constrain(operandA + operandB, INT16_MIN, INT16_MAX);
+ return constrain(operandA + operandB, INT32_MIN, INT32_MAX);
break;
case LOGIC_CONDITION_SUB:
- return constrain(operandA - operandB, INT16_MIN, INT16_MAX);
+ return constrain(operandA - operandB, INT32_MIN, INT32_MAX);
break;
case LOGIC_CONDITION_MUL:
- return constrain(operandA * operandB, INT16_MIN, INT16_MAX);
+ return constrain(operandA * operandB, INT32_MIN, INT32_MAX);
break;
case LOGIC_CONDITION_DIV:
if (operandB != 0) {
- return constrain(operandA / operandB, INT16_MIN, INT16_MAX);
+ return constrain(operandA / operandB, INT32_MIN, INT32_MAX);
} else {
return operandA;
}
@@ -328,7 +328,7 @@ static int logicConditionCompute(
case LOGIC_CONDITION_MODULUS:
if (operandB != 0) {
- return constrain(operandA % operandB, INT16_MIN, INT16_MAX);
+ return constrain(operandA % operandB, INT32_MIN, INT32_MAX);
} else {
return operandA;
}
@@ -350,6 +350,12 @@ static int logicConditionCompute(
}
break;
+ case LOGIC_CONDITION_LOITER_OVERRIDE:
+ logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE] = constrain(operandA, 0, 100000);
+ LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS);
+ return true;
+ break;
+
default:
return false;
break;
@@ -554,6 +560,9 @@ static int logicConditionGetFlightOperandValue(int operand) {
return getConfigProfile() + 1;
break;
+ case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS:
+ return getLoiterRadius(navConfig()->fw.loiter_radius);
+
default:
return 0;
break;
@@ -741,3 +750,16 @@ int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) {
return originalValue;
}
}
+
+uint32_t getLoiterRadius(uint32_t loiterRadius) {
+#ifdef USE_PROGRAMMING_FRAMEWORK
+ if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) &&
+ !(FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding())) {
+ return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000);
+ } else {
+ return loiterRadius;
+ }
+#else
+ return loiterRadius;
+#endif
+}
diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h
index 8b64ab6d7f..f20faa8bba 100644
--- a/src/main/programming/logic_condition.h
+++ b/src/main/programming/logic_condition.h
@@ -70,6 +70,7 @@ typedef enum {
LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 38,
LOGIC_CONDITION_SET_HEADING_TARGET = 39,
LOGIC_CONDITION_MODULUS = 40,
+ LOGIC_CONDITION_LOITER_OVERRIDE = 41,
LOGIC_CONDITION_SET_PROFILE = 42,
LOGIC_CONDITION_LAST = 43,
} logicOperation_e;
@@ -121,7 +122,8 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 32
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 33
LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 34
- LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 35
+ LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS // 35
+ LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 36
} logicFlightOperands_e;
@@ -150,6 +152,7 @@ typedef enum {
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE = (1 << 6),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT = (1 << 7),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8),
+ LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9),
} logicConditionsGlobalFlags_t;
typedef enum {
@@ -200,3 +203,4 @@ void logicConditionReset(void);
float getThrottleScale(float globalThrottleScale);
int16_t getRcCommandOverride(int16_t command[], uint8_t axis);
int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue);
+uint32_t getLoiterRadius(uint32_t loiterRadius);
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index a6ca548dd1..150d716c51 100644
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -42,6 +42,7 @@
#include "drivers/compass/compass_lis3mdl.h"
#include "drivers/compass/compass_rm3100.h"
#include "drivers/compass/compass_vcm5883.h"
+#include "drivers/compass/compass_mlx90393.h"
#include "drivers/compass/compass_msp.h"
#include "drivers/io.h"
#include "drivers/light_led.h"
@@ -260,6 +261,19 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
}
FALLTHROUGH;
+ case MAG_MLX90393:
+#ifdef USE_MAG_MLX90393
+ if (mlx90393Detect(dev)) {
+ magHardware = MAG_MLX90393;
+ break;
+ }
+#endif
+ /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+ if (magHardwareToUse != MAG_AUTODETECT) {
+ break;
+ }
+ FALLTHROUGH;
+
case MAG_FAKE:
#ifdef USE_FAKE_MAG
if (fakeMagDetect(dev)) {
diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h
index 389e516847..9f798f28c5 100644
--- a/src/main/sensors/compass.h
+++ b/src/main/sensors/compass.h
@@ -43,7 +43,8 @@ typedef enum {
MAG_MSP = 12,
MAG_RM3100 = 13,
MAG_VCM5883 = 14,
- MAG_FAKE = 15,
+ MAG_MLX90393 = 15,
+ MAG_FAKE = 16,
MAG_MAX = MAG_FAKE
} magSensor_e;
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 76ae9f60d9..4ea232a977 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -124,7 +124,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyroDynamicLpfMaxHz = SETTING_GYRO_DYN_LPF_MAX_HZ_DEFAULT,
.gyroDynamicLpfCurveExpo = SETTING_GYRO_DYN_LPF_CURVE_EXPO_DEFAULT,
#ifdef USE_DYNAMIC_FILTERS
- .dynamicGyroNotchRange = SETTING_DYNAMIC_GYRO_NOTCH_RANGE_DEFAULT,
.dynamicGyroNotchQ = SETTING_DYNAMIC_GYRO_NOTCH_Q_DEFAULT,
.dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT,
.dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT,
@@ -355,7 +354,6 @@ bool gyroInit(void)
gyroDataAnalyseStateInit(
&gyroAnalyseState,
gyroConfig()->dynamicGyroNotchMinHz,
- gyroConfig()->dynamicGyroNotchRange,
getLooptime()
);
#endif
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 4f14071ad2..bacce4b458 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -41,16 +41,6 @@ typedef enum {
GYRO_FAKE
} gyroSensor_e;
-typedef enum {
- DYN_NOTCH_RANGE_HIGH = 0,
- DYN_NOTCH_RANGE_MEDIUM,
- DYN_NOTCH_RANGE_LOW
-} dynamicFilterRange_e;
-
-#define DYN_NOTCH_RANGE_HZ_HIGH 2000
-#define DYN_NOTCH_RANGE_HZ_MEDIUM 1333
-#define DYN_NOTCH_RANGE_HZ_LOW 1000
-
typedef struct gyro_s {
bool initialized;
uint32_t targetLooptime;
@@ -78,7 +68,6 @@ typedef struct gyroConfig_s {
uint16_t gyroDynamicLpfMaxHz;
uint8_t gyroDynamicLpfCurveExpo;
#ifdef USE_DYNAMIC_FILTERS
- uint8_t dynamicGyroNotchRange;
uint16_t dynamicGyroNotchQ;
uint16_t dynamicGyroNotchMinHz;
uint8_t dynamicGyroNotchEnabled;
diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h
index 97764f6e92..1beb8db7ba 100644
--- a/src/main/target/KAKUTEF7/target.h
+++ b/src/main/target/KAKUTEF7/target.h
@@ -144,6 +144,7 @@
#define USE_MAG_IST8310
#define USE_MAG_IST8308
#define USE_MAG_LIS3MDL
+#define USE_MAG_MLX90393
#define TEMPERATURE_I2C_BUS BUS_I2C1
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index a02d8b683d..072473e596 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -260,6 +260,13 @@
BUSDEV_REGISTER_I2C(busdev_vcm5883, DEVHW_VCM5883, VCM5883_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0);
#endif
+#if defined(USE_MAG_MLX90393)
+ #if !defined(MLX90393_I2C_BUS)
+ #define MLX90393_I2C_BUS MAG_I2C_BUS
+ #endif
+ BUSDEV_REGISTER_I2C(busdev_mlx90393, DEVHW_MLX90393, MLX90393_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0);
+#endif
+
#endif
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 439ca8f2d0..079abf95d2 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -332,9 +332,9 @@ static void crsfFrameFlightMode(sbuf_t *dst)
} else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) {
flightMode = "HOLD";
} else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
- flightMode = "3CRS";
+ flightMode = "CRUZ";
} else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
- flightMode = "CRS";
+ flightMode = "CRSH";
} else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
flightMode = "AH";
} else if (FLIGHT_MODE(NAV_WP_MODE)) {