Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-15 20:35:29 +03:00
Code Issues Wiki Activity

Merge branch 'master' into abo_waypoint_tracking

Browse source
This commit is contained in:
breadoven 2022-08-12 13:02:36 +01:00 • committed by GitHub
commit 67adc42e75
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
61 changed files with 922 additions and 644 deletions
Download patch file Download diff file Expand all files Collapse all files

6
docs/Profiles.md
View file

@ -36,6 +36,12 @@ Or using the speed to change profiles. In this example:
[![Using speed to change profiles](https://i.imgur.com/WjkuhhWl.png)](https://i.imgur.com/WjkuhhW.png)
#### Configurator use with profile changing logic.
If you have logic conditions that change the profiles. You may find that if you manually change the profile using the drop down boxes in the top right of Configurator; that they switch back to a different profile. This is because the logic conditions are still running in the background. If this is the case, the simplest solutuion is to temporarily disable the switches that trigger the `set profile` operations. Remember to re-enable these switches after you have made your changes.
[![Disabled SET PROFILE switches](https://i.imgur.com/AeH9ll7l.png)](https://i.imgur.com/AeH9ll7.png)
## Profile Contents
The values contained within a profile can be seen by using the CLI `dump profile` command.

2
docs/Rx.md
View file

@ -9,6 +9,8 @@ There are 2 basic types of receivers:
## PPM Receivers
**Only supported in inav 3.x and below**
PPM is sometimes known as PPM SUM or CPPM.
12 channels via a single input pin, not as accurate or jitter free as methods that use serial communications, but readily available.

74
docs/Settings.md
View file

@ -362,16 +362,6 @@ Selection of baro hardware. See Wiki Sensor auto detect and hardware failure det
---
### baro_median_filter
3-point median filtering for barometer readouts. No reason to change this setting
| Default | Min | Max |
| --- | --- | --- |
| ON | OFF | ON |
---
### bat_cells
Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected.
@ -762,6 +752,16 @@ Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`,
---
### dynamic_gyro_notch_3d_q
Q factor for 3D dynamic notches
| Default | Min | Max |
| --- | --- | --- |
| 200 | 1 | 1000 |
---
### dynamic_gyro_notch_enabled
Enable/disable dynamic gyro notch also known as Matrix Filter
@ -782,6 +782,16 @@ Minimum frequency for dynamic notches. Default value of `150` works best with 5"
---
### dynamic_gyro_notch_mode
Gyro dynamic notch type
| Default | Min | Max |
| --- | --- | --- |
| 2D | | |
---
### dynamic_gyro_notch_q
Q factor for dynamic notches
@ -2952,6 +2962,16 @@ Dive angle that airplane will use during final landing phase. During dive phase,
---
### nav_fw_launch_abort_deadband
Launch abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch.
| Default | Min | Max |
| --- | --- | --- |
| 100 | 2 | 250 |
---
### nav_fw_launch_accel
Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s
@ -3012,6 +3032,16 @@ Launch idle throttle - throttle to be set before launch sequence is initiated. I
---
### nav_fw_launch_manual_throttle
Allows launch with manually controlled throttle. INAV only levels wings and controls climb pitch during launch. Throttle is controlled directly by throttle stick movement. IF USED WITHOUT A GPS LOCK plane must be launched immediately after throttle is increased to avoid issues with climb out stabilisation and the launch ending sooner than expected (launch end timer starts as soon as the throttle stick is raised).
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### nav_fw_launch_max_altitude
Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000].
@ -5052,13 +5082,33 @@ Exposition value used for the PITCH/ROLL axes by all the stabilized flights mode
---
### rc_filter_frequency
### rc_filter_auto
When enabled, INAV will set RC filtering based on refresh rate and smoothing factor.
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### rc_filter_lpf_hz
RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values
| Default | Min | Max |
| --- | --- | --- |
| 50 | 0 | 100 |
| 50 | 15 | 250 |
---
### rc_filter_smoothing_factor
The RC filter smoothing factor. The higher the value, the more smoothing but also the more delay in response. Value 1 sets the filter at half the refresh rate. Value 100 sets the filter to aprox. 10% of the RC refresh rate
| Default | Min | Max |
| --- | --- | --- |
| 30 | 1 | 100 |
---

2
docs/development/Building in Linux.md
View file

@ -148,7 +148,7 @@ It is unlikely that the typical user will need to employ these options, other th
* Configure `cmake` to use `ninja` as the build system
```
cd buid
cd build
# add other cmake options as required.
cmake -GNinja ..
```

33
docs/development/Building in Windows 10 or 11 with MSYS2.md
View file

@ -1,6 +1,6 @@
# General Info
This is a guide on how to use Windows MSYS2 distribution and building platform to build iNav firmware. This environment is very simple to manage and does not require installing docker for Windows which may get in the way of VMWare or any other virtualization software you already have running for other reasons. Another benefit of this approach is that the compiler runs natively on Windows, so performance is much better than compiling in a virtual environment or a container. You can also integrate with whatever IDE you are using to make code edits and work with github, which makes the entire development and testing workflow a lot more efficient. In addition to MSYS2, this build environment also uses Arm Embedded GCC tolkit from The xPack Project, which provides many benefits over the toolkits maintained by arm.com
This is a guide on how to use Windows MSYS2 distribution and building platform to build INAV firmware. This environment is very simple to manage and does not require installing docker for Windows which may get in the way of VMWare or any other virtualization software you already have running for other reasons. Another benefit of this approach is that the compiler runs natively on Windows, so performance is much better than compiling in a virtual environment or a container. You can also integrate with whatever IDE you are using to make code edits and work with github, which makes the entire development and testing workflow a lot more efficient. In addition to MSYS2, this build environment also uses Arm Embedded GCC tolkit from The xPack Project, which provides many benefits over the toolkits maintained by arm.com
Some of those benefits are described here:
@ -38,11 +38,18 @@ cd /c/Workspace
# you can also check out your own fork here which makes contributing easier
git clone https://github.com/iNavFlight/inav
cd inav
# switch to release you want or skip next 2 lines if you want latest
git fetch origin
git checkout -b release_2.6.1 origin/release_2.6.1
```
Now create the build and xpack directories and get the toolkit version you need for your inav version
(Optional) Switch to a release instead of master
```
git fetch origin
# on the next line, tags/5.0.0 is the release's tag, and local_5.0.0 is the name of a local branch you will create.
# tags can be found on https://github.com/iNavFlight/inav/tags as well as the releases page
git checkout tags/5.0.0 -b local_5.0.0
# you can also checkout with a branch if applicable:
# git checkout -b release_5.1.0 origin/release_5.1.0
```
Now create the build and xpack directories and get the toolkit version you need for your INAV version
```
mkdir build
cd build
@ -54,13 +61,13 @@ This will give you the version you need for any given release or master branch.
https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/
```
# for version 2.6.1, version needed is 9.2.1
wget https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/download/v9.2.1-1.1/xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip
unzip xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip
# for INAV version 5.0.0, toolchain version needed is 10.2.1
wget https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/download/v10.2.1-1.1/xpack-arm-none-eabi-gcc-10.2.1-1.1-win32-x64.zip
unzip xpack-arm-none-eabi-gcc-10.2.1-1.1-win32-x64.zip
```
This is important, put the toolkit first before your path so that it is picked up ahead of any other versions that may be present on your system
```
export PATH=/c/Workspace/xpack/xpack-arm-none-eabi-gcc-9.2.1-1.1/bin:$PATH
export PATH=/c/Workspace/xpack/xpack-arm-none-eabi-gcc-10.2.1-1.1/bin:$PATH
cd /c/Workspace/inav/build
```
You may need to run rm -rf * in build directory if you had any failed previous runs now run cmake
@ -68,13 +75,13 @@ You may need to run rm -rf * in build directory if you had any failed previous r
# while inside the build directory
cmake ..
```
Once that's done you can compile the firmware for your controller
Once that's done you can compile the firmware for your flight controller
```
make DALRCF405
```
To get a list of available targets in iNav, see the target src folder
https://github.com/tednv/inav/tree/master/src/main/target
To get a list of available targets in INAV, see the target src folder
[https://github.com/tednv/inav/tree/master/src/main/target](https://github.com/inavflight/inav/tree/master/src/main/target)
The generated hex file will be in /c/Workspace/inav/build folder
At the time of writting this document, I believe this is the fastest, easiest, and most efficient Windows build environment that is available. I have used this approach several years ago and was very happy with it building iNav 2.1 and 2.2, and now I'm getting back into it so figured I would share my method
At the time of writting this document, I believe this is the fastest, easiest, and most efficient Windows build environment that is available. I have used this approach several years ago and was very happy with it building INAV 2.1 and 2.2, and now I'm getting back into it so figured I would share my method

4
src/main/CMakeLists.txt
View file

@ -88,8 +88,6 @@ main_sources(COMMON_SRC
drivers/accgyro/accgyro_mpu.h
drivers/accgyro/accgyro_mpu6000.c
drivers/accgyro/accgyro_mpu6000.h
drivers/accgyro/accgyro_mpu6050.c
drivers/accgyro/accgyro_mpu6050.h
drivers/accgyro/accgyro_mpu6500.c
drivers/accgyro/accgyro_mpu6500.h
drivers/accgyro/accgyro_mpu9250.c
@ -333,6 +331,8 @@ main_sources(COMMON_SRC
flight/rpm_filter.h
flight/dynamic_gyro_notch.c
flight/dynamic_gyro_notch.h
flight/secondary_dynamic_gyro_notch.c
flight/secondary_dynamic_gyro_notch.h
flight/dynamic_lpf.c
flight/dynamic_lpf.h
flight/secondary_imu.c

7
src/main/blackbox/blackbox.c
View file

@ -54,6 +54,7 @@
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "fc/settings.h"
#include "fc/rc_smoothing.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
@ -433,6 +434,7 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
{"escRPM", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"escTemperature", -1, SIGNED, PREDICT(PREVIOUS), ENCODING(SIGNED_VB)},
#endif
{"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB)},
};
typedef enum BlackboxState {
@ -550,6 +552,7 @@ typedef struct blackboxSlowState_s {
uint32_t escRPM;
int8_t escTemperature;
#endif
uint16_t rxUpdateRate;
} __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
//From rc_controls.c
@ -1284,6 +1287,8 @@ static void writeSlowFrame(void)
blackboxWriteUnsignedVB(slowHistory.escRPM);
blackboxWriteSignedVB(slowHistory.escTemperature);
#endif
blackboxWriteUnsignedVB(slowHistory.rxUpdateRate);
blackboxSlowFrameIterationTimer = 0;
}
@ -1341,6 +1346,8 @@ static void loadSlowState(blackboxSlowState_t *slow)
slow->escRPM = escSensor->rpm;
slow->escTemperature = escSensor->temperature;
#endif
slow->rxUpdateRate = getRcUpdateFrequency();
}
/**

1
src/main/cms/cms_menu_navigation.c
View file

@ -149,6 +149,7 @@ static const OSD_Entry cmsx_menuFWLaunchEntries[] =
OSD_SETTING_ENTRY("VELOCITY", SETTING_NAV_FW_LAUNCH_VELOCITY),
OSD_SETTING_ENTRY("ACCELERATION", SETTING_NAV_FW_LAUNCH_ACCEL),
OSD_SETTING_ENTRY("DETECT TIME", SETTING_NAV_FW_LAUNCH_DETECT_TIME),
OSD_SETTING_ENTRY("MANUAL THROTTLE", SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE),
OSD_BACK_AND_END_ENTRY,
};

1
src/main/common/constants.h
View file

@ -22,4 +22,5 @@
#define FEET_PER_KILOFEET 1000 // Used for altitude
#define METERS_PER_KILOMETER 1000
#define METERS_PER_MILE 1609.344
#define METERS_PER_FOOT 3.28084
#define METERS_PER_NAUTICALMILE 1852.001

16
src/main/common/log.h
View file

@ -47,13 +47,7 @@ void logInit(void);
void _logf(logTopic_e topic, unsigned level, const char *fmt, ...) __attribute__ ((format (printf, 3, 4)));
void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t size);
// LOG_* macro definitions
#if !defined(LOG_LEVEL_MAXIMUM)
#define LOG_LEVEL_MAXIMUM LOG_LEVEL_DEBUG
#endif
#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_ERROR
#if defined(USE_LOG)
#define LOG_E(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, fmt, ##__VA_ARGS__)
#define LOG_BUFFER_E(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, buf, size)
#else
@ -61,7 +55,7 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t
#define LOG_BUFFER_E(...)
#endif
#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_WARNING
#if defined(USE_LOG)
#define LOG_W(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, fmt, ##__VA_ARGS__)
#define LOG_BUF_W(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, buf, size)
#else
@ -69,7 +63,7 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t
#define LOG_BUF_W(...)
#endif
#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_INFO
#if defined(USE_LOG)
#define LOG_I(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, fmt, ##__VA_ARGS__)
#define LOG_BUF_I(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, buf, size)
#else
@ -77,7 +71,7 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t
#define LOG_BUF_I(...)
#endif
#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_VERBOSE
#if defined(USE_LOG)
#define LOG_V(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, fmt, ##__VA_ARGS__)
#define LOG_BUF_V(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, buf, size)
#else
@ -85,7 +79,7 @@ void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t
#define LOG_BUF_V(...)
#endif
#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_DEBUG
#if defined(USE_LOG)
#define LOG_D(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, fmt, ##__VA_ARGS__)
#define LOG_BUF_D(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, buf, size)
#else

29
src/main/config/config_eeprom.c
View file

@ -123,17 +123,6 @@ void initEEPROM(void)
#endif
}
static uint16_t updateCRC(uint16_t crc, const void *data, uint32_t length)
{
const uint8_t *p = (const uint8_t *)data;
const uint8_t *pend = p + length;
for (; p != pend; p++) {
crc = crc16_ccitt(crc, *p);
}
return crc;
}
// Scan the EEPROM config. Returns true if the config is valid.
bool isEEPROMContentValid(void)
{
@ -143,7 +132,7 @@ bool isEEPROMContentValid(void)
if (header->format != EEPROM_CONF_VERSION) {
return false;
}
uint16_t crc = updateCRC(0, header, sizeof(*header));
uint16_t crc = crc16_ccitt_update(0, header, sizeof(*header));
p += sizeof(*header);
for (;;) {
@ -164,13 +153,13 @@ bool isEEPROMContentValid(void)
return false;
}
crc = updateCRC(crc, p, record->size);
crc = crc16_ccitt_update(crc, p, record->size);
p += record->size;
}
const configFooter_t *footer = (const configFooter_t *)p;
crc = updateCRC(crc, footer, sizeof(*footer));
crc = crc16_ccitt_update(crc, footer, sizeof(*footer));
p += sizeof(*footer);
const uint16_t checkSum = *(uint16_t *)p;
p += sizeof(checkSum);
@ -255,7 +244,7 @@ static bool writeSettingsToEEPROM(void)
if (config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header)) < 0) {
return false;
}
uint16_t crc = updateCRC(0, (uint8_t *)&header, sizeof(header));
uint16_t crc = crc16_ccitt_update(0, (uint8_t *)&header, sizeof(header));
PG_FOREACH(reg) {
const uint16_t regSize = pgSize(reg);
configRecord_t record = {
@ -271,11 +260,11 @@ static bool writeSettingsToEEPROM(void)
if (config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)) < 0) {
return false;
}
crc = updateCRC(crc, (uint8_t *)&record, sizeof(record));
crc = crc16_ccitt_update(crc, (uint8_t *)&record, sizeof(record));
if (config_streamer_write(&streamer, reg->address, regSize) < 0) {
return false;
}
crc = updateCRC(crc, reg->address, regSize);
crc = crc16_ccitt_update(crc, reg->address, regSize);
} else {
// write one instance for each profile
for (uint8_t profileIndex = 0; profileIndex < MAX_PROFILE_COUNT; profileIndex++) {
@ -285,12 +274,12 @@ static bool writeSettingsToEEPROM(void)
if (config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)) < 0) {
return false;
}
crc = updateCRC(crc, (uint8_t *)&record, sizeof(record));
crc = crc16_ccitt_update(crc, (uint8_t *)&record, sizeof(record));
const uint8_t *address = reg->address + (regSize * profileIndex);
if (config_streamer_write(&streamer, address, regSize) < 0) {
return false;
}
crc = updateCRC(crc, address, regSize);
crc = crc16_ccitt_update(crc, address, regSize);
}
}
}
@ -302,7 +291,7 @@ static bool writeSettingsToEEPROM(void)
if (config_streamer_write(&streamer, (uint8_t *)&footer, sizeof(footer)) < 0) {
return false;
}
crc = updateCRC(crc, (uint8_t *)&footer, sizeof(footer));
crc = crc16_ccitt_update(crc, (uint8_t *)&footer, sizeof(footer));
// append checksum now
if (config_streamer_write(&streamer, (uint8_t *)&crc, sizeof(crc)) < 0) {

100
src/main/config/config_unittest.h
View file

@ -1,100 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef SRC_MAIN_SCHEDULER_C_
#ifdef UNIT_TEST
cfTask_t *unittest_scheduler_selectedTask;
uint8_t unittest_scheduler_selectedTaskDynamicPriority;
uint16_t unittest_scheduler_waitingTasks;
uint32_t unittest_scheduler_timeToNextRealtimeTask;
bool unittest_outsideRealtimeGuardInterval;
#define GET_SCHEDULER_LOCALS() \
{ \
unittest_scheduler_selectedTask = selectedTask; \
unittest_scheduler_selectedTaskDynamicPriority = selectedTaskDynamicPriority; \
unittest_scheduler_waitingTasks = waitingTasks; \
unittest_scheduler_timeToNextRealtimeTask = timeToNextRealtimeTask; \
}
#else
#define SET_SCHEDULER_LOCALS() {}
#define GET_SCHEDULER_LOCALS() {}
#endif
#endif
#ifdef SRC_MAIN_FLIGHT_PID_C_
#ifdef UNIT_TEST
float unittest_pidLuxFloat_lastErrorForDelta[3];
float unittest_pidLuxFloat_delta1[3];
float unittest_pidLuxFloat_delta2[3];
float unittest_pidLuxFloat_PTerm[3];
float unittest_pidLuxFloat_ITerm[3];
float unittest_pidLuxFloat_DTerm[3];
#define SET_PID_LUX_FLOAT_LOCALS(axis) \
{ \
lastErrorForDelta[axis] = unittest_pidLuxFloat_lastErrorForDelta[axis]; \
delta1[axis] = unittest_pidLuxFloat_delta1[axis]; \
delta2[axis] = unittest_pidLuxFloat_delta2[axis]; \
}
#define GET_PID_LUX_FLOAT_LOCALS(axis) \
{ \
unittest_pidLuxFloat_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \
unittest_pidLuxFloat_delta1[axis] = delta1[axis]; \
unittest_pidLuxFloat_delta2[axis] = delta2[axis]; \
unittest_pidLuxFloat_PTerm[axis] = PTerm; \
unittest_pidLuxFloat_ITerm[axis] = ITerm; \
unittest_pidLuxFloat_DTerm[axis] = DTerm; \
}
int32_t unittest_pidMultiWiiRewrite_lastErrorForDelta[3];
int32_t unittest_pidMultiWiiRewrite_PTerm[3];
int32_t unittest_pidMultiWiiRewrite_ITerm[3];
int32_t unittest_pidMultiWiiRewrite_DTerm[3];
#define SET_PID_MULTI_WII_REWRITE_LOCALS(axis) \
{ \
lastErrorForDelta[axis] = unittest_pidMultiWiiRewrite_lastErrorForDelta[axis]; \
}
#define GET_PID_MULTI_WII_REWRITE_LOCALS(axis) \
{ \
unittest_pidMultiWiiRewrite_lastErrorForDelta[axis] = lastErrorForDelta[axis]; \
unittest_pidMultiWiiRewrite_PTerm[axis] = PTerm; \
unittest_pidMultiWiiRewrite_ITerm[axis] = ITerm; \
unittest_pidMultiWiiRewrite_DTerm[axis] = DTerm; \
}
#else
#define SET_PID_LUX_FLOAT_LOCALS(axis) {}
#define GET_PID_LUX_FLOAT_LOCALS(axis) {}
#define SET_PID_MULTI_WII_REWRITE_LOCALS(axis) {}
#define GET_PID_MULTI_WII_REWRITE_LOCALS(axis) {}
#endif // UNIT_TEST
#endif // SRC_MAIN_FLIGHT_PID_C_

6
src/main/drivers/accgyro/accgyro_mpu.h
View file

@ -21,12 +21,6 @@
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#define MPU_I2C_ADDRESS 0x68
// MPU6050
#define MPU_RA_WHO_AM_I_LEGACY 0x00
#define MPUx0x0_WHO_AM_I_CONST (0x68) // MPU6000 and 6050
#define MPU6000_WHO_AM_I_CONST (0x68)
#define MPU6500_WHO_AM_I_CONST (0x70)
#define MPU9250_WHO_AM_I_CONST (0x71)

222
src/main/drivers/accgyro/accgyro_mpu6050.c
View file

@ -1,222 +0,0 @@
/*
* 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/>.
*/
/*
* Authors:
* Dominic Clifton - Cleanflight implementation
* John Ihlein - Initial FF32 code
* Konstantin Sharlaimov - busDevice refactoring
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/io.h"
#include "drivers/exti.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_mpu6050.h"
#if defined(USE_IMU_MPU6050)
#define BIT_H_RESET 0x80
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_INQUIRY_MASK 0x7E
typedef enum {
MPU6050_NONE = 0,
MPU6050_HALF_RESOLUTION = 1,
MPU6050_FULL_RESOLUTION = 2
} mpuDetectionResult_e;
static bool mpu6050InitDone = false;
static void mpu6050AccAndGyroInit(gyroDev_t *gyro)
{
const gyroFilterAndRateConfig_t * config = mpuChooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs);
gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;
gyroIntExtiInit(gyro);
busSetSpeed(gyro->busDev, BUS_SPEED_INITIALIZATION);
if (!mpu6050InitDone) {
// Device Reset
busWrite(gyro->busDev, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
delay(150);
// Clock Source PPL with Z axis gyro reference
busWrite(gyro->busDev, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
delayMicroseconds(15);
// Accel Sample Rate 1kHz
// Gyroscope Output Rate = 1kHz when the DLPF is enabled
busWrite(gyro->busDev, MPU_RA_SMPLRT_DIV, config->gyroConfigValues[1]);
delayMicroseconds(15);
// Accel and Gyro DLPF Setting
busWrite(gyro->busDev, MPU_RA_CONFIG, config->gyroConfigValues[0]);
delayMicroseconds(1);
// Gyro +/- 2000 DPS Full Scale
busWrite(gyro->busDev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delayMicroseconds(15);
// Accel +/- 16 G Full Scale
busWrite(gyro->busDev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delayMicroseconds(15);
busWrite(gyro->busDev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
delayMicroseconds(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
busWrite(gyro->busDev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
delayMicroseconds(15);
#endif
mpu6050InitDone = true;
}
busSetSpeed(gyro->busDev, BUS_SPEED_FAST);
}
static void mpu6050AccInit(accDev_t *acc)
{
mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
if (ctx->chipMagicNumber == 0x6850) {
acc->acc_1G = 512 * 4;
}
else {
acc->acc_1G = 256 * 4;
}
}
bool mpu6050AccDetect(accDev_t *acc)
{
acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_MPU6050, acc->imuSensorToUse);
if (acc->busDev == NULL) {
return false;
}
mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
if (ctx->chipMagicNumber == 0x6850 || ctx->chipMagicNumber == 0x6050) {
acc->initFn = mpu6050AccInit;
acc->readFn = mpuAccReadScratchpad;
acc->accAlign = acc->busDev->param;
return true;
}
return false;
}
static mpuDetectionResult_e mpu6050DeviceDetect(busDevice_t * dev)
{
uint8_t in;
uint8_t readBuffer[6];
uint8_t attemptsRemaining = 5;
busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
busWrite(dev, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
do {
delay(150);
busRead(dev, MPU_RA_WHO_AM_I, &in);
in &= MPU_INQUIRY_MASK;
if (in == MPUx0x0_WHO_AM_I_CONST) {
break;
}
if (!attemptsRemaining) {
return MPU6050_NONE;
}
} while (attemptsRemaining--);
// There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
// determine product ID and accel revision
busReadBuf(dev, MPU_RA_XA_OFFS_H, readBuffer, 6);
uint8_t revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (revision) {
/* Congrats, these parts are better. */
if (revision == 1) {
return MPU6050_HALF_RESOLUTION;
} else if (revision == 2) {
return MPU6050_FULL_RESOLUTION;
} else if ((revision == 3) || (revision == 7)) {
return MPU6050_FULL_RESOLUTION;
} else {
return MPU6050_NONE;
}
} else {
uint8_t productId;
busRead(dev, MPU_RA_PRODUCT_ID, &productId);
revision = productId & 0x0F;
if (!revision) {
return MPU6050_NONE;
} else if (revision == 4) {
return MPU6050_HALF_RESOLUTION;
} else {
return MPU6050_FULL_RESOLUTION;
}
}
return MPU6050_NONE;
}
bool mpu6050GyroDetect(gyroDev_t *gyro)
{
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_MPU6050, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
mpuDetectionResult_e res = mpu6050DeviceDetect(gyro->busDev);
if (res == MPU6050_NONE) {
busDeviceDeInit(gyro->busDev);
return false;
}
// Magic number for ACC detection to indicate that we have detected MPU6000 gyro
mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
ctx->chipMagicNumber = res == MPU6050_FULL_RESOLUTION ? 0x6850 : 0x6050;
gyro->initFn = mpu6050AccAndGyroInit;
gyro->readFn = mpuGyroReadScratchpad;
gyro->intStatusFn = gyroCheckDataReady;
gyro->temperatureFn = mpuTemperatureReadScratchpad;
gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor
gyro->gyroAlign = gyro->busDev->param;
return true;
}
#endif

23
src/main/drivers/accgyro/accgyro_mpu6050.h
View file

@ -1,23 +0,0 @@
/*
* 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
#include "drivers/sensor.h"
bool mpu6050AccDetect(accDev_t *acc);
bool mpu6050GyroDetect(gyroDev_t *gyro);

1
src/main/drivers/bus.h
View file

@ -77,7 +77,6 @@ typedef enum {
/* Combined ACC/GYRO chips */
DEVHW_MPU6000,
DEVHW_MPU6050,
DEVHW_MPU6500,
DEVHW_BMI160,
DEVHW_BMI088_GYRO,

6
src/main/drivers/osd_symbols.h
View file

@ -165,6 +165,12 @@
#define SYM_SWITCH_INDICATOR_MID 0xD1 // 209 Switch Mid
#define SYM_SWITCH_INDICATOR_HIGH 0xD2 // 210 Switch Low
#define SYM_AH 0xD3 // 211 Amphours symbol
#define SYM_GLIDE_DIST 0xD4 // 212 Glide Distance
#define SYM_GLIDE_MINS 0xD5 // 213 Glide Minutes
#define SYM_AH_V_FT_0 0xD6 // 214 mAh/v-ft left
#define SYM_AH_V_FT_1 0xD7 // 215 mAh/v-ft right
#define SYM_AH_V_M_0 0xD8 // 216 mAh/v-m left
#define SYM_AH_V_M_1 0xD9 // 217 mAh/v-m right
#define SYM_LOGO_START 0x101 // 257 to 280, INAV logo
#define SYM_LOGO_WIDTH 6

18
src/main/fc/cli.c
View file

@ -125,10 +125,6 @@ bool cliMode = false;
#include "telemetry/telemetry.h"
#include "build/debug.h"
#if MCU_FLASH_SIZE > 128
#define PLAY_SOUND
#endif
extern timeDelta_t cycleTime; // FIXME dependency on mw.c
extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];
@ -182,7 +178,7 @@ static const char * const blackboxIncludeFlagNames[] = {
/* Sensor names (used in lookup tables for *_hardware settings and in status command output) */
// sync with gyroSensor_e
static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270", "FAKE"};
static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270", "FAKE"};
// sync this with sensors_e
static const char * const sensorTypeNames[] = {
@ -2956,7 +2952,6 @@ static void cliMotor(char *cmdline)
cliPrintLinef("motor %d: %d", motor_index, motor_disarmed[motor_index]);
}
#ifdef PLAY_SOUND
static void cliPlaySound(char *cmdline)
{
int i;
@ -2989,7 +2984,6 @@ static void cliPlaySound(char *cmdline)
cliPrintLinef("Playing sound %d: %s", i, name);
beeper(beeperModeForTableIndex(i));
}
#endif
static void cliProfile(char *cmdline)
{
@ -3479,7 +3473,6 @@ static void cliStatus(char *cmdline)
}
}
#ifndef SKIP_TASK_STATISTICS
static void cliTasks(char *cmdline)
{
UNUSED(cmdline);
@ -3508,7 +3501,6 @@ static void cliTasks(char *cmdline)
cliPrintLinef("Task check function %13d %7d %25d", (uint32_t)checkFuncInfo.maxExecutionTime, (uint32_t)checkFuncInfo.averageExecutionTime, (uint32_t)checkFuncInfo.totalExecutionTime / 1000);
cliPrintLinef("Total (excluding SERIAL) %21d.%1d%% %4d.%1d%%", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10);
}
#endif
static void cliVersion(char *cmdline)
{
@ -3541,7 +3533,6 @@ static void cliMemory(char *cmdline)
}
}
#if !defined(SKIP_TASK_STATISTICS) && !defined(SKIP_CLI_RESOURCES)
static void cliResource(char *cmdline)
{
UNUSED(cmdline);
@ -3560,7 +3551,6 @@ static void cliResource(char *cmdline)
}
}
}
#endif
static void backupConfigs(void)
{
@ -3907,16 +3897,12 @@ const clicmd_t cmdTable[] = {
#ifdef USE_USB_MSC
CLI_COMMAND_DEF("msc", "switch into msc mode", NULL, cliMsc),
#endif
#ifdef PLAY_SOUND
CLI_COMMAND_DEF("play_sound", NULL, "[<index>]\r\n", cliPlaySound),
#endif
CLI_COMMAND_DEF("profile", "change profile",
"[<index>]", cliProfile),
CLI_COMMAND_DEF("battery_profile", "change battery profile",
"[<index>]", cliBatteryProfile),
#if !defined(SKIP_TASK_STATISTICS) && !defined(SKIP_CLI_RESOURCES)
CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource),
#endif
CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
#ifdef USE_SECONDARY_IMU
CLI_COMMAND_DEF("imu2", "Secondary IMU", NULL, cliImu2),
@ -3951,9 +3937,7 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo),
#endif
CLI_COMMAND_DEF("status", "show status", NULL, cliStatus),
#ifndef SKIP_TASK_STATISTICS
CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks),
#endif
#ifdef USE_TEMPERATURE_SENSOR
CLI_COMMAND_DEF("temp_sensor", "change temp sensor settings", NULL, cliTempSensor),
#endif

