mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 16:55:29 +03:00
Merge branch 'master' into abo_mission_restart_option
This commit is contained in:
breadoven
GitHub
commit
0b31a10ee9
22 changed files with 531 additions and 431 deletions
|
|
@ -5,7 +5,7 @@ A profile is a set of configuration settings.
|
|||
Currently three profiles are supported. The default profile is profile `1`.
|
||||
|
||||
## Changing profiles
|
||||
|
||||
### Stick Commands
|
||||
Profiles can be selected using a GUI or the following stick combinations:
|
||||
|
||||
| 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 |
|
||||
| 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>
|
||||
```
|
||||
|
||||
### 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.
|
||||
|
||||
[](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
|
||||
|
||||
[](https://i.imgur.com/WjkuhhW.png)
|
||||
|
||||
## Profile Contents
|
||||
The values contained within a profile can be seen by using the CLI `dump profile` command.
|
||||
|
||||
e.g
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
_// TODO_
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
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).
|
||||
|
||||
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:
|
||||
|
||||
* [Omnibus F4 v5](https://inavflight.com/shop/p/OMNIBUSF4V5)
|
||||
|
|
|
|||
59
docs/development/wp_mission_schema/README.md
Normal file
59
docs/development/wp_mission_schema/README.md
Normal 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
132
docs/development/wp_mission_schema/mw-mission.xsd
Normal 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>
|
||||
|
|
@ -1812,10 +1812,6 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz);
|
||||
#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_hardware", "%d", accelerometerConfig()->acc_hardware);
|
||||
#ifdef USE_BARO
|
||||
|
|
|
|||
|
|
@ -481,7 +481,12 @@ static void dumpPgValue(const setting_t *value, uint8_t dumpMask)
|
|||
settingGetName(value, name);
|
||||
if (dumpMask & SHOW_DEFAULTS && !equalsDefault) {
|
||||
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();
|
||||
}
|
||||
cliPrintf(format, name);
|
||||
|
|
@ -3109,7 +3114,13 @@ static void cliGet(char *cmdline)
|
|||
val = settingGet(i);
|
||||
if (settingNameContains(val, name, cmdline)) {
|
||||
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();
|
||||
cliPrintVarRange(val);
|
||||
cliPrintLinefeed();
|
||||
|
|
@ -3167,7 +3178,12 @@ static void cliSet(char *cmdline)
|
|||
if (settingNameIsExactMatch(val, name, cmdline, variableNameLength)) {
|
||||
const setting_type_e type = SETTING_TYPE(val);
|
||||
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;
|
||||
}
|
||||
const setting_mode_e mode = SETTING_MODE(val);
|
||||
|
|
|
|||
|
|
@ -183,9 +183,6 @@ uint32_t getGyroLooptime(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) {
|
||||
accelerometerConfigMutable()->acc_notch_hz = 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1238,8 +1238,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz);
|
||||
sbufWriteU16(dst, pidProfile()->dterm_lpf_hz);
|
||||
sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);
|
||||
sbufWriteU16(dst, gyroConfig()->gyro_notch_hz);
|
||||
sbufWriteU16(dst, gyroConfig()->gyro_notch_cutoff);
|
||||
sbufWriteU16(dst, 0); //Was gyroConfig()->gyro_notch_hz
|
||||
sbufWriteU16(dst, 1); //Was gyroConfig()->gyro_notch_cutoff
|
||||
sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz
|
||||
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()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
|
||||
if (dataSize >= 9) {
|
||||
gyroConfigMutable()->gyro_notch_hz = constrain(sbufReadU16(src), 0, 500);
|
||||
gyroConfigMutable()->gyro_notch_cutoff = constrain(sbufReadU16(src), 1, 500);
|
||||
sbufReadU16(src); //Was gyroConfigMutable()->gyro_notch_hz
|
||||
sbufReadU16(src); //Was gyroConfigMutable()->gyro_notch_cutoff
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -235,15 +235,6 @@ groups:
|
|||
default_value: 32
|
||||
field: gyroMovementCalibrationThreshold
|
||||
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
|
||||
description: "Software based gyro main lowpass filter. Value is cutoff frequency (Hz)"
|
||||
default_value: 60
|
||||
|
|
|
|||
|
|
@ -61,12 +61,7 @@ FILE_COMPILE_FOR_SPEED
|
|||
#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
|
||||
* Y-axis = East/Right
|
||||
* Z-axis = Up
|
||||
|
|
|
|||
|
|
@ -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
|
||||
sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); // BF: currentPidProfile->dterm_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, gyroConfig()->gyro_notch_cutoff); // BF: gyroConfig()->gyro_soft_notch_cutoff_1
|
||||
sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_1
|
||||
sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_1
|
||||
sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_notch_hz
|
||||
sbufWriteU16(dst, 1); // BF: currentPidProfile->dterm_notch_cutoff
|
||||
sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_2
|
||||
|
|
|
|||
|
|
@ -388,8 +388,8 @@ void read_boot_sector(const emfat_t *emfat, uint8_t *sect)
|
|||
bs->volume_id[1] = 14;
|
||||
bs->volume_id[2] = 13;
|
||||
bs->volume_id[3] = 8;
|
||||
memcpy(bs->volume_label, "NO NAME ", 12);
|
||||
memcpy(bs->file_system_type, "FAT32 ", 8);
|
||||
memcpy(bs->volume_label, "NO NAME ", VOL_LABEL_LEN);
|
||||
memcpy(bs->file_system_type, "FAT32 ", FILE_SYS_TYPE_LENGTH);
|
||||
sect[SECT - 2] = 0x55;
|
||||
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;
|
||||
}
|
||||
|
||||
memset(entry->name, ' ', FILE_NAME_SHRT_LEN + FILE_NAME_EXTN_LEN);
|
||||
memset(entry->name, ' ', FILE_NAME_SHRT_LEN);
|
||||
memcpy(entry->name, name, l1);
|
||||
memset(entry->extn, ' ', FILE_NAME_EXTN_LEN);
|
||||
memcpy(entry->extn, name + dot_pos + 1, l2);
|
||||
|
||||
for (i = 0; i < FILE_NAME_SHRT_LEN; i++) {
|
||||
|
|
|
|||
|
|
@ -782,7 +782,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.mwState = MW_NAV_STATE_WP_ENROUTE,
|
||||
.mwError = MW_NAV_ERROR_FINISH,
|
||||
.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_ALTHOLD] = NAV_STATE_ALTHOLD_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
|
||||
|
||||
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)) {
|
||||
resetPositionController();
|
||||
|
|
@ -1028,7 +1030,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
|
|||
{
|
||||
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, 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)) {
|
||||
// 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
|
||||
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;
|
||||
}
|
||||
/* 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)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
rthAltControlStickOverrideCheck(PITCH);
|
||||
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE)) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
if (!STATE(ALTITUDE_CONTROL)) {
|
||||
//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
|
||||
}
|
||||
|
||||
// If we have valid pos sensor OR we are configured to ignore GPS loss
|
||||
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));
|
||||
rthAltControlStickOverrideCheck(PITCH);
|
||||
|
||||
// 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;
|
||||
|
||||
}
|
||||
}
|
||||
/* Position sensor failure timeout - land */
|
||||
else {
|
||||
/* Position sensor failure timeout and not configured to ignore GPS loss - land */
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE) ||
|
||||
(checkForPositionSensorTimeout() && !navConfig()->general.flags.rth_climb_ignore_emerg)) {
|
||||
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)
|
||||
|
|
@ -1267,7 +1263,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -1279,12 +1276,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
// 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);
|
||||
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
|
||||
}
|
||||
else if (!validateRTHSanityChecker()) {
|
||||
// Sanity check of RTH
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
|
||||
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;
|
||||
}
|
||||
/* 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)
|
||||
{
|
||||
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
|
||||
if (!STATE(ALTITUDE_CONTROL)) {
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
// If position ok OR within valid timeout - continue
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
|
||||
// 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 {
|
||||
/* 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;
|
||||
}
|
||||
|
||||
// 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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
if (!ARMING_FLAG(ARMED) || isLandingDetected()) {
|
||||
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
|
||||
if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
|
||||
// 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.
|
||||
descentVelLimited = navConfig()->general.land_minalt_vspd;
|
||||
}
|
||||
else {
|
||||
// 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) {
|
||||
// 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.
|
||||
descentVelLimited = navConfig()->general.land_minalt_vspd;
|
||||
} 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.
|
||||
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;
|
||||
descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
|
||||
}
|
||||
|
||||
updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL);
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
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)) {
|
||||
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)
|
||||
|
|
@ -1632,11 +1606,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
|
|||
|
||||
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;
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
|
|
@ -1681,17 +1655,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
|
|||
clearJumpCounters();
|
||||
posControl.wpMissionRestart = true;
|
||||
|
||||
// If no position sensor available - land immediately
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
/* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
|
||||
else if (checkForPositionSensorTimeout()) {
|
||||
/* If position sensors unavailable - land immediately (wait for timeout on GPS) */
|
||||
if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) {
|
||||
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)
|
||||
|
|
@ -1959,21 +1928,21 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali
|
|||
if (estPosValid && estVelValid) {
|
||||
posControl.flags.estPosStatus = EST_TRUSTED;
|
||||
posControl.flags.estVelStatus = EST_TRUSTED;
|
||||
posControl.flags.horizontalPositionDataNew = 1;
|
||||
posControl.flags.horizontalPositionDataNew = true;
|
||||
posControl.lastValidPositionTimeMs = millis();
|
||||
}
|
||||
// CASE 1: POS invalid, VEL valid
|
||||
else if (!estPosValid && estVelValid) {
|
||||
posControl.flags.estPosStatus = EST_USABLE; // Pos usable, but not trusted
|
||||
posControl.flags.estVelStatus = EST_TRUSTED;
|
||||
posControl.flags.horizontalPositionDataNew = 1;
|
||||
posControl.flags.horizontalPositionDataNew = true;
|
||||
posControl.lastValidPositionTimeMs = millis();
|
||||
}
|
||||
// CASE 3: can't use pos/vel data
|
||||
else {
|
||||
posControl.flags.estPosStatus = EST_NONE;
|
||||
posControl.flags.estVelStatus = EST_NONE;
|
||||
posControl.flags.horizontalPositionDataNew = 0;
|
||||
posControl.flags.horizontalPositionDataNew = false;
|
||||
}
|
||||
|
||||
//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.estAltStatus = EST_TRUSTED;
|
||||
posControl.flags.verticalPositionDataNew = 1;
|
||||
posControl.flags.verticalPositionDataNew = true;
|
||||
posControl.lastValidAltitudeTimeMs = millis();
|
||||
}
|
||||
else {
|
||||
posControl.flags.estAltStatus = EST_NONE;
|
||||
posControl.flags.estAglStatus = EST_NONE;
|
||||
posControl.flags.verticalPositionDataNew = 0;
|
||||
posControl.flags.verticalPositionDataNew = false;
|
||||
}
|
||||
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
|
|
@ -2068,8 +2037,6 @@ void updateActualHeading(bool headingValid, int32_t newHeading)
|
|||
/* Precompute sin/cos of yaw angle */
|
||||
posControl.actualState.sinYaw = sin_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 */
|
||||
if (posControl.flags.horizontalPositionDataConsumed)
|
||||
posControl.flags.horizontalPositionDataNew = 0;
|
||||
posControl.flags.horizontalPositionDataNew = false;
|
||||
|
||||
if (posControl.flags.verticalPositionDataConsumed)
|
||||
posControl.flags.verticalPositionDataNew = 0;
|
||||
posControl.flags.verticalPositionDataNew = false;
|
||||
|
||||
//Update blackbox data
|
||||
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)
|
||||
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
|
||||
// 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
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
|
@ -3276,7 +3243,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
|
||||
// Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
|
||||
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;
|
||||
}
|
||||
else {
|
||||
|
|
@ -3607,9 +3574,8 @@ void navigationInit(void)
|
|||
/* Initial state */
|
||||
posControl.navState = NAV_STATE_IDLE;
|
||||
|
||||
posControl.flags.horizontalPositionDataNew = 0;
|
||||
posControl.flags.verticalPositionDataNew = 0;
|
||||
posControl.flags.headingDataNew = 0;
|
||||
posControl.flags.horizontalPositionDataNew = false;
|
||||
posControl.flags.verticalPositionDataNew = false;
|
||||
|
||||
posControl.flags.estAltStatus = EST_NONE;
|
||||
posControl.flags.estPosStatus = EST_NONE;
|
||||
|
|
|
|||
|
|
@ -57,7 +57,7 @@
|
|||
#define UNUSED(x) ((void)(x))
|
||||
#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_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_FINISHING "FINISHING"
|
||||
|
||||
|
|
|
|||
|
|
@ -71,7 +71,6 @@ typedef enum {
|
|||
typedef struct navigationFlags_s {
|
||||
bool horizontalPositionDataNew;
|
||||
bool verticalPositionDataNew;
|
||||
bool headingDataNew;
|
||||
|
||||
bool horizontalPositionDataConsumed;
|
||||
bool verticalPositionDataConsumed;
|
||||
|
|
|
|||
|
|
@ -93,9 +93,6 @@ STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT];
|
|||
STATIC_FASTRAM filterApplyFnPtr gyroLpf2ApplyFn;
|
||||
STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT];
|
||||
|
||||
STATIC_FASTRAM filterApplyFnPtr notchFilter1ApplyFn;
|
||||
STATIC_FASTRAM void *notchFilter1[XYZ_AXIS_COUNT];
|
||||
|
||||
#ifdef USE_DYNAMIC_FILTERS
|
||||
|
||||
EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState;
|
||||
|
|
@ -103,7 +100,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
|
|||
|
||||
#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,
|
||||
.gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
|
||||
|
|
@ -115,8 +112,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
|
|||
#ifdef USE_DUAL_GYRO
|
||||
.gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT,
|
||||
#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_type = SETTING_GYRO_MAIN_LPF_TYPE_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_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
|
||||
notchFilter1ApplyFn = nullFilterApply;
|
||||
|
||||
//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());
|
||||
|
||||
//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());
|
||||
|
||||
//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
|
||||
if (gyroConfig()->kalmanEnabled) {
|
||||
gyroKalmanInitialize(gyroConfig()->kalman_q);
|
||||
|
|
@ -470,7 +453,6 @@ void FAST_CODE NOINLINE gyroFilter()
|
|||
#endif
|
||||
|
||||
gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf);
|
||||
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
|
||||
|
||||
#ifdef USE_DYNAMIC_FILTERS
|
||||
if (dynamicGyroNotchState.enabled) {
|
||||
|
|
|
|||
|
|
@ -59,8 +59,6 @@ typedef struct gyroConfig_s {
|
|||
#ifdef USE_DUAL_GYRO
|
||||
uint8_t gyro_to_use;
|
||||
#endif
|
||||
uint16_t gyro_notch_hz;
|
||||
uint16_t gyro_notch_cutoff;
|
||||
uint16_t gyro_main_lpf_hz;
|
||||
uint8_t gyro_main_lpf_type;
|
||||
uint8_t useDynamicLpf;
|
||||
|
|
|
|||
|
|
@ -61,8 +61,6 @@ void targetConfiguration(void)
|
|||
gyroConfigMutable()->looptime = 1000;
|
||||
gyroConfigMutable()->gyro_lpf = 0; // 256 Hz
|
||||
gyroConfigMutable()->gyro_main_lpf_hz = 90;
|
||||
gyroConfigMutable()->gyro_notch_hz = 150;
|
||||
gyroConfigMutable()->gyro_notch_cutoff = 80;
|
||||
|
||||
accelerometerConfigMutable()->acc_hardware = ACC_MPU6500;
|
||||
accelerometerConfigMutable()->acc_lpf_hz = 15;
|
||||
|
|
|
|||
|
|
@ -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 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 olc_unittest.cc PROPERTY depends "common/olc.c")
|
||||
|
|
|
|||
140
src/test/unit/flight_imu_unittest.cc
Normal file
140
src/test/unit/flight_imu_unittest.cc
Normal 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; }
|
||||
}
|
||||
|
|
@ -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; }
|
||||
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue