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

Merge branch 'master' into abo_mission_restart_option

Browse source
This commit is contained in:
breadoven 2021-11-06 14:29:06 +00:00 • committed by GitHub
commit 0b31a10ee9
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
22 changed files with 531 additions and 431 deletions
Download patch file Download diff file Expand all files Collapse all files

21
docs/Profiles.md
View file

@ -5,7 +5,7 @@ A profile is a set of configuration settings.
Currently three profiles are supported. The default profile is profile `1`. Currently three profiles are supported. The default profile is profile `1`.
## Changing profiles ## Changing profiles
### Stick Commands
Profiles can be selected using a GUI or the following stick combinations: Profiles can be selected using a GUI or the following stick combinations:
| Profile | Throttle | Yaw | Pitch | Roll | | Profile | Throttle | Yaw | Pitch | Roll |
@ -14,12 +14,29 @@ Profiles can be selected using a GUI or the following stick combinations:
| 2 | Down | Left | Up | Middle | | 2 | Down | Left | Up | Middle |
| 3 | Down | Left | Middle | Right | | 3 | Down | Left | Middle | Right |
The CLI `profile` command can also be used: ### CLI
The CLI `profile` command can also be used to change profiles:
``` ```
profile <index> profile <index>
``` ```
### Programming (4.0.0 onwards)
You can change profiles using the programming frame work. This allows a lot of flexability in how you change profiles.
For example, using a simple switch on channel 15.
[![For example, using a simple switch](https://i.imgur.com/SS9CaaOl.png)](https://i.imgur.com/SS9CaaO.png)
Or using the speed to change profiles. In this example:
- when lower than 25 cm/s (basically not flying), profiles are not effected.
- Below 2682 cm/s (60 mph | 97 Km/h) use Profile 1
- Above 5364 cm/s (120 mph | 193 Km/h) use Profile 3
- Between 2683 and 5364 cm/s, use Profile 2
[![Using speed to change profiles](https://i.imgur.com/WjkuhhWl.png)](https://i.imgur.com/WjkuhhW.png)
## Profile Contents
The values contained within a profile can be seen by using the CLI `dump profile` command. The values contained within a profile can be seen by using the CLI `dump profile` command.
e.g e.g

20
docs/Settings.md
View file

@ -1612,26 +1612,6 @@ Defines the type of the main gyro LPF filter. Possible values: `PT1`, `BIQUAD`.
--- ---
### gyro_notch_cutoff
_// TODO_
| Default | Min | Max |
| --- | --- | --- |
| 1 | 1 | 500 |
---
### gyro_notch_hz
_// TODO_
| Default | Min | Max |
| --- | --- | --- |
| 0 | | 500 |
---
### gyro_to_use ### gyro_to_use
_// TODO_ _// TODO_

12
docs/boards/Omnibus F4.md
View file

@ -166,9 +166,19 @@ Please note that this is *not* the motor PWM5/PWM6 pins, but small surface mount
### Omnibus F4 v3/v4/v5 SoftwareSerial Connections ### Omnibus F4 v3/v4/v5 SoftwareSerial Connections
The SOFTSERIAL1 is an uni-directional port mapped to UART6-TX pin. Softserial mappings can be selected by choosing between different build targets.
Some of these can be used for Smartport, FPort, or other inverted protocols.
OMNIBUSF4V3: Softserial1 on UART6-TX pin.
OMNIBUSF4V3_S6_SS: Softserial1 RX or TX on S6 (motor 6)
OMNIBUSF4V3_S5_S6_2SS: Softserial1 on S5 RX or TX (motor 5) and Softserial2 on S6 (motor 6)
OMNIBUSF4V3_S5S6_SS: Softserial1 on S5/RX and S6/TX
With the OMNIBUSF4V3 target, SOFTSERIAL1 is an uni-directional port mapped to UART6-TX pin.
When enabled, the UART6 is still available as hardware port but it's then RX-only port (good for e.g. receiving S.BUS input). TX instead is controlled in software and can be used for transmitting one-way telemetry (e.g. LTM). Please note that UART6-TX line passes programmable inverter on those boards, so it is a pure output-only port. SmartPort/FPort telemetry requires bi-directional communication, so it is not possible on this port without hardware modification (bypassing the inverter). When enabled, the UART6 is still available as hardware port but it's then RX-only port (good for e.g. receiving S.BUS input). TX instead is controlled in software and can be used for transmitting one-way telemetry (e.g. LTM). Please note that UART6-TX line passes programmable inverter on those boards, so it is a pure output-only port. SmartPort/FPort telemetry requires bi-directional communication, so it is not possible on this port without hardware modification (bypassing the inverter).
SmartPort / FPort is possible without a hardware inverter by using one of the OMNIBUSF4V3___SS builds and connecting SmartPort to the motor 5 or 6 pad.
## Where to buy: ## Where to buy:
* [Omnibus F4 v5](https://inavflight.com/shop/p/OMNIBUSF4V5) * [Omnibus F4 v5](https://inavflight.com/shop/p/OMNIBUSF4V5)

59
docs/development/wp_mission_schema/README.md Normal file
View file

@ -0,0 +1,59 @@
# XML Mission File Definition
## Overview
Historically, mission planners interoperating with inav (and multiwii) missions have used the XML mission file format defined by EOSBandi for MultiWii 2.3 (2013).
The format is defined the XSD schema here.
* Lower case tags are preferred by inav. Older tools may prefer uppercase (the original MW usage).
* For inav 4.0 and later, the `missionitem/flags` attribute is required for "fly-by home" waypoints and multi-mission files.
* For inav 4.0 and later, multi-mission files; mission segments are delimited by a flag value of `165` (the MSP protocol value).
* For multi-mission files, the waypoints may be numbered either sequentially across the whole file, or "reset-numbering" within each mission segment. The latter may (or not) be considered to be more "human readable".
* The `mwp` tag was introduced by the eponymous mission planner. Other mission planners may consider that reusing some of the tags (`cx`, `cy` - centre location, `zoom` TMS zoom level) is useful.
* The `version` tag may be intepreted by mission planners as they see fit. For example, the (obsolete) Android 'ez-gui' application requires '2.3-pre8'. For multi-mission files it is recommended to use another `version`.
## Examples
### Multi-mission file with sequential numbering
```
<?xml version="1.0" encoding="UTF-8" standalone="yes"?>
<mission>
<version value="2.3-pre8"/>
<mwp cx="-3.2869291" cy="54.5722109" home-x="0" home-y="0" zoom="14"/>
<missionitem no="1" action="WAYPOINT" lat="54.5722109" lon="-3.2869291" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"/>
<missionitem no="2" action="WAYPOINT" lat="54.5708178" lon="-3.2642698" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"/>
<missionitem no="3" action="WAYPOINT" lat="54.5698227" lon="-3.2385206" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="165"/>
<missionitem no="4" action="WAYPOINT" lat="54.5599696" lon="-3.2958555" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"/>
<missionitem no="5" action="WAYPOINT" lat="54.5537978" lon="-3.2958555" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"/>
<missionitem no="6" action="WAYPOINT" lat="54.5547933" lon="-3.2864141" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"/>
<missionitem no="7" action="WAYPOINT" lat="54.5597705" lon="-3.2695913" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"/>
<missionitem no="8" action="WAYPOINT" lat="54.555291" lon="-3.2598066" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"/>
<missionitem no="9" action="JUMP" lat="0" lon="0" alt="0" parameter1="1" parameter2="0" parameter3="0" flag="165"/>
<missionitem no="10" action="WAYPOINT" lat="54.5714148" lon="-3.2501936" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="165"/>
</mission>
```
### Multi-mission file with "reset" numbering and per-segment metadata
```
<?xml version="1.0" encoding="utf-8"?>
<mission>
<!--mw planner 0.01-->
<version value="42"></version>
<mwp save-date="2021-11-05T11:02:39+0000" zoom="14" cx="-3.2627249" cy="54.5710168" generator="mwp (mwptools)"><details><distance units="m" value="3130"></distance></details></mwp>
<missionitem no="1" action="WAYPOINT" lat="54.5722109" lon="-3.2869291" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
<missionitem no="2" action="WAYPOINT" lat="54.5708178" lon="-3.2642698" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
<missionitem no="3" action="WAYPOINT" lat="54.5698227" lon="-3.2385206" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="165"></missionitem>
<mwp save-date="2021-11-05T11:02:39+0000" zoom="15" cx="-3.2778311" cy="54.5568837" generator="mwp (mwptools)"><details><distance units="m" value="3324"></distance><nav-speed units="m/s" value="10"></nav-speed><fly-time units="s" value="344"></fly-time><loiter-time units="s" value="0"></loiter-time></details></mwp>
<missionitem no="1" action="WAYPOINT" lat="54.5599696" lon="-3.2958555" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
<missionitem no="2" action="WAYPOINT" lat="54.5537978" lon="-3.2958555" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
<missionitem no="3" action="WAYPOINT" lat="54.5547933" lon="-3.2864141" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
<missionitem no="4" action="WAYPOINT" lat="54.5597705" lon="-3.2695913" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
<missionitem no="5" action="WAYPOINT" lat="54.5552910" lon="-3.2598066" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="0"></missionitem>
<missionitem no="6" action="JUMP" lat="0.0000000" lon="0.0000000" alt="0" parameter1="1" parameter2="0" parameter3="0" flag="165"></missionitem>
<mwp save-date="2021-11-05T11:02:39+0000" zoom="20" cx="-3.2501936" cy="54.5714148" generator="mwp (mwptools)"><details><distance units="m" value="0"></distance></details></mwp>
<missionitem no="1" action="WAYPOINT" lat="54.5714148" lon="-3.2501936" alt="50" parameter1="0" parameter2="0" parameter3="0" flag="165"></missionitem>
</mission>
```

132
docs/development/wp_mission_schema/mw-mission.xsd Normal file
View file

@ -0,0 +1,132 @@
<?xml version="1.0" encoding="UTF-8"?>
<xs:schema xmlns:xs="http://www.w3.org/2001/XMLSchema"
elementFormDefault="qualified">
<!-- XSchema for MW / iNav missions
e.g. xmllint -​-format -​-schema mw-mission.xsd example.mission
-->
<xs:element name="mission">
<xs:complexType>
<xs:sequence>
<xs:element ref="version"/>
<xs:element ref="mwp" minOccurs="0" maxOccurs="unbounded"/>
<xs:element maxOccurs="unbounded" ref="missionitem"/>
</xs:sequence>
</xs:complexType>
</xs:element>
<xs:element name="version">
<xs:complexType>
<xs:attribute name="value" use="required"/>
</xs:complexType>
</xs:element>
<!-- mwp extension -->
<xs:element name="mwp">
<xs:complexType>
<xs:sequence>
<xs:element ref="details" minOccurs="0"/>
</xs:sequence>
<xs:attribute name="cx" type="xs:decimal"/>
<xs:attribute name="cy" type="xs:decimal"/>
<xs:attribute name="home-x" type="xs:decimal"/>
<xs:attribute name="home-y" type="xs:decimal"/>
<xs:attribute name="save-date" use="required" />
<xs:attribute name="generator" type="xs:string"/>
<xs:attribute name="zoom">
<xs:simpleType>
<xs:restriction base="xs:integer">
<xs:minInclusive value="0"/>
<xs:maxInclusive value="20"/>
</xs:restriction>
</xs:simpleType>
</xs:attribute>
</xs:complexType>
</xs:element>
<xs:element name="details">
<xs:complexType>
<xs:sequence>
<xs:element ref="distance" minOccurs="0"/>
<xs:element ref="nav-speed" minOccurs="0"/>
<xs:element ref="fly-time" minOccurs="0"/>
<xs:element ref="loiter-time" minOccurs="0"/>
</xs:sequence>
</xs:complexType>
</xs:element>
<xs:element name="distance">
<xs:complexType>
<xs:attribute name="units" use="required" type="xs:NCName"/>
<xs:attribute name="value" use="required" type="xs:decimal"/>
</xs:complexType>
</xs:element>
<xs:element name="nav-speed">
<xs:complexType>
<xs:attribute name="units" use="required"/>
<xs:attribute name="value" use="required" type="xs:decimal"/>
</xs:complexType>
</xs:element>
<xs:element name="fly-time">
<xs:complexType>
<xs:attribute name="units" use="required" type="xs:NCName"/>
<xs:attribute name="value" use="required" type="xs:integer"/>
</xs:complexType>
</xs:element>
<xs:element name="loiter-time">
<xs:complexType>
<xs:attribute name="units" use="required" type="xs:NCName"/>
<xs:attribute name="value" use="required" type="xs:integer"/>
</xs:complexType>
</xs:element>
<xs:element name="missionitem">
<xs:complexType>
<xs:attribute name="action" use="required">
<xs:simpleType>
<xs:restriction base="xs:token">
<xs:enumeration value="UNASSIGNED"/>
<xs:enumeration value="WAYPOINT"/>
<xs:enumeration value="POSHOLD_UNLIM"/>
<xs:enumeration value="POSHOLD_TIME"/>
<xs:enumeration value="RTH"/>
<xs:enumeration value="SET_POI"/>
<xs:enumeration value="JUMP"/>
<xs:enumeration value="SET_HEAD"/>
<xs:enumeration value="LAND"/>
</xs:restriction>
</xs:simpleType>
</xs:attribute>
<!-- no min,max as may be AMSL -->
<xs:attribute name="alt" use="required" type="xs:integer"/>
<!--
Locations are decimal degrees, WGS84 (EPSG:4326)
-->
<xs:attribute name="lat" use="required">
<xs:simpleType>
<xs:restriction base="xs:decimal">
<xs:minInclusive value="-90"/>
<xs:maxInclusive value="90"/>
</xs:restriction>
</xs:simpleType>
</xs:attribute>
<xs:attribute name="lon" use="required">
<xs:simpleType>
<xs:restriction base="xs:decimal">
<xs:minInclusive value="-180"/>
<xs:maxInclusive value="180"/>
</xs:restriction>
</xs:simpleType>
</xs:attribute>
<xs:attribute name="no" use="required" type="xs:integer"/>
<xs:attribute name="parameter1" use="required" type="xs:integer"/>
<xs:attribute name="parameter2" use="required" type="xs:integer"/>
<xs:attribute name="parameter3" type="xs:integer"/>
<!--
flag is not strictly required unless the WP is "Flyby Home"
or the are multiple mission segments 'multi-mission', inav 4.0
-->
<xs:attribute name="flag" use="optional" type="xs:integer"/>
</xs:complexType>
</xs:element>
<xs:element name="MISSION" substitutionGroup="mission"/>
<xs:element name="MISSIONITEM" substitutionGroup="missionitem"/>
<xs:element name="VERSION" substitutionGroup="version"/>
</xs:schema>

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

@ -1812,10 +1812,6 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz);
#endif #endif
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_notch_hz,
0);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_notch_cutoff,
1);
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", accelerometerConfig()->acc_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", accelerometerConfig()->acc_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware); BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware);
#ifdef USE_BARO #ifdef USE_BARO

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