6
src/main/fc/config.c
View file

@ -421,16 +421,10 @@ void setGyroCalibrationAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]) {
gyroConfigMutable()->gyro_zero_cal[X] = getGyroZero[X];
gyroConfigMutable()->gyro_zero_cal[Y] = getGyroZero[Y];
gyroConfigMutable()->gyro_zero_cal[Z] = getGyroZero[Z];
// save the calibration
writeEEPROM();
readEEPROM();
}
void setGravityCalibrationAndWriteEEPROM(float getGravity) {
gyroConfigMutable()->gravity_cmss_cal = getGravity;
// save the calibration
writeEEPROM();
readEEPROM();
}
void beeperOffSet(uint32_t mask)

2
src/main/fc/fc_core.c
View file

@ -886,7 +886,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
annexCode(dT);
if (rxConfig()->rcFilterFrequency) {
rcInterpolationApply(isRXDataNew);
rcInterpolationApply(isRXDataNew, currentTimeUs);
}
if (isRXDataNew) {

14
src/main/fc/fc_msp.c
View file

@ -116,6 +116,7 @@
#include "sensors/gyro.h"
#include "sensors/opflow.h"
#include "sensors/temperature.h"
#include "sensors/esc_sensor.h"
#include "telemetry/telemetry.h"
@ -1521,6 +1522,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
#endif
#ifdef USE_ESC_SENSOR
case MSP2_INAV_ESC_RPM:
{
uint8_t motorCount = getMotorCount();
for (uint8_t i = 0; i < motorCount; i++){
const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry
sbufWriteU32(dst, escState->rpm);
}
}
break;
#endif
default:
return false;
}

3
src/main/fc/fc_msp_box.c
View file

@ -86,6 +86,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ .boxId = BOXBRAKING, .boxName = "MC BRAKING", .permanentId = 46 },
{ .boxId = BOXUSER1, .boxName = "USER1", .permanentId = BOX_PERMANENT_ID_USER1 },
{ .boxId = BOXUSER2, .boxName = "USER2", .permanentId = BOX_PERMANENT_ID_USER2 },
{ .boxId = BOXUSER3, .boxName = "USER3", .permanentId = BOX_PERMANENT_ID_USER3 },
{ .boxId = BOXLOITERDIRCHN, .boxName = "LOITER CHANGE", .permanentId = 49 },
{ .boxId = BOXMSPRCOVERRIDE, .boxName = "MSP RC OVERRIDE", .permanentId = 50 },
{ .boxId = BOXPREARM, .boxName = "PREARM", .permanentId = 51 },
@ -313,6 +314,7 @@ void initActiveBoxIds(void)
// USER modes are only used for PINIO at the moment
ADD_ACTIVE_BOX(BOXUSER1);
ADD_ACTIVE_BOX(BOXUSER2);
ADD_ACTIVE_BOX(BOXUSER3);
#endif
#if defined(USE_OSD) && defined(OSD_LAYOUT_COUNT)
@ -390,6 +392,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBRAKING)), BOXBRAKING);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER1)), BOXUSER1);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER2)), BOXUSER2);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER3)), BOXUSER3);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)), BOXLOITERDIRCHN);
#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)), BOXMSPRCOVERRIDE);