@ -481,7 +481,12 @@ static void dumpPgValue(const setting_t *value, uint8_t dumpMask)
settingGetName(value, name); settingGetName(value, name);
if (dumpMask & SHOW_DEFAULTS && !equalsDefault) { if (dumpMask & SHOW_DEFAULTS && !equalsDefault) {
cliPrintf(defaultFormat, name); cliPrintf(defaultFormat, name);
printValuePointer(value, defaultValuePointer, 0); // if the craftname has a leading space, then enclose the name in quotes
if (strcmp(name, "name") == 0 && ((const char *)valuePointer)[0] == ' ') {
cliPrintf("\"%s\"", (const char *)valuePointer);
} else {
printValuePointer(value, valuePointer, 0);
}
cliPrintLinefeed(); cliPrintLinefeed();
} }
cliPrintf(format, name); cliPrintf(format, name);
@ -3109,7 +3114,13 @@ static void cliGet(char *cmdline)
val = settingGet(i); val = settingGet(i);
if (settingNameContains(val, name, cmdline)) { if (settingNameContains(val, name, cmdline)) {
cliPrintf("%s = ", name); cliPrintf("%s = ", name);
cliPrintVar(val, 0); if (strcmp(name, "name") == 0) {
// if the craftname has a leading space, then enclose the name in quotes
const char * v = (const char *)settingGetValuePointer(val);
cliPrintf(v[0] == ' ' ? "\"%s\"" : "%s", v);
} else {
cliPrintVar(val, 0);
}
cliPrintLinefeed(); cliPrintLinefeed();
cliPrintVarRange(val); cliPrintVarRange(val);
cliPrintLinefeed(); cliPrintLinefeed();
@ -3167,7 +3178,12 @@ static void cliSet(char *cmdline)
if (settingNameIsExactMatch(val, name, cmdline, variableNameLength)) { if (settingNameIsExactMatch(val, name, cmdline, variableNameLength)) {
const setting_type_e type = SETTING_TYPE(val); const setting_type_e type = SETTING_TYPE(val);
if (type == VAR_STRING) { if (type == VAR_STRING) {
settingSetString(val, eqptr, strlen(eqptr)); // if setting the craftname, remove any quotes around the name. This allows leading spaces in the name
if (strcmp(name, "name") == 0 && eqptr[0] == '"' && eqptr[strlen(eqptr)-1] == '"') {
settingSetString(val, eqptr + 1, strlen(eqptr)-2);
} else {
settingSetString(val, eqptr, strlen(eqptr));
}
return; return;
} }
const setting_mode_e mode = SETTING_MODE(val); const setting_mode_e mode = SETTING_MODE(val);

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

@ -183,9 +183,6 @@ uint32_t getGyroLooptime(void) {
void validateAndFixConfig(void) void validateAndFixConfig(void)
{ {
if (gyroConfig()->gyro_notch_cutoff >= gyroConfig()->gyro_notch_hz) {
gyroConfigMutable()->gyro_notch_hz = 0;
}
if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) { if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
accelerometerConfigMutable()->acc_notch_hz = 0; accelerometerConfigMutable()->acc_notch_hz = 0;
} }

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

@ -1238,8 +1238,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz); sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz);
sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); sbufWriteU16(dst, pidProfile()->dterm_lpf_hz);
sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);
sbufWriteU16(dst, gyroConfig()->gyro_notch_hz); sbufWriteU16(dst, 0); //Was gyroConfig()->gyro_notch_hz
sbufWriteU16(dst, gyroConfig()->gyro_notch_cutoff); sbufWriteU16(dst, 1); //Was gyroConfig()->gyro_notch_cutoff
sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz
sbufWriteU16(dst, 1); //pidProfile()->dterm_notch_cutoff sbufWriteU16(dst, 1); //pidProfile()->dterm_notch_cutoff
@ -2176,8 +2176,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 500); pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 500);
pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255); pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
if (dataSize >= 9) { if (dataSize >= 9) {
gyroConfigMutable()->gyro_notch_hz = constrain(sbufReadU16(src), 0, 500); sbufReadU16(src); //Was gyroConfigMutable()->gyro_notch_hz
gyroConfigMutable()->gyro_notch_cutoff = constrain(sbufReadU16(src), 1, 500); sbufReadU16(src); //Was gyroConfigMutable()->gyro_notch_cutoff
} else { } else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
} }

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