2
src/main/fc/fc_tasks.c
View file

@ -459,7 +459,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
.taskName = "RX",
.checkFunc = taskUpdateRxCheck,
.taskFunc = taskUpdateRxMain,
.desiredPeriod = TASK_PERIOD_HZ(50), // If event-based scheduling doesn't work, fallback to periodic scheduling
.desiredPeriod = TASK_PERIOD_HZ(10), // If event-based scheduling doesn't work, fallback to periodic scheduling
.staticPriority = TASK_PRIORITY_HIGH,
},

1
src/main/fc/rc_modes.h
View file

@ -74,6 +74,7 @@ typedef enum {
BOXAUTOLEVEL = 45,
BOXPLANWPMISSION = 46,
BOXSOARING = 47,
BOXUSER3 = 48,
CHECKBOX_ITEM_COUNT
} boxId_e;

106
src/main/fc/rc_smoothing.c
View file

@ -47,25 +47,53 @@
#include "flight/mixer.h"
static biquadFilter_t rcSmoothFilter[4];
// RC Interpolation is not allowed to go below this value.
#define RC_INTERPOLATION_MIN_FREQUENCY 15
static pt3Filter_t rcSmoothFilter[4];
static float rcStickUnfiltered[4];
static uint16_t rcUpdateFrequency;
static void rcInterpolationInit(int rcFilterFreqency)
{
for (int stick = 0; stick < 4; stick++) {
biquadFilterInitLPF(&rcSmoothFilter[stick], rcFilterFreqency, getLooptime());
}
uint16_t getRcUpdateFrequency(void) {
return rcUpdateFrequency;
}
void rcInterpolationApply(bool isRXDataNew)
static int32_t applyRcUpdateFrequencyMedianFilter(int32_t newReading)
{
#define RC_FILTER_SAMPLES_MEDIAN 9
static int32_t filterSamples[RC_FILTER_SAMPLES_MEDIAN];
static int filterSampleIndex = 0;
static bool medianFilterReady = false;
filterSamples[filterSampleIndex] = newReading;
++filterSampleIndex;
if (filterSampleIndex == RC_FILTER_SAMPLES_MEDIAN) {
filterSampleIndex = 0;
medianFilterReady = true;
}
return medianFilterReady ? quickMedianFilter9(filterSamples) : newReading;
}
void rcInterpolationApply(bool isRXDataNew, timeUs_t currentTimeUs)
{
// Compute the RC update frequency
static timeUs_t previousRcData;
static int filterFrequency;
static bool initDone = false;
static float initFilterFreqency = 0;
const float dT = getLooptime() * 1e-6f;
if (isRXDataNew) {
if (!initDone || (initFilterFreqency != rxConfig()->rcFilterFrequency)) {
rcInterpolationInit(rxConfig()->rcFilterFrequency);
initFilterFreqency = rxConfig()->rcFilterFrequency;
if (!initDone) {
filterFrequency = rxConfig()->rcFilterFrequency;
// Initialize the RC smooth filter
for (int stick = 0; stick < 4; stick++) {
pt3FilterInit(&rcSmoothFilter[stick], pt3FilterGain(filterFrequency, dT));
}
initDone = true;
}
@ -79,32 +107,40 @@ void rcInterpolationApply(bool isRXDataNew)
return;
}
if (isRXDataNew) {
const timeDelta_t delta = cmpTimeUs(currentTimeUs, previousRcData);
rcUpdateFrequency = applyRcUpdateFrequencyMedianFilter(1.0f / (delta * 0.000001f));
previousRcData = currentTimeUs;
/*
* If auto smoothing is enabled, update the filters
*/
if (rxConfig()->autoSmooth) {
const int nyquist = rcUpdateFrequency / 2;
int newFilterFrequency = scaleRange(
rxConfig()->autoSmoothFactor,
1,
100,
nyquist,
rcUpdateFrequency / 10
);
// Do not allow filter frequency to go below RC_INTERPOLATION_MIN_FREQUENCY or above nuyquist frequency.
newFilterFrequency = constrain(newFilterFrequency, RC_INTERPOLATION_MIN_FREQUENCY, nyquist);
if (newFilterFrequency != filterFrequency) {
for (int stick = 0; stick < 4; stick++) {
rcCommand[stick] = biquadFilterApply(&rcSmoothFilter[stick], rcStickUnfiltered[stick]);
pt3FilterUpdateCutoff(&rcSmoothFilter[stick], pt3FilterGain(newFilterFrequency, dT));
}
filterFrequency = newFilterFrequency;
}
}
}
for (int stick = 0; stick < 4; stick++) {
rcCommand[stick] = pt3FilterApply(&rcSmoothFilter[stick], rcStickUnfiltered[stick]);
}
}

3
src/main/fc/rc_smoothing.h
View file

@ -28,4 +28,5 @@
#include <stdlib.h>
#include <stdint.h>
void rcInterpolationApply(bool isRXDataNew);
uint16_t getRcUpdateFrequency(void);
void rcInterpolationApply(bool isRXDataNew, timeUs_t currentTimeUs);

49
src/main/fc/settings.yaml
View file

@ -4,7 +4,7 @@ tables:
- name: gyro_lpf
values: ["256HZ", "188HZ", "98HZ", "42HZ", "20HZ", "10HZ"]
- name: acc_hardware
values: ["NONE", "AUTO", "MPU6050", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270", "FAKE"]
values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270", "FAKE"]
enum: accelerationSensor_e
- name: rangefinder_hardware
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C"]
@ -187,6 +187,9 @@ tables:
- name: rth_trackback_mode
values: ["OFF", "ON", "FS"]
enum: rthTrackbackMode_e
- name: dynamic_gyro_notch_mode
values: ["2D", "3D_R", "3D_P", "3D_Y", "3D_RP", "3D_RY", "3D_PY", "3D"]
enum: dynamicGyroNotchMode_e
- name: nav_fw_wp_turn_smoothing
values: ["OFF", "ON", "ON-CUT"]
enum: wpFwTurnSmoothing_e
@ -288,6 +291,19 @@ groups:
condition: USE_DYNAMIC_FILTERS
min: 30
max: 250
- name: dynamic_gyro_notch_mode
description: "Gyro dynamic notch type"
default_value: "2D"
table: dynamic_gyro_notch_mode
field: dynamicGyroNotchMode
condition: USE_DYNAMIC_FILTERS
- name: dynamic_gyro_notch_3d_q
description: "Q factor for 3D dynamic notches"
default_value: 200
field: dynamicGyroNotch3dQ
condition: USE_DYNAMIC_FILTERS
min: 1
max: 1000
- name: gyro_to_use
condition: USE_DUAL_GYRO
min: 0
@ -652,11 +668,6 @@ groups:
description: "Selection of baro hardware. See Wiki Sensor auto detect and hardware failure detection for more info"
default_value: "AUTO"
table: baro_hardware
- name: baro_median_filter
description: "3-point median filtering for barometer readouts. No reason to change this setting"
default_value: ON
field: use_median_filtering
type: bool
- name: baro_cal_tolerance
description: "Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]."
default_value: 150
@ -730,11 +741,22 @@ groups:
min: 500
max: 10000
default_value: 3000
- name: rc_filter_frequency
- name: rc_filter_lpf_hz
description: "RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values"
default_value: 50
field: rcFilterFrequency
min: 0
min: 15
max: 250
- name: rc_filter_auto
description: "When enabled, INAV will set RC filtering based on refresh rate and smoothing factor."
type: bool
default_value: OFF
field: autoSmooth
- name: rc_filter_smoothing_factor
description: "The RC filter smoothing factor. The higher the value, the more smoothing but also the more delay in response. Value 1 sets the filter at half the refresh rate. Value 100 sets the filter to aprox. 10% of the RC refresh rate"
field: autoSmoothFactor
default_value: 30
min: 1
max: 100
- name: serialrx_provider
description: "When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section."
@ -2830,6 +2852,17 @@ groups:
field: fw.launch_climb_angle
min: 5
max: 45
- name: nav_fw_launch_manual_throttle
description: "Allows launch with manually controlled throttle. INAV only levels wings and controls climb pitch during launch. Throttle is controlled directly by throttle stick movement. IF USED WITHOUT A GPS LOCK plane must be launched immediately after throttle is increased to avoid issues with climb out stabilisation and the launch ending sooner than expected (launch end timer starts as soon as the throttle stick is raised)."
default_value: OFF
field: fw.launch_manual_throttle
type: bool
- name: nav_fw_launch_abort_deadband
description: "Launch abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch."
default_value: 100
field: fw.launch_abort_deadband
min: 2
max: 250
- name: nav_fw_cruise_yaw_rate
description: "Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]"
default_value: 20

4
src/main/flight/dynamic_gyro_notch.h
View file

@ -31,14 +31,12 @@
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
/*
* Number of peaks to detect with Dynamic Notch Filter aka Matrixc Filter. This is equal to the number of dynamic notch filters
* Number of peaks to detect with Dynamic Notch Filter aka Matrix Filter. This is equal to the number of dynamic notch filters
*/
#define DYN_NOTCH_PEAK_COUNT 3
typedef struct dynamicGyroNotchState_s {
uint16_t frequency[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT];
float dynNotchQ;
float dynNotch1Ctr;
float dynNotch2Ctr;
uint32_t looptime;
uint8_t enabled;

2
src/main/flight/rth_estimator.c
View file

@ -153,7 +153,7 @@ static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
uint16_t windHeading; // centidegrees
const float horizontalWindSpeed = takeWindIntoAccount ? getEstimatedHorizontalWindSpeed(&windHeading) / 100 : 0; // m/s
const float windHeadingDegrees = CENTIDEGREES_TO_DEGREES((float)windHeading);
const float verticalWindSpeed = getEstimatedWindSpeed(Z) / 100;
const float verticalWindSpeed = -getEstimatedWindSpeed(Z) / 100; //from NED to NEU
const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading);
const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, RTH_heading, horizontalWindSpeed, windHeadingDegrees);

112
src/main/flight/secondary_dynamic_gyro_notch.c Normal file
View file

@ -0,0 +1,112 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute 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.
*
* This file 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 this program. If not, see http://www.gnu.org/licenses/.
*/
#include "platform.h"
#ifdef USE_DYNAMIC_FILTERS
FILE_COMPILE_FOR_SPEED
#include <stdint.h>
#include "secondary_dynamic_gyro_notch.h"
#include "fc/config.h"
#include "build/debug.h"
#include "sensors/gyro.h"
#define SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 150
void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *state) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
state->filtersApplyFn[axis] = nullFilterApply;
}
state->dynNotchQ = gyroConfig()->dynamicGyroNotch3dQ / 100.0f;
state->enabled = gyroConfig()->dynamicGyroNotchMode != DYNAMIC_NOTCH_MODE_2D;
state->looptime = getLooptime();
if (
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_R ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
) {
/*
* Enable ROLL filter
*/
biquadFilterInit(&state->filters[FD_ROLL], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
state->filtersApplyFn[FD_ROLL] = (filterApplyFnPtr)biquadFilterApplyDF1;
}
if (
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_P ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
) {
/*
* Enable PITCH filter
*/
biquadFilterInit(&state->filters[FD_PITCH], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
state->filtersApplyFn[FD_PITCH] = (filterApplyFnPtr)biquadFilterApplyDF1;
}
if (
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_Y ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY ||
gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
) {
/*
* Enable YAW filter
*/
biquadFilterInit(&state->filters[FD_YAW], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
state->filtersApplyFn[FD_YAW] = (filterApplyFnPtr)biquadFilterApplyDF1;
}
}
void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]) {
if (state->enabled) {
/*
* The secondary dynamic notch uses only the first detected frequency
* The rest of peaks are ignored
*/
state->frequency[axis] = frequency[0];
// Filter update happens only if peak was detected
if (frequency[0] > 0.0f) {
biquadFilterUpdate(&state->filters[axis], state->frequency[axis], state->looptime, state->dynNotchQ, FILTER_NOTCH);
}
}
}
float secondaryDynamicGyroNotchFiltersApply(secondaryDynamicGyroNotchState_t *state, int axis, float input) {
return state->filtersApplyFn[axis]((filter_t *)&state->filters[axis], input);
}
#endif

43
src/main/flight/secondary_dynamic_gyro_notch.h Normal file
View file

@ -0,0 +1,43 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute 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.
*
* This file 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 this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
#include <stdint.h>
#include "common/axis.h"
#include "common/filter.h"
typedef struct secondaryDynamicGyroNotchState_s {
uint16_t frequency[XYZ_AXIS_COUNT];
float dynNotchQ;
uint32_t looptime;
uint8_t enabled;
biquadFilter_t filters[XYZ_AXIS_COUNT];
filterApplyFnPtr filtersApplyFn[XYZ_AXIS_COUNT];
} secondaryDynamicGyroNotchState_t;
void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *state);
void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]);
float secondaryDynamicGyroNotchFiltersApply(secondaryDynamicGyroNotchState_t *state, int axis, float input);

6
src/main/flight/wind_estimator.c
View file

@ -102,8 +102,8 @@ void updateWindEstimator(timeUs_t currentTimeUs)
// Fuselage direction in earth frame
fuselageDirection[X] = rMat[0][0];
fuselageDirection[Y] = rMat[1][0];
fuselageDirection[Z] = rMat[2][0];
fuselageDirection[Y] = -rMat[1][0];
fuselageDirection[Z] = -rMat[2][0];
timeDelta_t timeDelta = cmpTimeUs(currentTimeUs, lastUpdateUs);
// scrap our data and start over if we're taking too long to get a direction change
@ -131,7 +131,7 @@ void updateWindEstimator(timeUs_t currentTimeUs)
// when turning, use the attitude response to estimate wind speed
groundVelocityDiff[X] = groundVelocity[X] - lastGroundVelocity[X];
groundVelocityDiff[Y] = groundVelocity[Y] - lastGroundVelocity[Y];
groundVelocityDiff[Z] = groundVelocity[X] - lastGroundVelocity[Z];
groundVelocityDiff[Z] = groundVelocity[Z] - lastGroundVelocity[Z];
// estimate airspeed it using equation 6
float V = (calc_length_pythagorean_3D(groundVelocityDiff[X], groundVelocityDiff[Y], groundVelocityDiff[Z])) / fast_fsqrtf(diffLengthSq);

116
src/main/io/osd.c
View file