@ -235,15 +235,6 @@ groups:
default_value: 32 default_value: 32
field: gyroMovementCalibrationThreshold field: gyroMovementCalibrationThreshold
max: 128 max: 128
- name: gyro_notch_hz
field: gyro_notch_hz
max: 500
default_value: 0
- name: gyro_notch_cutoff
field: gyro_notch_cutoff
min: 1
max: 500
default_value: 1
- name: gyro_main_lpf_hz - name: gyro_main_lpf_hz
description: "Software based gyro main lowpass filter. Value is cutoff frequency (Hz)" description: "Software based gyro main lowpass filter. Value is cutoff frequency (Hz)"
default_value: 60 default_value: 60

7
src/main/flight/imu.c
View file

@ -61,12 +61,7 @@ FILE_COMPILE_FOR_SPEED
#include "sensors/sensors.h" #include "sensors/sensors.h"
/** /*
* In Cleanflight accelerometer is aligned in the following way:
* X-axis = Forward
* Y-axis = Left
* Z-axis = Up
* Our INAV uses different convention
* X-axis = North/Forward * X-axis = North/Forward
* Y-axis = East/Right * Y-axis = East/Right
* Z-axis = Up * Z-axis = Up

4
src/main/io/osd_dji_hd.c
View file

@ -1428,8 +1428,8 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz
sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); // BF: currentPidProfile->dterm_lowpass_hz sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); // BF: currentPidProfile->dterm_lowpass_hz
sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); // BF: currentPidProfile->yaw_lowpass_hz sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); // BF: currentPidProfile->yaw_lowpass_hz
sbufWriteU16(dst, gyroConfig()->gyro_notch_hz); // BF: gyroConfig()->gyro_soft_notch_hz_1 sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_1
sbufWriteU16(dst, gyroConfig()->gyro_notch_cutoff); // BF: gyroConfig()->gyro_soft_notch_cutoff_1 sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_1
sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_notch_hz sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_notch_hz
sbufWriteU16(dst, 1); // BF: currentPidProfile->dterm_notch_cutoff sbufWriteU16(dst, 1); // BF: currentPidProfile->dterm_notch_cutoff
sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_2 sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_2

7
src/main/msc/emfat.c
View file

@ -388,8 +388,8 @@ void read_boot_sector(const emfat_t *emfat, uint8_t *sect)
bs->volume_id[1] = 14; bs->volume_id[1] = 14;
bs->volume_id[2] = 13; bs->volume_id[2] = 13;
bs->volume_id[3] = 8; bs->volume_id[3] = 8;
memcpy(bs->volume_label, "NO NAME ", 12); memcpy(bs->volume_label, "NO NAME ", VOL_LABEL_LEN);
memcpy(bs->file_system_type, "FAT32 ", 8); memcpy(bs->file_system_type, "FAT32 ", FILE_SYS_TYPE_LENGTH);
sect[SECT - 2] = 0x55; sect[SECT - 2] = 0x55;
sect[SECT - 1] = 0xAA; sect[SECT - 1] = 0xAA;
} }
@ -525,8 +525,9 @@ void fill_entry(dir_entry *entry, const char *name, uint8_t attr, uint32_t clust
l2 = l2 > FILE_NAME_EXTN_LEN ? FILE_NAME_EXTN_LEN : l2; l2 = l2 > FILE_NAME_EXTN_LEN ? FILE_NAME_EXTN_LEN : l2;
} }
memset(entry->name, ' ', FILE_NAME_SHRT_LEN + FILE_NAME_EXTN_LEN); memset(entry->name, ' ', FILE_NAME_SHRT_LEN);
memcpy(entry->name, name, l1); memcpy(entry->name, name, l1);
memset(entry->extn, ' ', FILE_NAME_EXTN_LEN);
memcpy(entry->extn, name + dot_pos + 1, l2); memcpy(entry->extn, name + dot_pos + 1, l2);
for (i = 0; i < FILE_NAME_SHRT_LEN; i++) { for (i = 0; i < FILE_NAME_SHRT_LEN; i++) {

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

@ -782,7 +782,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.mwState = MW_NAV_STATE_WP_ENROUTE, .mwState = MW_NAV_STATE_WP_ENROUTE,
.mwError = MW_NAV_ERROR_FINISH, .mwError = MW_NAV_ERROR_FINISH,
.onEvent = { .onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_FINISHED, [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_FINISHED, // re-process state
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
@ -1011,7 +1011,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
DEBUG_SET(DEBUG_CRUISE, 0, 1); DEBUG_SET(DEBUG_CRUISE, 0, 1);
if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Try the next iteration. if (checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
} // Switch to IDLE if we do not have an healty position. Try the next iteration.
if (!(prevFlags & NAV_CTL_POS)) { if (!(prevFlags & NAV_CTL_POS)) {
resetPositionController(); resetPositionController();
@ -1028,7 +1030,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
{ {
const timeMs_t currentTimeMs = millis(); const timeMs_t currentTimeMs = millis();
if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration. if (checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
} // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
DEBUG_SET(DEBUG_CRUISE, 0, 2); DEBUG_SET(DEBUG_CRUISE, 0, 2);
DEBUG_SET(DEBUG_CRUISE, 2, 0); DEBUG_SET(DEBUG_CRUISE, 2, 0);
@ -1114,7 +1118,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) { if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
// Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
// Relevant to failsafe forced RTH only. Switched RTH blocked in selectNavEventFromBoxModeInput if sensors unavailanle. // Relevant to failsafe forced RTH only. Switched RTH blocked in selectNavEventFromBoxModeInput if sensors unavailable.
// If we are in dead-reckoning mode - also fail, since coordinates may be unreliable // If we are in dead-reckoning mode - also fail, since coordinates may be unreliable
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
@ -1175,90 +1179,82 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
/* No valid POS sensor but still within valid timeout - wait */ /* No valid POS sensor but still within valid timeout - wait */
else { return NAV_FSM_EVENT_NONE;
return NAV_FSM_EVENT_NONE;
}
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState)
{ {
UNUSED(previousState); UNUSED(previousState);
rthAltControlStickOverrideCheck(PITCH);
if ((posControl.flags.estHeadingStatus == EST_NONE)) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
if (!STATE(ALTITUDE_CONTROL)) { if (!STATE(ALTITUDE_CONTROL)) {
//If altitude control is not a thing, switch to RTH in progress instead //If altitude control is not a thing, switch to RTH in progress instead
return NAV_FSM_EVENT_SUCCESS; //Will cause NAV_STATE_RTH_HEAD_HOME return NAV_FSM_EVENT_SUCCESS; //Will cause NAV_STATE_RTH_HEAD_HOME
} }
// If we have valid pos sensor OR we are configured to ignore GPS loss rthAltControlStickOverrideCheck(PITCH);
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
// If we reached desired initial RTH altitude or we don't want to climb first /* Position sensor failure timeout and not configured to ignore GPS loss - land */
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF) || rthAltControlStickOverrideCheck(ROLL)) { if ((posControl.flags.estHeadingStatus == EST_NONE) ||
(checkForPositionSensorTimeout() && !navConfig()->general.flags.rth_climb_ignore_emerg)) {
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
if (STATE(FIXED_WING_LEGACY)) {
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
}
// Save initial home distance for future use
posControl.rthState.rthInitialDistance = posControl.homeDistance;
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
}
else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
}
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME
} else {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
/* For multi-rotors execute sanity check during initial ascent as well */
if (!STATE(FIXED_WING_LEGACY) && !validateRTHSanityChecker()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
// Climb to safe altitude and turn to correct direction
if (STATE(FIXED_WING_LEGACY)) {
if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) {
float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
} else {
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
}
} else {
// Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
// it in a reasonable time. Immediately after we finish this phase - target the original altitude.
tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM;
if (navConfig()->general.flags.rth_tail_first) {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
} else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
}
}
return NAV_FSM_EVENT_NONE;
}
}
/* Position sensor failure timeout - land */
else {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
// If we reached desired initial RTH altitude or we don't want to climb first
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF) || rthAltControlStickOverrideCheck(ROLL)) {
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
if (STATE(FIXED_WING_LEGACY)) {
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
}
// Save initial home distance for future use
posControl.rthState.rthInitialDistance = posControl.homeDistance;
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
}
else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
}
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME
} else {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
/* For multi-rotors execute sanity check during initial ascent as well */
if (!STATE(FIXED_WING_LEGACY) && !validateRTHSanityChecker()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
// Climb to safe altitude and turn to correct direction
if (STATE(FIXED_WING_LEGACY)) {
if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) {
float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
} else {
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
}
} else {
// Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
// it in a reasonable time. Immediately after we finish this phase - target the original altitude.
tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM;
if (navConfig()->general.flags.rth_tail_first) {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
} else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
}
}
return NAV_FSM_EVENT_NONE;
}
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState)
@ -1267,7 +1263,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
rthAltControlStickOverrideCheck(PITCH); rthAltControlStickOverrideCheck(PITCH);
if ((posControl.flags.estHeadingStatus == EST_NONE)) { /* If position sensors unavailable - land immediately */
if ((posControl.flags.estHeadingStatus == EST_NONE) || !validateRTHSanityChecker()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
@ -1279,12 +1276,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
// Successfully reached position target - update XYZ-position // Successfully reached position target - update XYZ-position
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
} } else {
else if (!validateRTHSanityChecker()) {
// Sanity check of RTH
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY); setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
@ -1294,52 +1286,44 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
/* No valid POS sensor but still within valid timeout - wait */ /* No valid POS sensor but still within valid timeout - wait */
else { return NAV_FSM_EVENT_NONE;
return NAV_FSM_EVENT_NONE;
}
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState)
{ {
UNUSED(previousState); UNUSED(previousState);
if ((posControl.flags.estHeadingStatus == EST_NONE)) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
//On ROVER and BOAT we immediately switch to the next event //On ROVER and BOAT we immediately switch to the next event
if (!STATE(ALTITUDE_CONTROL)) { if (!STATE(ALTITUDE_CONTROL)) {
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} }
// If position ok OR within valid timeout - continue /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) { if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
// Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
resetLandingDetector();
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
}
else if (!validateRTHSanityChecker()) {
// Continue to check for RTH sanity during pre-landing hover
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
else {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE;
}
} else {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
// If position ok OR within valid timeout - continue
// Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
resetLandingDetector();
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
} else {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE;
}
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState)
{ {
UNUSED(previousState); UNUSED(previousState);
if (posControl.flags.estHeadingStatus == EST_NONE || !(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout())) /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER); fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
@ -1370,42 +1354,35 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} }
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED) || isLandingDetected()) {
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} }
else if (isLandingDetected()) {
return NAV_FSM_EVENT_SUCCESS; /* If position sensors unavailable - land immediately (wait for timeout on GPS)
* Continue to check for RTH sanity during landing */
if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
else {
/* If position sensors unavailable - land immediately (wait for timeout on GPS)
* Continue to check for RTH sanity during landing */
if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() ||!validateRTHSanityChecker()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
float descentVelLimited = 0; float descentVelLimited = 0;
// A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed // A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
// land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown
// Do not allow descent velocity slower than -30cm/s so the landing detector works. // Do not allow descent velocity slower than -30cm/s so the landing detector works.
descentVelLimited = navConfig()->general.land_minalt_vspd; descentVelLimited = navConfig()->general.land_minalt_vspd;
} } else {
else { // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z,
navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt,
navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm. descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z,
navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt,
navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
}
updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL);
return NAV_FSM_EVENT_NONE;
} }
updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL);
return NAV_FSM_EVENT_NONE;
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState)
@ -1588,11 +1565,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
else if (checkForPositionSensorTimeout() || (posControl.flags.estHeadingStatus == EST_NONE)) { else if (checkForPositionSensorTimeout() || (posControl.flags.estHeadingStatus == EST_NONE)) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
else {
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
}
UNREACHABLE(); return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState)
@ -1632,11 +1606,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
timeMs_t currentTime = millis(); timeMs_t currentTime = millis();
if(posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0) if (posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0 ||
(posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)) {
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
}
if(posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)
return NAV_FSM_EVENT_SUCCESS;
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
} }
@ -1681,17 +1655,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
clearJumpCounters(); clearJumpCounters();
posControl.wpMissionRestart = true; posControl.wpMissionRestart = true;
// If no position sensor available - land immediately /* If position sensors unavailable - land immediately (wait for timeout on GPS) */
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) { if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_NONE;
}
/* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
else if (checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
else {
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
}
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState)
@ -1959,21 +1928,21 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali
if (estPosValid && estVelValid) { if (estPosValid && estVelValid) {
posControl.flags.estPosStatus = EST_TRUSTED; posControl.flags.estPosStatus = EST_TRUSTED;
posControl.flags.estVelStatus = EST_TRUSTED; posControl.flags.estVelStatus = EST_TRUSTED;
posControl.flags.horizontalPositionDataNew = 1; posControl.flags.horizontalPositionDataNew = true;
posControl.lastValidPositionTimeMs = millis(); posControl.lastValidPositionTimeMs = millis();
} }
// CASE 1: POS invalid, VEL valid // CASE 1: POS invalid, VEL valid
else if (!estPosValid && estVelValid) { else if (!estPosValid && estVelValid) {
posControl.flags.estPosStatus = EST_USABLE; // Pos usable, but not trusted posControl.flags.estPosStatus = EST_USABLE; // Pos usable, but not trusted
posControl.flags.estVelStatus = EST_TRUSTED; posControl.flags.estVelStatus = EST_TRUSTED;
posControl.flags.horizontalPositionDataNew = 1; posControl.flags.horizontalPositionDataNew = true;
posControl.lastValidPositionTimeMs = millis(); posControl.lastValidPositionTimeMs = millis();
} }
// CASE 3: can't use pos/vel data // CASE 3: can't use pos/vel data
else { else {
posControl.flags.estPosStatus = EST_NONE; posControl.flags.estPosStatus = EST_NONE;
posControl.flags.estVelStatus = EST_NONE; posControl.flags.estVelStatus = EST_NONE;
posControl.flags.horizontalPositionDataNew = 0; posControl.flags.horizontalPositionDataNew = false;
} }
//Update blackbox data //Update blackbox data
@ -2007,13 +1976,13 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
posControl.flags.estAglStatus = surfaceStatus; // Could be TRUSTED or USABLE posControl.flags.estAglStatus = surfaceStatus; // Could be TRUSTED or USABLE
posControl.flags.estAltStatus = EST_TRUSTED; posControl.flags.estAltStatus = EST_TRUSTED;
posControl.flags.verticalPositionDataNew = 1; posControl.flags.verticalPositionDataNew = true;
posControl.lastValidAltitudeTimeMs = millis(); posControl.lastValidAltitudeTimeMs = millis();
} }
else { else {
posControl.flags.estAltStatus = EST_NONE; posControl.flags.estAltStatus = EST_NONE;
posControl.flags.estAglStatus = EST_NONE; posControl.flags.estAglStatus = EST_NONE;
posControl.flags.verticalPositionDataNew = 0; posControl.flags.verticalPositionDataNew = false;
} }
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
@ -2068,8 +2037,6 @@ void updateActualHeading(bool headingValid, int32_t newHeading)
/* Precompute sin/cos of yaw angle */ /* Precompute sin/cos of yaw angle */
posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(newHeading)); posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(newHeading));
posControl.actualState.cosYaw = cos_approx(CENTIDEGREES_TO_RADIANS(newHeading)); posControl.actualState.cosYaw = cos_approx(CENTIDEGREES_TO_RADIANS(newHeading));
posControl.flags.headingDataNew = 1;
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------
@ -3133,10 +3100,10 @@ void applyWaypointNavigationAndAltitudeHold(void)
/* Consume position data */ /* Consume position data */
if (posControl.flags.horizontalPositionDataConsumed) if (posControl.flags.horizontalPositionDataConsumed)
posControl.flags.horizontalPositionDataNew = 0; posControl.flags.horizontalPositionDataNew = false;
if (posControl.flags.verticalPositionDataConsumed) if (posControl.flags.verticalPositionDataConsumed)
posControl.flags.verticalPositionDataNew = 0; posControl.flags.verticalPositionDataNew = false;
//Update blackbox data //Update blackbox data
if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 6); if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 6);
@ -3260,10 +3227,10 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
// Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection) // Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection)
if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !IS_RC_MODE_ACTIVE(BOXMANUAL))) { if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !IS_RC_MODE_ACTIVE(BOXMANUAL))) {
// Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss // Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss
// If don't keep this, loss of any of the canActivatePosHold && canActivateNavigation && canActivateAltHold // If don't keep this, loss of any of the canActivateNavigation && canActivateAltHold
// will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back // will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back
// logic to kick in (waiting for GPS on airplanes, switch to emergency landing etc) // logic to kick in (waiting for GPS on airplanes, switch to emergency landing etc)
if (isExecutingRTH || (canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
return NAV_FSM_EVENT_SWITCH_TO_RTH; return NAV_FSM_EVENT_SWITCH_TO_RTH;
} }
} }
@ -3276,7 +3243,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
// Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded // Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
if (IS_RC_MODE_ACTIVE(BOXNAVWP)) { if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
} }
else { else {
@ -3607,9 +3574,8 @@ void navigationInit(void)
/* Initial state */ /* Initial state */
posControl.navState = NAV_STATE_IDLE; posControl.navState = NAV_STATE_IDLE;
posControl.flags.horizontalPositionDataNew = 0; posControl.flags.horizontalPositionDataNew = false;
posControl.flags.verticalPositionDataNew = 0; posControl.flags.verticalPositionDataNew = false;
posControl.flags.headingDataNew = 0;
posControl.flags.estAltStatus = EST_NONE; posControl.flags.estAltStatus = EST_NONE;
posControl.flags.estPosStatus = EST_NONE; posControl.flags.estPosStatus = EST_NONE;

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

@ -57,7 +57,7 @@
#define UNUSED(x) ((void)(x)) #define UNUSED(x) ((void)(x))
#define FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE "RAISE THE THROTTLE" #define FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE "RAISE THE THROTTLE"
#define FW_LAUNCH_MESSAGE_TEXT_WAIT_IDLE "WAITING FOR IDLE" #define FW_LAUNCH_MESSAGE_TEXT_WAIT_IDLE "WAITING FOR IDLE"
#define FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION "READY" #define FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION "READY TO LAUNCH"
#define FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS "MOVE THE STICKS TO ABORT" #define FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS "MOVE THE STICKS TO ABORT"
#define FW_LAUNCH_MESSAGE_TEXT_FINISHING "FINISHING" #define FW_LAUNCH_MESSAGE_TEXT_FINISHING "FINISHING"

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

@ -71,7 +71,6 @@ typedef enum {
typedef struct navigationFlags_s { typedef struct navigationFlags_s {
bool horizontalPositionDataNew; bool horizontalPositionDataNew;
bool verticalPositionDataNew; bool verticalPositionDataNew;
bool headingDataNew;
bool horizontalPositionDataConsumed; bool horizontalPositionDataConsumed;
bool verticalPositionDataConsumed; bool verticalPositionDataConsumed;

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

@ -93,9 +93,6 @@ STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT];
STATIC_FASTRAM filterApplyFnPtr gyroLpf2ApplyFn; STATIC_FASTRAM filterApplyFnPtr gyroLpf2ApplyFn;
STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT]; STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT];
STATIC_FASTRAM filterApplyFnPtr notchFilter1ApplyFn;
STATIC_FASTRAM void *notchFilter1[XYZ_AXIS_COUNT];
#ifdef USE_DYNAMIC_FILTERS #ifdef USE_DYNAMIC_FILTERS
EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState; EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState;
@ -103,7 +100,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
#endif #endif
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
@ -115,8 +112,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
#ifdef USE_DUAL_GYRO #ifdef USE_DUAL_GYRO
.gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT, .gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT,
#endif #endif
.gyro_notch_hz = SETTING_GYRO_NOTCH_HZ_DEFAULT,
.gyro_notch_cutoff = SETTING_GYRO_NOTCH_CUTOFF_DEFAULT,
.gyro_main_lpf_hz = SETTING_GYRO_MAIN_LPF_HZ_DEFAULT, .gyro_main_lpf_hz = SETTING_GYRO_MAIN_LPF_HZ_DEFAULT,
.gyro_main_lpf_type = SETTING_GYRO_MAIN_LPF_TYPE_DEFAULT, .gyro_main_lpf_type = SETTING_GYRO_MAIN_LPF_TYPE_DEFAULT,
.useDynamicLpf = SETTING_GYRO_USE_DYN_LPF_DEFAULT, .useDynamicLpf = SETTING_GYRO_USE_DYN_LPF_DEFAULT,
@ -282,24 +277,12 @@ static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t
static void gyroInitFilters(void) static void gyroInitFilters(void)
{ {
STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
notchFilter1ApplyFn = nullFilterApply;
//First gyro LPF running at full gyro frequency 8kHz //First gyro LPF running at full gyro frequency 8kHz
initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_type, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime()); initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_type, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime());
//Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON //Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON
initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_type, gyroConfig()->gyro_main_lpf_hz, getLooptime()); initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_type, gyroConfig()->gyro_main_lpf_hz, getLooptime());
//Static Gyro notch running and PID frequency
if (gyroConfig()->gyro_notch_hz) {
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) {
notchFilter1[axis] = &gyroFilterNotch_1[axis];
biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_notch_hz, gyroConfig()->gyro_notch_cutoff);
}
}
#ifdef USE_GYRO_KALMAN #ifdef USE_GYRO_KALMAN
if (gyroConfig()->kalmanEnabled) { if (gyroConfig()->kalmanEnabled) {
gyroKalmanInitialize(gyroConfig()->kalman_q); gyroKalmanInitialize(gyroConfig()->kalman_q);
@ -470,7 +453,6 @@ void FAST_CODE NOINLINE gyroFilter()
#endif #endif
gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf); gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf);
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
#ifdef USE_DYNAMIC_FILTERS #ifdef USE_DYNAMIC_FILTERS
if (dynamicGyroNotchState.enabled) { if (dynamicGyroNotchState.enabled) {

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

@ -59,8 +59,6 @@ typedef struct gyroConfig_s {
#ifdef USE_DUAL_GYRO #ifdef USE_DUAL_GYRO
uint8_t gyro_to_use; uint8_t gyro_to_use;
#endif #endif
uint16_t gyro_notch_hz;
uint16_t gyro_notch_cutoff;
uint16_t gyro_main_lpf_hz; uint16_t gyro_main_lpf_hz;
uint8_t gyro_main_lpf_type; uint8_t gyro_main_lpf_type;
uint8_t useDynamicLpf; uint8_t useDynamicLpf;

2
src/main/target/FALCORE/config.c
View file

@ -61,8 +61,6 @@ void targetConfiguration(void)
gyroConfigMutable()->looptime = 1000; gyroConfigMutable()->looptime = 1000;
gyroConfigMutable()->gyro_lpf = 0; // 256 Hz gyroConfigMutable()->gyro_lpf = 0; // 256 Hz
gyroConfigMutable()->gyro_main_lpf_hz = 90; gyroConfigMutable()->gyro_main_lpf_hz = 90;
gyroConfigMutable()->gyro_notch_hz = 150;
gyroConfigMutable()->gyro_notch_cutoff = 80;
accelerometerConfigMutable()->acc_hardware = ACC_MPU6500; accelerometerConfigMutable()->acc_hardware = ACC_MPU6500;
accelerometerConfigMutable()->acc_lpf_hz = 15; accelerometerConfigMutable()->acc_lpf_hz = 15;

5
src/test/unit/CMakeLists.txt
View file

@ -9,6 +9,11 @@ set_property(SOURCE alignsensor_unittest.cc PROPERTY depends
set_property(SOURCE bitarray_unittest.cc PROPERTY depends "common/bitarray.c") set_property(SOURCE bitarray_unittest.cc PROPERTY depends "common/bitarray.c")
set_property(SOURCE flight_imu_unittest.cc PROPERTY depends "build/debug.c"
"common/maths.c" "common/calibration.c" "common/filter.c"
"drivers/accgyro/accgyro_fake.c" "flight/imu.c" "sensors/boardalignment.c"
"sensors/gyro.c")
set_property(SOURCE maths_unittest.cc PROPERTY depends "common/maths.c") set_property(SOURCE maths_unittest.cc PROPERTY depends "common/maths.c")
set_property(SOURCE olc_unittest.cc PROPERTY depends "common/olc.c") set_property(SOURCE olc_unittest.cc PROPERTY depends "common/olc.c")

140
src/test/unit/flight_imu_unittest.cc Normal file
View file

@ -0,0 +1,140 @@
/*
* 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 <limits.h>
extern "C" {
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "scheduler/scheduler.h"
#include "fc/runtime_config.h"
#include "io/gps.h"
#include "flight/imu.h"
}
#include "unittest_macros.h"
#include "gtest/gtest.h"
extern "C" {
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void);
STATIC_UNIT_TESTED void imuComputeQuaternionFromRPY(int16_t initialRoll, int16_t initialPitch, int16_t initialYaw);
}
TEST(FlightImuTest, TestEulerAngleCalculation)
{
imuComputeQuaternionFromRPY(0, 0, 0);
imuUpdateEulerAngles();
EXPECT_NEAR(attitude.values.roll, 0, 1);
EXPECT_NEAR(attitude.values.pitch, 0, 1);
EXPECT_NEAR(attitude.values.yaw, 0, 1);
imuComputeQuaternionFromRPY(450, 450, 0);
imuUpdateEulerAngles();
EXPECT_NEAR(attitude.values.roll, 450, 1);
EXPECT_NEAR(attitude.values.pitch, 450, 1);
EXPECT_NEAR(attitude.values.yaw, 0, 1);
imuComputeQuaternionFromRPY(-450, -450, 0);
imuUpdateEulerAngles();
EXPECT_NEAR(attitude.values.roll, -450, 1);
EXPECT_NEAR(attitude.values.pitch, -450, 1);
EXPECT_NEAR(attitude.values.yaw, 0, 1);
imuComputeQuaternionFromRPY(1790, 0, 0);
imuUpdateEulerAngles();
EXPECT_NEAR(attitude.values.roll, 1790, 1);
EXPECT_NEAR(attitude.values.pitch, 0, 1);
EXPECT_NEAR(attitude.values.yaw, 0, 1);
imuComputeQuaternionFromRPY(-1790, 0, 0);
imuUpdateEulerAngles();
EXPECT_NEAR(attitude.values.roll, -1790, 1);
EXPECT_NEAR(attitude.values.pitch, 0, 1);
EXPECT_NEAR(attitude.values.yaw, 0, 1);
imuComputeQuaternionFromRPY(0, 0, 900);
imuUpdateEulerAngles();
EXPECT_NEAR(attitude.values.roll, 0, 1);
EXPECT_NEAR(attitude.values.pitch, 0, 1);
EXPECT_NEAR(attitude.values.yaw, 900, 1);
imuComputeQuaternionFromRPY(0, 0, 2700);
imuUpdateEulerAngles();
EXPECT_NEAR(attitude.values.roll, 0, 1);
EXPECT_NEAR(attitude.values.pitch, 0, 1);
EXPECT_NEAR(attitude.values.yaw, 2700, 1);
}
// STUBS
extern "C" {
uint32_t stateFlags;
uint32_t flightModeFlags;
uint32_t armingFlags;
acc_t acc;
mag_t mag;
gpsSolutionData_t gpsSol;
compassConfig_t compassConfig_System;
uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE };
bool sensors(uint32_t mask)
{
UNUSED(mask);
return false;
};
uint32_t millis(void) { return 0; }
timeDelta_t getLooptime(void) { return gyro.targetLooptime; }
timeDelta_t getGyroLooptime(void) { return gyro.targetLooptime; }
void schedulerResetTaskStatistics(cfTaskId_e) {}
void sensorsSet(uint32_t) {}
bool compassIsHealthy(void) { return true; }
void accGetVibrationLevels(fpVector3_t *accVibeLevels)
{
accVibeLevels->x = fast_fsqrtf(acc.accVibeSq[X]);
accVibeLevels->y = fast_fsqrtf(acc.accVibeSq[Y]);
accVibeLevels->z = fast_fsqrtf(acc.accVibeSq[Z]);
}
void accGetMeasuredAcceleration(fpVector3_t *measuredAcc)
{
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
}
}
uint32_t accGetClipCount(void)
{
return acc.accClipCount;
}
void accUpdate(void)
{
}
void resetHeadingHoldTarget(int16_t heading)
{
UNUSED(heading);
}
bool isGPSHeadingValid(void) { return true; }
}

182
src/test/unit/flight_imu_unittest.cc.txt
View file

@ -1,182 +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/>.
*/
#include <stdint.h>
#include <stdbool.h>
#include <limits.h>
#define USE_BARO
extern "C" {
#include "debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "sensors/sensors.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "config/runtime_config.h"
#include "rx/rx.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
}
#include "unittest_macros.h"
#include "gtest/gtest.h"
extern float q0, q1, q2, q3;
extern "C" {
void imuComputeRotationMatrix(void);
void imuUpdateEulerAngles(void);
}
void imuComputeQuaternionFromRPY(int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
{
if (initialRoll > 1800) initialRoll -= 3600;
if (initialPitch > 1800) initialPitch -= 3600;
if (initialYaw > 1800) initialYaw -= 3600;
float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
imuComputeRotationMatrix();
}
TEST(FlightImuTest, TestEulerAngleCalculation)
{
imuComputeQuaternionFromRPY(0, 0, 0);
imuUpdateEulerAngles();
EXPECT_FLOAT_EQ(attitude.values.roll, 0);
EXPECT_FLOAT_EQ(attitude.values.pitch, 0);
EXPECT_FLOAT_EQ(attitude.values.yaw, 0);
imuComputeQuaternionFromRPY(450, 450, 0);
imuUpdateEulerAngles();
EXPECT_FLOAT_EQ(attitude.values.roll, 450);
EXPECT_FLOAT_EQ(attitude.values.pitch, 450);
EXPECT_FLOAT_EQ(attitude.values.yaw, 0);
imuComputeQuaternionFromRPY(-450, -450, 0);
imuUpdateEulerAngles();
EXPECT_FLOAT_EQ(attitude.values.roll, -450);
EXPECT_FLOAT_EQ(attitude.values.pitch, -450);
EXPECT_FLOAT_EQ(attitude.values.yaw, 0);
imuComputeQuaternionFromRPY(1790, 0, 0);
imuUpdateEulerAngles();
EXPECT_FLOAT_EQ(attitude.values.roll, 1790);
EXPECT_FLOAT_EQ(attitude.values.pitch, 0);
EXPECT_FLOAT_EQ(attitude.values.yaw, 0);
imuComputeQuaternionFromRPY(-1790, 0, 0);
imuUpdateEulerAngles();
EXPECT_FLOAT_EQ(attitude.values.roll, -1790);
EXPECT_FLOAT_EQ(attitude.values.pitch, 0);
EXPECT_FLOAT_EQ(attitude.values.yaw, 0);
imuComputeQuaternionFromRPY(0, 0, 900);
imuUpdateEulerAngles();
EXPECT_FLOAT_EQ(attitude.values.roll, 0);
EXPECT_FLOAT_EQ(attitude.values.pitch, 0);
EXPECT_FLOAT_EQ(attitude.values.yaw, 900);
imuComputeQuaternionFromRPY(0, 0, 2700);
imuUpdateEulerAngles();
EXPECT_FLOAT_EQ(attitude.values.roll, 0);
EXPECT_FLOAT_EQ(attitude.values.pitch, 0);
EXPECT_FLOAT_EQ(attitude.values.yaw, 2700);
}
// STUBS
extern "C" {
uint32_t rcModeActivationMask;
int16_t rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint16_t acc_1G;
int16_t heading;
gyro_t gyro;
int32_t magADC[XYZ_AXIS_COUNT];
int32_t BaroAlt;
int32_t debug[DEBUG32_VALUE_COUNT];
uint8_t stateFlags;
uint16_t flightModeFlags;
uint8_t armingFlags;
int32_t sonarAlt;
int16_t sonarCfAltCm;
int32_t accADC[XYZ_AXIS_COUNT];
int32_t gyroADC[XYZ_AXIS_COUNT];
int16_t GPS_speed;
int16_t GPS_ground_course;
int16_t GPS_numSat;
int16_t cycleTime = 2000;
uint16_t enableFlightMode(flightModeFlags_e mask)
{
return flightModeFlags |= (mask);
}
uint16_t disableFlightMode(flightModeFlags_e mask)
{
return flightModeFlags &= ~(mask);
}
void gyroUpdate(void) {};
bool sensors(uint32_t mask)
{
UNUSED(mask);
return false;
};
void updateAccelerationReadings(void)
{
}
bool isGyroCalibrationComplete(void) { return 1; }
bool isCompassReady(void) { return 1; }
uint32_t micros(void) { return 0; }
uint32_t millis(void) { return 0; }
bool isBaroCalibrationComplete(void) { return true; }
void performBaroCalibrationCycle(void) {}
int32_t baroCalculateAltitude(void) { return 0; }
}