@ -197,7 +197,7 @@ static bool osdDisplayHasCanvas;
#define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 6);
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 7);
PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1);
static int digitCount(int32_t value)
@ -1001,7 +1001,7 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y)
{
*x = osdDisplayPort->cols / 2;
*y = osdDisplayPort->rows / 2;
*y += osdConfig()->horizon_offset;
*y -= osdConfig()->horizon_offset; // positive horizon_offset moves the HUD up, negative moves down
}
/**
@ -1090,6 +1090,24 @@ static inline int32_t osdGetAltitudeMsl(void)
return getEstimatedActualPosition(Z)+GPS_home.alt;
}
uint16_t osdGetRemainingGlideTime(void) {
float value = getEstimatedActualVelocity(Z);
static pt1Filter_t glideTimeFilterState;
const timeMs_t curTimeMs = millis();
static timeMs_t glideTimeUpdatedMs;
value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs));
glideTimeUpdatedMs = curTimeMs;
if (value < 0) {
value = osdGetAltitude() / abs((int)value);
} else {
value = 0;
}
return (uint16_t)round(value);
}
static bool osdIsHeadingValid(void)
{
#ifdef USE_SECONDARY_IMU
@ -2313,6 +2331,93 @@ static bool osdDrawSingleElement(uint8_t item)
buff[4] = '\0';
break;
}
case OSD_CLIMB_EFFICIENCY:
{
// amperage is in centi amps (10mA), vertical speed is in cms/s. We want
// Ah/dist only to show when vertical speed > 1m/s.
static pt1Filter_t veFilterState;
static timeUs_t vEfficiencyUpdated = 0;
int32_t value = 0;
timeUs_t currentTimeUs = micros();
timeDelta_t vEfficiencyTimeDelta = cmpTimeUs(currentTimeUs, vEfficiencyUpdated);
if (getEstimatedActualVelocity(Z) > 0) {
if (vEfficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
// Centiamps (kept for osdFormatCentiNumber) / m/s - Will appear as A / m/s in OSD
value = pt1FilterApply4(&veFilterState, (float)getAmperage() / (getEstimatedActualVelocity(Z) / 100.0f), 1, US2S(vEfficiencyTimeDelta));
vEfficiencyUpdated = currentTimeUs;
} else {
value = veFilterState.state;
}
}
bool efficiencyValid = (value > 0) && (getEstimatedActualVelocity(Z) > 100);
switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_GA:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL:
// mAh/foot
if (efficiencyValid) {
osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3);
tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_FT_0, SYM_AH_V_FT_1);
} else {
buff[0] = buff[1] = buff[2] = '-';
buff[3] = SYM_AH_V_FT_0;
buff[4] = SYM_AH_V_FT_1;
buff[5] = '\0';
}
break;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC:
// mAh/metre
if (efficiencyValid) {
osdFormatCentiNumber(buff, value, 1, 2, 2, 3);
tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_M_0, SYM_AH_V_M_1);
} else {
buff[0] = buff[1] = buff[2] = '-';
buff[3] = SYM_AH_V_M_0;
buff[4] = SYM_AH_V_M_1;
buff[5] = '\0';
}
break;
}
break;
}
case OSD_GLIDE_TIME_REMAINING:
{
uint16_t glideTime = osdGetRemainingGlideTime();
buff[0] = SYM_GLIDE_MINS;
if (glideTime > 0) {
// Maximum value we can show in minutes is 99 minutes and 59 seconds. It is extremely unlikely that glide
// time will be longer than 99 minutes. If it is, it will show 99:^^
if (glideTime > (99 * 60) + 59) {
tfp_sprintf(buff + 1, "%02d:", (int)(glideTime / 60));
buff[4] = SYM_DIRECTION;
buff[5] = SYM_DIRECTION;
} else {
tfp_sprintf(buff + 1, "%02d:%02d", (int)(glideTime / 60), (int)(glideTime % 60));
}
} else {
tfp_sprintf(buff + 1, "%s", "--:--");
}
buff[6] = '\0';
break;
}
case OSD_GLIDE_RANGE:
{
uint16_t glideSeconds = osdGetRemainingGlideTime();
buff[0] = SYM_GLIDE_DIST;
if (glideSeconds > 0) {
uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed;
osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0);
} else {
tfp_sprintf(buff + 1, "%s%c", "---", SYM_BLANK);
buff[5] = '\0';
}
break;
}
#endif
case OSD_SWITCH_INDICATOR_0:
@ -2802,7 +2907,7 @@ static bool osdDrawSingleElement(uint8_t item)
if (valid) {
uint16_t angle;
horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle);
int16_t windDirection = osdGetHeadingAngle((int)angle - DECIDEGREES_TO_DEGREES(attitude.values.yaw));
int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22);
buff[1] = SYM_DIRECTION + (windDirection*2 / 90);
} else {
horizontalWindSpeed = 0;
@ -2824,7 +2929,7 @@ static bool osdDrawSingleElement(uint8_t item)
bool valid = isEstimatedWindSpeedValid();
float verticalWindSpeed;
if (valid) {
verticalWindSpeed = getEstimatedWindSpeed(Z);
verticalWindSpeed = -getEstimatedWindSpeed(Z); //from NED to NEU
if (verticalWindSpeed < 0) {
buff[1] = SYM_AH_DECORATION_DOWN;
verticalWindSpeed = -verticalWindSpeed;
@ -4342,7 +4447,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
}
} else { /* messages shown only when Failsafe, WP, RTH or Emergency Landing not active */
if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH);
messages[messageCount++] = navConfig()->fw.launch_manual_throttle ? OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH_MANUAL) :
OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH);
const char *launchStateMessage = fixedWingLaunchStateMessage();
if (launchStateMessage) {
messages[messageCount++] = launchStateMessage;

4
src/main/io/osd.h
View file

@ -106,6 +106,7 @@
#define OSD_MSG_LANDED "LANDED"
#define OSD_MSG_PREPARING_LAND "PREPARING TO LAND"
#define OSD_MSG_AUTOLAUNCH "AUTOLAUNCH"
#define OSD_MSG_AUTOLAUNCH_MANUAL "AUTOLAUNCH (MANUAL)"
#define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)"
#define OSD_MSG_AUTOTRIM "(AUTOTRIM)"
#define OSD_MSG_AUTOTUNE "(AUTOTUNE)"
@ -260,6 +261,9 @@ typedef enum {
OSD_SWITCH_INDICATOR_3,
OSD_TPA_TIME_CONSTANT,
OSD_FW_LEVEL_TRIM,
OSD_GLIDE_TIME_REMAINING,
OSD_GLIDE_RANGE,
OSD_CLIMB_EFFICIENCY,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;

1
src/main/io/piniobox.h
View file

@ -29,6 +29,7 @@
#define BOX_PERMANENT_ID_USER1 47
#define BOX_PERMANENT_ID_USER2 48
#define BOX_PERMANENT_ID_USER3 49
#define BOX_PERMANENT_ID_NONE 255 // A permanent ID for no box mode

2
src/main/msp/msp_protocol_v2_inav.h
View file

@ -81,3 +81,5 @@
#define MSP2_INAV_MISC2 0x203A
#define MSP2_INAV_LOGIC_CONDITIONS_SINGLE 0x203B
#define MSP2_INAV_ESC_RPM 0x2040

12
src/main/navigation/navigation.c
View file

@ -203,6 +203,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended
.launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees
.launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg
.launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF
.launch_abort_deadband = SETTING_NAV_FW_LAUNCH_ABORT_DEADBAND_DEFAULT, // 100 us
.cruise_yaw_rate = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT, // 20dps
.allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
.useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
@ -1829,13 +1831,18 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
const timeUs_t currentTimeUs = micros();
UNUSED(previousState);
// Continue immediately to launch in progress if manual launch throttle used
if (navConfig()->fw.launch_manual_throttle) {
return NAV_FSM_EVENT_SUCCESS;
}
if (fixedWingLaunchStatus() == FW_LAUNCH_DETECTED) {
enableFixedWingLaunchController(currentTimeUs);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_IN_PROGRESS
}
// abort NAV_LAUNCH_MODE by moving sticks with low throttle or throttle stick < launch idle throttle
if (abortLaunchAllowed() && isRollPitchStickDeflected(LAUNCH_ABORT_STICK_DEADBAND)) {
if (abortLaunchAllowed() && isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {
abortFixedWingLaunch();
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
@ -4234,7 +4241,8 @@ bool navigationIsExecutingAnEmergencyLanding(void)
bool navigationInAutomaticThrottleMode(void)
{
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND));
return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAND)) ||
((stateFlags & NAV_CTL_LAUNCH) && !navConfig()->fw.launch_manual_throttle);
}
bool navigationIsControllingThrottle(void)

2
src/main/navigation/navigation.h
View file

@ -307,6 +307,8 @@ typedef struct navConfig_s {
uint16_t launch_max_altitude; // cm, altitude where to consider launch ended
uint8_t launch_climb_angle; // Target climb angle for launch (deg)
uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
bool launch_manual_throttle; // Allows launch with manual throttle control
uint8_t launch_abort_deadband; // roll/pitch stick movement deadband for launch abort
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
bool allow_manual_thr_increase;
bool useFwNavYawControl;

2
src/main/navigation/navigation_fixedwing.c
View file

@ -665,7 +665,7 @@ bool isFixedWingFlying(void)
#ifdef USE_PITOT
airspeed = pitot.airSpeed;
#endif
bool throttleCondition = rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle;
bool throttleCondition = getMotorCount() == 0 || rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle;
bool velCondition = posControl.actualState.velXY > 250 || airspeed > 250;
bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING;

47
src/main/navigation/navigation_fw_launch.c Executable file → Normal file
View file

@ -260,12 +260,19 @@ static inline bool isLaunchMaxAltitudeReached(void)
static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs)
{
return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && isRollPitchStickDeflected(LAUNCH_ABORT_STICK_DEADBAND);
return (initialTime + currentStateElapsedMs(currentTimeUs)) >= navConfig()->fw.launch_min_time &&
isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband);
}
static inline bool isProbablyNotFlying(void)
{
// Check flight status but only if GPS lock valid
return posControl.flags.estPosStatus == EST_TRUSTED && !isFixedWingFlying();
}
static void resetPidsIfNeeded(void) {
// Until motors are started don't use PID I-term and reset TPA filter
if (fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) {
// Don't use PID I-term and reset TPA filter until motors are started or until flight is detected
if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP || (navConfig()->fw.launch_manual_throttle && isThrottleLow())) {
pidResetErrorAccumulators();
pidResetTPAFilter();
}
@ -416,13 +423,32 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs)
{
uint16_t initialTime = 0;
if (navConfig()->fw.launch_manual_throttle) {
// reset timers when throttle is low or until flight detected and abort launch regardless of launch settings
if (isThrottleLow()) {
fwLaunch.currentStateTimeUs = currentTimeUs;
fwLaunch.pitchAngle = 0;
if (isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {
return FW_LAUNCH_EVENT_ABORT;
}
} else {
if (isProbablyNotFlying()) {
fwLaunch.currentStateTimeUs = currentTimeUs;
}
fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle;
}
} else {
initialTime = navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time;
rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
}
if (isLaunchMaxAltitudeReached()) {
return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
}
if (areSticksMoved(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) {
if (areSticksMoved(initialTime, currentTimeUs)) {
return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_ABORTED state
}
@ -438,13 +464,16 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
const timeMs_t endTimeMs = navConfig()->fw.launch_end_time;
if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected(LAUNCH_ABORT_STICK_DEADBAND)) {
if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {
return FW_LAUNCH_EVENT_SUCCESS; // End launch go to FW_LAUNCH_STATE_FLYING state
}
else {
// make a smooth transition from the launch state to the current state for throttle and the pitch angle
// Make a smooth transition from the launch state to the current state for pitch angle
// Do the same for throttle when manual launch throttle isn't used
if (!navConfig()->fw.launch_manual_throttle) {
const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, launchThrottle, rcCommand[THROTTLE]);
}
fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);
}
@ -498,8 +527,14 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs)
void resetFixedWingLaunchController(timeUs_t currentTimeUs)
{
if (navConfig()->fw.launch_manual_throttle) {
// no detection or motor control required with manual launch throttle
// so start at launch in progress
setCurrentState(FW_LAUNCH_STATE_IN_PROGRESS, currentTimeUs);
} else {
setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs);
}
}
void enableFixedWingLaunchController(timeUs_t currentTimeUs)
{

2
src/main/navigation/navigation_private.h
View file

@ -47,8 +47,6 @@
#define NAV_RTH_TRACKBACK_POINTS 50 // max number RTH trackback points
#define LAUNCH_ABORT_STICK_DEADBAND 250 // pitch/roll stick deflection for launch abort (us)
#define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro
_Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!");

4
src/main/programming/logic_condition.c
View file

@ -683,6 +683,10 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
return IS_RC_MODE_ACTIVE(BOXUSER2);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER3:
return IS_RC_MODE_ACTIVE(BOXUSER3);
break;
default:
return 0;
break;

1
src/main/programming/logic_condition.h
View file

@ -147,6 +147,7 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1, // 9
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2, // 10
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD, // 11
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER3, // 12
} logicFlightModeOperands_e;
typedef enum {

8
src/main/rx/rx.c
View file

@ -101,7 +101,7 @@ rxLinkStatistics_t rxLinkStatistics;
rxRuntimeConfig_t rxRuntimeConfig;
static uint8_t rcSampleIndex = 0;
PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 11);
PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 12);
#ifndef SERIALRX_PROVIDER
#define SERIALRX_PROVIDER 0
@ -129,7 +129,9 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig,
.rssiMin = SETTING_RSSI_MIN_DEFAULT,
.rssiMax = SETTING_RSSI_MAX_DEFAULT,
.sbusSyncInterval = SETTING_SBUS_SYNC_INTERVAL_DEFAULT,
.rcFilterFrequency = SETTING_RC_FILTER_FREQUENCY_DEFAULT,
.rcFilterFrequency = SETTING_RC_FILTER_LPF_HZ_DEFAULT,
.autoSmooth = SETTING_RC_FILTER_AUTO_DEFAULT,
.autoSmoothFactor = SETTING_RC_FILTER_SMOOTHING_FACTOR_DEFAULT,
#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
.mspOverrideChannels = SETTING_MSP_OVERRIDE_CHANNELS_DEFAULT,
#endif
@ -451,7 +453,7 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
}
rxDataProcessingRequired = false;
rxNextUpdateAtUs = currentTimeUs + DELAY_50_HZ;
rxNextUpdateAtUs = currentTimeUs + DELAY_10_HZ;
// only proceed when no more samples to skip and suspend period is over
if (skipRxSamples) {

2
src/main/rx/rx.h
View file

@ -124,6 +124,8 @@ typedef struct rxConfig_s {
uint16_t rx_min_usec;
uint16_t rx_max_usec;
uint8_t rcFilterFrequency; // RC filter cutoff frequency (smoothness vs response sharpness)
uint8_t autoSmooth; // auto smooth rx input (0 = off, 1 = on)
uint8_t autoSmoothFactor; // auto smooth rx input factor (1 = no smoothing, 100 = lots of smoothing)
uint16_t mspOverrideChannels; // Channels to override with MSP RC when BOXMSPRCOVERRIDE is active
uint8_t rssi_source;
#ifdef USE_SERIALRX_SRXL2

20
src/main/scheduler/scheduler.c
View file

@ -131,7 +131,6 @@ void taskSystem(timeUs_t currentTimeUs)
}
}
#ifndef SKIP_TASK_STATISTICS
#define TASK_MOVING_SUM_COUNT 32
FASTRAM timeUs_t checkFuncMaxExecutionTime;
FASTRAM timeUs_t checkFuncTotalExecutionTime;
@ -155,7 +154,6 @@ void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo)
taskInfo->averageExecutionTime = cfTasks[taskId].movingSumExecutionTime / TASK_MOVING_SUM_COUNT;
taskInfo->latestDeltaTime = cfTasks[taskId].taskLatestDeltaTime;
}
#endif
void rescheduleTask(cfTaskId_e taskId, timeDelta_t newPeriodUs)
{
@ -193,9 +191,6 @@ timeDelta_t getTaskDeltaTime(cfTaskId_e taskId)
void schedulerResetTaskStatistics(cfTaskId_e taskId)
{
#ifdef SKIP_TASK_STATISTICS
UNUSED(taskId);
#else
if (taskId == TASK_SELF) {
currentTask->movingSumExecutionTime = 0;
currentTask->totalExecutionTime = 0;
@ -205,7 +200,6 @@ void schedulerResetTaskStatistics(cfTaskId_e taskId)
cfTasks[taskId].totalExecutionTime = 0;
cfTasks[taskId].totalExecutionTime = 0;
}
#endif
}
void schedulerInit(void)
@ -237,13 +231,11 @@ void FAST_CODE NOINLINE scheduler(void)
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
waitingTasks++;
} else if (task->checkFunc(currentTimeBeforeCheckFuncCallUs, currentTimeBeforeCheckFuncCallUs - task->lastExecutedAt)) {
#ifndef SKIP_TASK_STATISTICS
const timeUs_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCallUs;
checkFuncMovingSumExecutionTime -= checkFuncMovingSumExecutionTime / TASK_MOVING_SUM_COUNT;
checkFuncMovingSumExecutionTime += checkFuncExecutionTime;
checkFuncTotalExecutionTime += checkFuncExecutionTime; // time consumed by scheduler + task
checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime);
#endif
task->lastSignaledAt = currentTimeBeforeCheckFuncCallUs;
task->taskAgeCycles = 1;
task->dynamicPriority = 1 + task->staticPriority;
@ -289,32 +281,20 @@ void FAST_CODE NOINLINE scheduler(void)
// Execute task
const timeUs_t currentTimeBeforeTaskCall = micros();
selectedTask->taskFunc(currentTimeBeforeTaskCall);
#ifndef SKIP_TASK_STATISTICS
const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_MOVING_SUM_COUNT;
selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
#endif
#if defined(SCHEDULER_DEBUG)
DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler
#endif
}
if (!selectedTask || forcedRealTimeTask) {
// Execute system real-time callbacks and account for them to SYSTEM account
const timeUs_t currentTimeBeforeTaskCall = micros();
taskRunRealtimeCallbacks(currentTimeBeforeTaskCall);
#ifndef SKIP_TASK_STATISTICS
selectedTask = &cfTasks[TASK_SYSTEM];
const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_MOVING_SUM_COUNT;
selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
#endif
#if defined(SCHEDULER_DEBUG)
DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs);
#endif
}
}

4
src/main/scheduler/scheduler.h
View file

@ -19,8 +19,6 @@
#include "common/time.h"
//#define SCHEDULER_DEBUG
typedef enum {
TASK_PRIORITY_IDLE = 0, // Disables dynamic scheduling, task is executed only if no other task is active this cycle
TASK_PRIORITY_LOW = 1,
@ -149,10 +147,8 @@ typedef struct {
/* Statistics */
timeUs_t movingSumExecutionTime; // moving sum over 32 samples
#ifndef SKIP_TASK_STATISTICS
timeUs_t maxExecutionTime;
timeUs_t totalExecutionTime; // total time consumed by task since boot
#endif
} cfTask_t;
extern cfTask_t cfTasks[TASK_COUNT];

14
src/main/sensors/acceleration.c
View file

@ -39,7 +39,6 @@ FILE_COMPILE_FOR_SPEED
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_mpu6000.h"
#include "drivers/accgyro/accgyro_mpu6050.h"
#include "drivers/accgyro/accgyro_mpu6500.h"
#include "drivers/accgyro/accgyro_mpu9250.h"
@ -119,19 +118,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
case ACC_AUTODETECT:
FALLTHROUGH;
#ifdef USE_IMU_MPU6050
case ACC_MPU6050: // MPU6050
if (mpu6050AccDetect(dev)) {
accHardware = ACC_MPU6050;
break;
}
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
if (accHardwareToUse != ACC_AUTODETECT) {
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_MPU6000
case ACC_MPU6000:
if (mpu6000AccDetect(dev)) {

1
src/main/sensors/acceleration.h
View file

@ -35,7 +35,6 @@
typedef enum {
ACC_NONE = 0,
ACC_AUTODETECT,
ACC_MPU6050,
ACC_MPU6000,
ACC_MPU6500,
ACC_MPU9250,

51
src/main/sensors/barometer.c
View file

@ -60,11 +60,10 @@ baro_t baro; // barometer access functions
#ifdef USE_BARO
PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 3);
PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 4);
PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig,
.baro_hardware = SETTING_BARO_HARDWARE_DEFAULT,
.use_median_filtering = SETTING_BARO_MEDIAN_FILTER_DEFAULT,
.baro_calibration_tolerance = SETTING_BARO_CAL_TOLERANCE_DEFAULT
);
@ -248,51 +247,6 @@ bool baroInit(void)
return true;
}
#define PRESSURE_SAMPLES_MEDIAN 3
/*
altitude pressure
0m 101325Pa
100m 100129Pa delta = 1196
1000m 89875Pa
1100m 88790Pa delta = 1085
At sea level an altitude change of 100m results in a pressure change of 1196Pa, at 1000m pressure change is 1085Pa
So set glitch threshold at 1000 - this represents an altitude change of approximately 100m.
*/
#define PRESSURE_DELTA_GLITCH_THRESHOLD 1000
static int32_t applyBarometerMedianFilter(int32_t newPressureReading)
{
static int32_t barometerFilterSamples[PRESSURE_SAMPLES_MEDIAN];
static int currentFilterSampleIndex = 0;
static bool medianFilterReady = false;
int nextSampleIndex = currentFilterSampleIndex + 1;
if (nextSampleIndex == PRESSURE_SAMPLES_MEDIAN) {
nextSampleIndex = 0;
medianFilterReady = true;
}
int previousSampleIndex = currentFilterSampleIndex - 1;
if (previousSampleIndex < 0) {
previousSampleIndex = PRESSURE_SAMPLES_MEDIAN - 1;
}
const int previousPressureReading = barometerFilterSamples[previousSampleIndex];
if (medianFilterReady) {
if (ABS(previousPressureReading - newPressureReading) < PRESSURE_DELTA_GLITCH_THRESHOLD) {
barometerFilterSamples[currentFilterSampleIndex] = newPressureReading;
currentFilterSampleIndex = nextSampleIndex;
return quickMedianFilter3(barometerFilterSamples);
} else {
// glitch threshold exceeded, so just return previous reading and don't add the glitched reading to the filter array
return barometerFilterSamples[previousSampleIndex];
}
} else {
barometerFilterSamples[currentFilterSampleIndex] = newPressureReading;
currentFilterSampleIndex = nextSampleIndex;
return newPressureReading;
}
}
typedef enum {
BAROMETER_NEEDS_SAMPLES = 0,
BAROMETER_NEEDS_CALCULATION
@ -323,9 +277,6 @@ uint32_t baroUpdate(void)
baro.dev.start_ut(&baro.dev);
}
baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature);
if (barometerConfig()->use_median_filtering) {
baro.baroPressure = applyBarometerMedianFilter(baro.baroPressure);
}
state = BAROMETER_NEEDS_SAMPLES;
return baro.dev.ut_delay;
break;

1
src/main/sensors/barometer.h
View file

@ -51,7 +51,6 @@ extern baro_t baro;
typedef struct barometerConfig_s {
uint8_t baro_hardware; // Barometer hardware to use
uint8_t use_median_filtering; // Use 3-point median filtering
uint16_t baro_calibration_tolerance; // Baro calibration tolerance (cm at sea level)
} barometerConfig_t;

33
src/main/sensors/gyro.c
View file

@ -41,7 +41,6 @@ FILE_COMPILE_FOR_SPEED
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_mpu6000.h"
#include "drivers/accgyro/accgyro_mpu6050.h"
#include "drivers/accgyro/accgyro_mpu6500.h"
#include "drivers/accgyro/accgyro_mpu9250.h"
@ -93,10 +92,11 @@ STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT];
EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState;
EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState;
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 4);
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
@ -117,6 +117,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.dynamicGyroNotchQ = SETTING_DYNAMIC_GYRO_NOTCH_Q_DEFAULT,
.dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT,
.dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT,
.dynamicGyroNotchMode = SETTING_DYNAMIC_GYRO_NOTCH_MODE_DEFAULT,
.dynamicGyroNotch3dQ = SETTING_DYNAMIC_GYRO_NOTCH_3D_Q_DEFAULT,
#endif
#ifdef USE_GYRO_KALMAN
.kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT,
@ -135,15 +137,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
case GYRO_AUTODETECT:
FALLTHROUGH;
#ifdef USE_IMU_MPU6050
case GYRO_MPU6050:
if (mpu6050GyroDetect(dev)) {
gyroHardware = GYRO_MPU6050;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_IMU_MPU6000
case GYRO_MPU6000:
if (mpu6000GyroDetect(dev)) {
@ -308,6 +301,9 @@ bool gyroInit(void)
#ifdef USE_DYNAMIC_FILTERS
// Dynamic notch running at PID frequency
dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
secondaryDynamicGyroNotchFiltersInit(&secondaryDynamicGyroNotchState);
gyroDataAnalyseStateInit(
&gyroAnalyseState,
gyroConfig()->dynamicGyroNotchMinHz,
@ -459,6 +455,14 @@ void FAST_CODE NOINLINE gyroFilter()
gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
gyroADCf = dynamicGyroNotchFiltersApply(&dynamicGyroNotchState, axis, gyroADCf);
}
/**
* Secondary dynamic notch filter.
* In some cases, noise amplitude is high enough not to be filtered by the primary filter.
* This happens on the first frequency with the biggest aplitude
*/
gyroADCf = secondaryDynamicGyroNotchFiltersApply(&secondaryDynamicGyroNotchState, axis, gyroADCf);
#endif
#ifdef USE_GYRO_KALMAN
@ -480,6 +484,13 @@ void FAST_CODE NOINLINE gyroFilter()
gyroAnalyseState.filterUpdateAxis,
gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis]
);
secondaryDynamicGyroNotchFiltersUpdate(
&secondaryDynamicGyroNotchState,
gyroAnalyseState.filterUpdateAxis,
gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis]
);
}
}
#endif

15
src/main/sensors/gyro.h
View file

@ -24,11 +24,11 @@
#include "config/parameter_group.h"
#include "drivers/sensor.h"
#include "flight/dynamic_gyro_notch.h"
#include "flight/secondary_dynamic_gyro_notch.h"
typedef enum {
GYRO_NONE = 0,
GYRO_AUTODETECT,
GYRO_MPU6050,
GYRO_MPU6000,
GYRO_MPU6500,
GYRO_MPU9250,
@ -40,6 +40,17 @@ typedef enum {
GYRO_FAKE
} gyroSensor_e;
typedef enum {
DYNAMIC_NOTCH_MODE_2D = 0,
DYNAMIC_NOTCH_MODE_R,
DYNAMIC_NOTCH_MODE_P,
DYNAMIC_NOTCH_MODE_Y,
DYNAMIC_NOTCH_MODE_RP,
DYNAMIC_NOTCH_MODE_RY,
DYNAMIC_NOTCH_MODE_PY,
DYNAMIC_NOTCH_MODE_3D
} dynamicGyroNotchMode_e;
typedef struct gyro_s {
bool initialized;
uint32_t targetLooptime;
@ -69,6 +80,8 @@ typedef struct gyroConfig_s {
uint16_t dynamicGyroNotchQ;
uint16_t dynamicGyroNotchMinHz;
uint8_t dynamicGyroNotchEnabled;
uint8_t dynamicGyroNotchMode;
uint16_t dynamicGyroNotch3dQ;
#endif
#ifdef USE_GYRO_KALMAN
uint16_t kalman_q;

1
src/main/target/NEUTRONRCH7BT/CMakeLists.txt Normal file
View file

@ -0,0 +1 @@
target_stm32h743xi(NEUTRONRCH7BT)

35
src/main/target/NEUTRONRCH7BT/config.c Normal file
View file

@ -0,0 +1,35 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include "platform.h"
#include "fc/fc_msp_box.h"
#include "fc/config.h"
#include "io/piniobox.h"
#include "io/serial.h"
void targetConfiguration(void)
{
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].functionMask = FUNCTION_MSP;
pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
pinioBoxConfigMutable()->permanentId[2] = BOX_PERMANENT_ID_USER3;
// compassConfigMutable()->mag_align = XXX;
}

43
src/main/target/NEUTRONRCH7BT/target.c Normal file
View file

@ -0,0 +1,43 @@
/*
* 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 <stdint.h>
#include "platform.h"
#include "drivers/bus.h"
#include "drivers/io.h"
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
#include "drivers/pinio.h"
#include "drivers/sensor.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2
DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4
DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5
DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S6
DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S7
DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S8
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

192
src/main/target/NEUTRONRCH7BT/target.h Normal file
View file

@ -0,0 +1,192 @@
/*
* 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
#define TARGET_BOARD_IDENTIFIER "NH7BT"
#define USBD_PRODUCT_STRING "NeuronRC H7 BT"
#define USE_TARGET_CONFIG
#define LED0 PE3
#define LED1 PE4
#define BEEPER PA15
#define BEEPER_INVERTED
// *************** IMU generic ***********************
// #define USE_DUAL_GYRO
// #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
#define USE_EXTI
#define USE_MPU_DATA_READY_SIGNAL
// *************** SPI1 IMU0 BMI270 *****************
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PD7
#define USE_IMU_BMI270
#define IMU_BMI270_ALIGN CW270_DEG
#define BMI270_SPI_BUS BUS_SPI1
#define BMI270_CS_PIN PC15
#define BMI270_EXTI_PIN PB2
// *************** SPI4 IMU1 BMI270 *****************
#define USE_SPI_DEVICE_4
#define SPI4_SCK_PIN PE12
#define SPI4_MISO_PIN PE13
#define SPI4_MOSI_PIN PE14
// #define USE_IMU_BMI270
// #define IMU_BMI270_ALIGN CW0_DEG_FLIP
// #define BMI270_SPI_BUS BUS_SPI4
// #define BMI270_CS_PIN PE11
// #define BMI270_EXTI_PIN PE15
// *************** SPI2 OSD ***********************
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI2
#define MAX7456_CS_PIN PB12
// *************** I2C /Baro/Mag *********************
#define USE_I2C
#define USE_I2C_DEVICE_1 // I2C1 is the broken out I2C bus
#define I2C1_SCL PB6
#define I2C1_SDA PB7
#define USE_I2C_DEVICE_2 // I2C2 is not broken out
#define I2C2_SCL PB10
#define I2C2_SDA PB11
#define USE_BARO
#define BARO_I2C_BUS BUS_I2C2
#define USE_BARO_BMP280
#define USE_BARO_MS5611
#define USE_BARO_DPS310
#define USE_BARO_SPL06
#define USE_MAG
#define MAG_I2C_BUS BUS_I2C1
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_MAG_IST8310
#define USE_MAG_IST8308
#define USE_MAG_MAG3110
#define USE_MAG_LIS3MDL
#define TEMPERATURE_I2C_BUS BUS_I2C1
#define PITOT_I2C_BUS BUS_I2C1
#define USE_RANGEFINDER
#define RANGEFINDER_I2C_BUS BUS_I2C1
// *************** UART *****************************
#define USE_VCP
#define USE_UART1
#define UART1_TX_PIN PA9
#define UART1_RX_PIN PA10
#define USE_UART2
#define UART2_TX_PIN PD5
#define UART2_RX_PIN PD6
// UART3 is connected to the BT chip
#define USE_UART3
#define UART3_TX_PIN PD8
#define UART3_RX_PIN PD9
#define USE_UART4
#define UART4_TX_PIN PB9
#define UART4_RX_PIN PB8
#define USE_UART6
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#define USE_UART7
#define UART7_TX_PIN PE8
#define UART7_RX_PIN PE7
#define USE_UART8
#define UART8_TX_PIN PE1
#define UART8_RX_PIN PE0
#define SERIAL_PORT_COUNT 8
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART6
// *************** SDIO SD BLACKBOX*******************
#define USE_SDCARD
#define USE_SDCARD_SDIO
#define SDCARD_SDIO_DEVICE SDIODEV_1
#define SDCARD_SDIO_4BIT
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
// *************** ADC *****************************
#define USE_ADC
#define ADC_INSTANCE ADC1
#define ADC_CHANNEL_1_PIN PC0 // VBAT1
#define ADC_CHANNEL_2_PIN PC1 // CURR1
#define ADC_CHANNEL_3_PIN PC4 // AirS
#define ADC_CHANNEL_4_PIN PA4 // VBAT2
#define ADC_CHANNEL_5_PIN PA7 // CURR2
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define AIRSPEED_ADC_CHANNEL ADC_CHN_3
// *************** PINIO ***************************
#define USE_PINIO
#define USE_PINIOBOX
#define PINIO1_PIN PD10 // Vsw control
#define PINIO2_PIN PD11 // Cam1/Cam2 switch control
#define PINIO3_PIN PC3 // BT Mode
// *************** LEDSTRIP ************************
#define USE_LED_STRIP
#define WS2811_PIN PA8
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
#define CURRENT_METER_SCALE 250
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD 0xffff
#define TARGET_IO_PORTE 0xffff
#define MAX_PWM_OUTPUT_PORTS 8
#define USE_DSHOT
#define USE_ESC_SENSOR

14
src/main/target/common.h
View file

@ -66,8 +66,6 @@
#define SCHEDULER_DELAY_LIMIT 100
#endif
#if (MCU_FLASH_SIZE > 256)
#if defined(MAG_I2C_BUS) || defined(VCM5883_I2C_BUS)
#define USE_MAG_VCM5883
#endif
@ -152,11 +150,6 @@
#define USE_POWER_LIMITS
#else // MCU_FLASH_SIZE < 256
#define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR
#endif
#if (MCU_FLASH_SIZE > 128)
#define NAV_FIXED_WING_LANDING
#define USE_SAFE_HOME
#define USE_AUTOTUNE_FIXED_WING
@ -191,18 +184,11 @@
// Wind estimator
#define USE_WIND_ESTIMATOR
#else // MCU_FLASH_SIZE < 128
#define SKIP_TASK_STATISTICS
#endif
//Designed to free space of F722 and F411 MCUs
#if (MCU_FLASH_SIZE > 512)
#define USE_VTX_FFPV
#define USE_PITOT_VIRTUAL
#define USE_SERIALRX_SUMD
#define USE_TELEMETRY_HOTT
#define USE_HOTT_TEXTMODE

4
src/main/target/common_hardware.c
View file

@ -36,10 +36,6 @@
BUSDEV_REGISTER_SPI(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
#endif
#if defined(USE_IMU_MPU6050)
BUSDEV_REGISTER_I2C(busdev_mpu6050, DEVHW_MPU6050, MPU6050_I2C_BUS, MPU_ADDRESS, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_MPU6050_ALIGN);
#endif
#if defined(USE_IMU_MPU6500)
#if defined(MPU6500_SPI_BUS)
BUSDEV_REGISTER_SPI(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_MPU6500_ALIGN);

4
src/main/target/common_post.h
View file

@ -31,11 +31,11 @@ extern uint8_t __config_end;
// Enable MSP_DISPLAYPORT for F3 targets without builtin OSD,
// since it's used to display CMS on MWOSD
#if !defined(USE_MSP_DISPLAYPORT) && (MCU_FLASH_SIZE > 128) && !defined(USE_OSD)
#if !defined(USE_MSP_DISPLAYPORT) && !defined(USE_OSD)
#define USE_MSP_DISPLAYPORT
#endif
#if defined(USE_OSD) && (MCU_FLASH_SIZE > 256)
#if defined(USE_OSD)
#define USE_CANVAS
#endif

7
src/main/target/sanity_check.h
View file

@ -29,7 +29,7 @@
#error "Unnecessary USE_ACC and/or USE_GYRO"
#endif
#if defined (USE_GYRO_MPU6000) || defined (USE_ACC_MPU6000) || defined (USE_GYRO_MPU6050) || defined (USE_ACC_MPU6050)
#if defined (USE_GYRO_MPU6000) || defined (USE_ACC_MPU6000)
#error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx"
#endif
@ -49,11 +49,6 @@
#error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx"
#endif
// Make sure IMU alignments are migrated to IMU_xxx_ALIGN
#if defined (GYRO_MPU6050_ALIGN) || defined (ACC_MPU6050_ALIGN)
#error "Replace GYRO_MPU6050_ALIGN and ACC_MPU6050_ALIGN with IMU_MPU6050_ALIGN"
#endif
#if defined (GYRO_MPU6000_ALIGN) || defined (ACC_MPU6000_ALIGN)
#error "Replace GYRO_MPU6000_ALIGN and ACC_MPU6000_ALIGN with IMU_MPU6000_ALIGN"
#endif