mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 09:45:33 +03:00
Merge remote-tracking branch 'upstream/master' into abo_fw_alt_vel_control
This commit is contained in:
breadoven
commit
6d626acdb2
50 changed files with 1829 additions and 322 deletions
2
.github/workflows/ci.yml
vendored
2
.github/workflows/ci.yml
vendored
|
|
@ -100,7 +100,7 @@ jobs:
|
|||
- name: Build SITL
|
||||
run: |
|
||||
mkdir -p build_SITL && cd build_SITL
|
||||
cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja ..
|
||||
cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -G Ninja ..
|
||||
ninja
|
||||
|
||||
- name: Upload artifacts
|
||||
|
|
|
|||
|
|
@ -21,6 +21,22 @@ Following rangefinders are supported:
|
|||
* MSP - experimental
|
||||
* TOF10120 - small & lightweight laser range sensor, usable up to 200cm
|
||||
* TERARANGER EVO - 30cm to 600cm, depends on version https://www.terabee.com/sensors-modules/lidar-tof-range-finders/#individual-distance-measurement-sensors
|
||||
* NRA15/NRA24 - experimental, UART version
|
||||
|
||||
#### NRA15/NRA24
|
||||
NRA15/NRA24 from nanoradar use US-D1_V0 or NRA protocol, it depends which firmware you use. Radar can be set by firmware
|
||||
to two different resolutions. See table below.
|
||||
|
||||
| Radar | Protocol | Resolution | Name in configurator |
|
||||
|-------|----------|-----------------|----------------------|
|
||||
| NRA15 | US-D1_V0 | 0-30m (+-4cm) | USD1_V0 |
|
||||
| NRA15 | NRA | 0-30m (+-4cm) | NRA |
|
||||
| NRA15 | NRA | 0-100m (+-10cm) | NRA |
|
||||
| NRA24 | US-D1_V0 | 0-50m (+-4cm) | USD1_V0 |
|
||||
| NRA24 | US-D1_V0 | 0-200m (+-10cm) | USD1_V0 |
|
||||
| NRA24 | NRA | 0-50m (+-4cm) | NRA |
|
||||
| NRA24 | NRA | 0-200m (+-10cm) | NRA |
|
||||
|
||||
|
||||
## Connections
|
||||
|
||||
|
|
|
|||
|
|
@ -16,6 +16,10 @@ Currently supported are
|
|||
|
||||
INAV SITL communicates for sensor data and control directly with the corresponding simulator, see the documentation of the individual simulators and the Configurator or the command line options.
|
||||
|
||||
AS SITL is still an inav software, but running on PC, it is possible to use HITL interface for communication.
|
||||
|
||||
INAV-X-Plane-HITL plugin https://github.com/RomanLut/INAV-X-Plane-HITL can be used with SITL.
|
||||
|
||||
## Sensors
|
||||
The following sensors are emulated:
|
||||
- IMU (Gyro, Accelerometer)
|
||||
|
|
@ -30,13 +34,18 @@ The following sensors are emulated:
|
|||
|
||||
Select "FAKE" as type for all mentioned, so that they receive the data from the simulator.
|
||||
|
||||
## Serial ports+
|
||||
UARTs are replaced by TCP starting with port 5760 ascending. UART 1 port 5760, UART2 5761, ...
|
||||
By default, UART1 and UART2 are available as MSP connections. Other UARTs will have TCP listeners if they have an INAV function assigned.
|
||||
To connect the Configurator to SITL: Select TCP and connect to ```localhost:5760``` (or ```127.0.0.1:5760``` if your OS doesn't understand `localhost`) (if SITL is running on the same machine).
|
||||
## Serial ports
|
||||
UARTs are replaced by TCP starting with port 5760 ascending. UART1 is mapped to port 5760, UART2 to 5761, etc.
|
||||
|
||||
By default, UART1 and UART2 are configured for MSP connections. Other UARTs will have TCP listeners if they have an INAV function assigned.
|
||||
|
||||
To connect the Configurator to SITL, select "SITL".
|
||||
|
||||
Alternativelly, select "TCP" and connect to ```localhost:5760``` (or ```127.0.0.1:5760``` if your OS doesn't understand `localhost`) (if SITL is running on the same machine).
|
||||
|
||||
IPv4 and IPv6 are supported, either raw addresses or host-name lookup.
|
||||
|
||||
The assignment and status of user UART/TCP connections is displayed on the console.
|
||||
The assignment and status of used UART/TCP connections is displayed on the console.
|
||||
|
||||
```
|
||||
INAV 6.1.0 SITL
|
||||
|
|
@ -51,39 +60,73 @@ INAV 6.1.0 SITL
|
|||
All other interfaces (I2C, SPI, etc.) are not emulated.
|
||||
|
||||
## Remote control
|
||||
MSP_RX (TCP/IP) or joystick (via simulator) or serial receiver via USB/Serial interface are supported.
|
||||
Multiple methods for connecting RC Controllers are available:
|
||||
- MSP_RX (TCP/IP)
|
||||
- joystick (via simulator)
|
||||
- serial receiver via USB to serial converter
|
||||
- any receiver with proxy flight controller
|
||||
|
||||
|
||||
### MSP_RX
|
||||
|
||||
MSP_RX is the default, 18 channels are supported over TCP/IP serial emulation.
|
||||
MSP_RX is the default, 18 channels are supported over TCP/IP connection.
|
||||
|
||||
### Joystick interface
|
||||
Only 8 channels are supported.
|
||||
Select "SIM (SITL)" as the receiver and set up a joystick in the simulator, details of which can be found in the documentation for the individual simulators.
|
||||
|
||||
Select "SIM (SITL)" as the receiver and set up a joystick in the simulator.
|
||||
|
||||
*Not available with INAV-X-Plane-HITL plugin.*
|
||||
|
||||
### Serial Receiver via USB
|
||||
|
||||
Connect a serial receiver (e.g. SBUS) to the PC via a UART/USB adapter. Configure the receiver in the Configurator as usual.
|
||||
- Connect a serial receiver to the PC via a USB-to-serial adapter
|
||||
- Configure the receiver in the SITL as usual
|
||||
- While starting SITL from configurator, enable "Serial receiver" option
|
||||
|
||||
The Configurator offers a built-in option for forwarding the serial data to the SITL TCP port, if SITL is started manually the following option can be used:
|
||||
The SITL offers a built-in option for forwarding the host's serial port to the SITL UART.
|
||||
|
||||
The connection can then be established with a programme that forwards the serial data unaltered to TCP, e.g. with the Python script tcp_serial_redirect.py (https://github.com/Scavanger/TCP-Serial-Redirect)
|
||||
If necessary, please download the required runtime environment from https://www.python.org/.
|
||||
Please use the linked version, which has a smaller buffer, otherwise the control response is relatively slow.
|
||||
Please note that 100000(SBUS) and 420000(CRSF) are non-standart baud rates which may not be supported by some USB-to-serial adapters. FDTI and CH340 should work. CP2102/9 does not work.
|
||||
|
||||
### Example SBUS:
|
||||
For this you need a FT232 module. With FT-Prog (https://ftdichip.com/utilities/) the signals can be inverted: Devices->Scan and Parse, then Hardware Specific -> Invert RS232 Signals -> Invert RXD.
|
||||
|
||||
#### Example SBUS:
|
||||
For this you need a USB-to-serial adapter, receiver with inverter, or receiver which can output inverted SBUS (normal UART).
|
||||
|
||||
SBUS protocol is inverted UART.
|
||||
|
||||
Receiver's SBUS output should be connected to the USB-to-serial adapter's RX pin (via inverter).
|
||||
|
||||
With FT-Prog (https://ftdichip.com/utilities/) the signal can be inverted by adapter: Devices->Scan and Parse, then Hardware Specific -> Invert RS232 Signals -> Invert RXD.
|
||||
|
||||
|
||||
|
||||
For SBUS, the command line arguments of the python script are:
|
||||
```python tcp_serial_redirect.py --parity E --stopbits 2 -c 127.0.0.1:[INAV-UART-PORT] COMXX 100000```
|
||||
|
||||
|
||||
### Telemetry
|
||||
In the SITL configuration, enable serial receiver on some port and configure receiver type "Serial", "SBUS".
|
||||
|
||||
LTM and MAVLink telemetry are supported, either as a discrete function or shared with MSP.
|
||||
#### Example CRSF:
|
||||
|
||||
On receiver side, CRSF is normal UART.
|
||||
|
||||
Connect receiver's RX/TX pins (and GND, 5V of course) to USB-To-Serial adapter's TX/RX pins (RX to TX, TX to RX).
|
||||
|
||||
|
||||
|
||||
In the SITL configuration, enable serial receiver on some port and configure receiver type "Serial", "CRSF".
|
||||
|
||||
### Proxy Flight controller
|
||||
|
||||
The last, but probably the most easiest way to connect receiver to the SITL, is to use any inav/betaflight Flight controler as proxy.
|
||||
|
||||
Connect receiver of any type to FC and configure FC to the point where channels are correctly updated in the "Receiver" tab. Inav and Betaflight are supported.
|
||||
|
||||
You also can use your plane/quad ( if receiver is powered from USB).
|
||||
|
||||
|
||||
|
||||
In the SITL configuration, select "Receiver type: SIM" regardles of the kind of receiver used.
|
||||
|
||||
RX Telemetry via a return channel through the receiver is not yet supported by SITL.
|
||||
|
||||
## OSD
|
||||
For the OSD the program INAV-Sim-OSD is available: https://github.com/Scavanger/INAV-SIM-OSD.
|
||||
|
|
@ -91,6 +134,8 @@ For this, activate MSP-Displayport on a UART/TCP port and connect to the corresp
|
|||
|
||||
Note: INAV-Sim-OSD only works if the simulator is in window mode.
|
||||
|
||||
*With INAV-X-Plane-HITL plugin, OSD is supported natively.*
|
||||
|
||||
## Command line
|
||||
|
||||
The command line options are only necessary if the SITL executable is started by hand.
|
||||
|
|
@ -103,7 +148,7 @@ If SITL is started without command line options, only a serial MSP / CLI connect
|
|||
|
||||
```--path``` Path and file name to config file. If not present, eeprom.bin in the current directory is used. Example: ```C:\INAV_SITL\flying-wing.bin```, ```/home/user/sitl-eeproms/test-eeprom.bin```.
|
||||
|
||||
```--sim=[sim]``` Select the simulator. xp = X-Plane, rf = RealFlight. Example: ```--sim=xp```
|
||||
```--sim=[sim]``` Select the simulator. xp = X-Plane, rf = RealFlight. Example: ```--sim=xp```. If not specified, configurator-only mode is started. Omit for usage with INAV-X-Plane-HITL plugin.
|
||||
|
||||
```--simip=[ip]``` Hostname or IP address of the simulator, if you specify a simulator with "--sim" and omit this option IPv4 localhost (`127.0.0.1`) will be used. Example: ```--simip=172.65.21.15```, ```--simip acme-sims.org```, ```--sim ::1```.
|
||||
|
||||
|
|
@ -118,6 +163,18 @@ To assign motor1 to virtual receiver channel 1, servo 1 to channel 2, and servo2
|
|||
```--chanmap:M01-01,S01-02,S02-03```
|
||||
Please also read the documentation of the individual simulators.
|
||||
|
||||
```--serialport``` Use serial receiver or proxy FC connected to host's serial port, f.e. ```--serialportCOM5``` or ```--serialportdev/ttyACM3```
|
||||
|
||||
```--serialuart``` Map serial receiver to SITL UART, f.e. ```--serialuart=3``` for UART3. Omit if using ```--fcproxy```.
|
||||
|
||||
```--baudrate``` Serial receiver baudrate (default: 115200)
|
||||
|
||||
```--stopbits=[None|One|Two]``` Serial receiver stopbits (default: One)
|
||||
|
||||
```--parity=[Even|None|Odd]``` Serial receiver parity (default: None)
|
||||
|
||||
```--fcproxy``` Use inav/betaflight FC as a proxy for serial receiver.
|
||||
|
||||
```--help``` Displays help for the command line options.
|
||||
|
||||
For options that take an argument, either form `--flag=value` or `--flag value` may be used.
|
||||
|
|
@ -125,46 +182,13 @@ For options that take an argument, either form `--flag=value` or `--flag value`
|
|||
## Running SITL
|
||||
It is recommended to start the tools in the following order:
|
||||
1. Simulator, aircraft should be ready for take-off
|
||||
2. INAV-SITL
|
||||
2. SITL
|
||||
3. OSD
|
||||
4. serial redirect for RC input
|
||||
|
||||
## Compile
|
||||
For INav-X-Plane-HITL plugin:
|
||||
1. SITL (Run in configurator-only mode)
|
||||
2. X-Plane
|
||||
|
||||
### Linux and FreeBSD:
|
||||
Almost like normal, ruby, cmake and make are also required.
|
||||
With cmake, the option "-DSITL=ON" must be specified.
|
||||
# #Forwarding serial data for other UART
|
||||
|
||||
```
|
||||
mkdir build_SITL
|
||||
cd build_SITL
|
||||
cmake -DSITL=ON ..
|
||||
make
|
||||
```
|
||||
|
||||
### Windows:
|
||||
Compile under cygwin, then as in Linux.
|
||||
Copy cygwin1.dll into the directory, or include cygwin's /bin/ directory in the environment variable PATH.
|
||||
|
||||
If the build fails (segfault, possibly out of memory), adding `-DCMAKE_BUILD_TYPE=MinRelSize` to the `cmake` command may help.
|
||||
|
||||
#### Build manager
|
||||
|
||||
`ninja` may also be used (parallel builds without `-j $(nproc)`):
|
||||
|
||||
```
|
||||
cmake -GNinja -DSITL=ON ..
|
||||
ninja
|
||||
```
|
||||
|
||||
### Compiler requirements
|
||||
|
||||
* Modern GCC. Must be a *real* GCC, macOS faking it with clang will not work. GCC 10 to GCC 13 are known to work.
|
||||
* Unix sockets networking. Cygwin is required on Windows (vice `winsock`).
|
||||
* Pthreads
|
||||
|
||||
## Supported environments
|
||||
|
||||
* Linux on x86_64, ia-32, Aarch64 (e.g. Rpi4), RISCV64 (e.g. VisionFive2)
|
||||
* Windows on x86_64
|
||||
* FreeBSD (x86_64 at least).
|
||||
Other UARTs can then be mapped to host's serial port using external tool, which can be found in directories ```inav-configurator\resources\sitl\linux\Ser2TCP```, ```inav-configurator\resources\sitl\windows\Ser2TCP.exe```
|
||||
|
|
|
|||
BIN
docs/SITL/assets/serial_receiver_crsf.png
Normal file
BIN
docs/SITL/assets/serial_receiver_crsf.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 6.8 KiB |
BIN
docs/SITL/assets/serial_receiver_proxy.png
Normal file
BIN
docs/SITL/assets/serial_receiver_proxy.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 6.9 KiB |
BIN
docs/SITL/assets/serial_receiver_sbus.png
Normal file
BIN
docs/SITL/assets/serial_receiver_sbus.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 6.6 KiB |
|
|
@ -932,6 +932,16 @@ EzTune response
|
|||
|
||||
---
|
||||
|
||||
### ez_snappiness
|
||||
|
||||
EzTune snappiness
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 0 | 0 | 100 |
|
||||
|
||||
---
|
||||
|
||||
### ez_stability
|
||||
|
||||
EzTune stability
|
||||
|
|
@ -1804,11 +1814,11 @@ Allows to chose when the home position is reset. Can help prevent resetting home
|
|||
|
||||
### inav_use_gps_no_baro
|
||||
|
||||
_// TODO_
|
||||
Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed.
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| OFF | OFF | ON |
|
||||
| ON | OFF | ON |
|
||||
|
||||
---
|
||||
|
||||
|
|
|
|||
39
docs/development/Building SITL.md
Normal file
39
docs/development/Building SITL.md
Normal file
|
|
@ -0,0 +1,39 @@
|
|||
## Building SITL
|
||||
|
||||
### Linux and FreeBSD:
|
||||
Almost like normal, ruby, cmake and make are also required.
|
||||
With cmake, the option "-DSITL=ON" must be specified.
|
||||
|
||||
```
|
||||
mkdir build_SITL
|
||||
cd build_SITL
|
||||
cmake -DSITL=ON ..
|
||||
make
|
||||
```
|
||||
|
||||
### Windows:
|
||||
Compile under cygwin, then as in Linux.
|
||||
Copy cygwin1.dll into the directory, or include cygwin's /bin/ directory in the environment variable PATH.
|
||||
|
||||
If the build fails (segfault, possibly out of memory), adding `-DCMAKE_BUILD_TYPE=MinRelSize` to the `cmake` command may help.
|
||||
|
||||
#### Build manager
|
||||
|
||||
`ninja` may also be used (parallel builds without `-j $(nproc)`):
|
||||
|
||||
```
|
||||
cmake -GNinja -DSITL=ON ..
|
||||
ninja
|
||||
```
|
||||
|
||||
### Compiler requirements
|
||||
|
||||
* Modern GCC. Must be a *real* GCC, macOS faking it with clang will not work. GCC 10 to GCC 13 are known to work.
|
||||
* Unix sockets networking. Cygwin is required on Windows (vice `winsock`).
|
||||
* Pthreads
|
||||
|
||||
## Supported environments
|
||||
|
||||
* Linux on x86_64, ia-32, Aarch64 (e.g. Rpi4), RISCV64 (e.g. VisionFive2)
|
||||
* Windows on x86_64
|
||||
* FreeBSD (x86_64 at least).
|
||||
19
docs/development/Building in Gitpod.md
Normal file
19
docs/development/Building in Gitpod.md
Normal file
|
|
@ -0,0 +1,19 @@
|
|||
# Building in Gitpod
|
||||
|
||||
Gitpod offers an online build environment for building INAV targets.
|
||||
## Setting up the environment and building targets
|
||||
|
||||
1. Go to https://gitpod.io/new
|
||||
1. Paste `https://github.com/iNavFlight/inav/tree/[version]` into the field called "Select a repository".
|
||||
1. Ensure that you substitute [version] (e.g. 7.1.0) with the version number of INAV that you want to build.
|
||||
1. Cick on the link that shows in the drop down and Gitpod will atomatically selects the adequate Editor and Browser.
|
||||
1. Leave the other fields as default and click "Continue". Your build environment will be created.
|
||||
1. At the bottom of the page, you will see a command line. Type `make [TARGET]` and wait for the target to be built.
|
||||
1. Once the build has finished, navigate to the build folder using `cd build`.
|
||||
1. Once in the folder, run `objcopy -O ihex -R .eeprom [TARGET].elf [TARGET].hex` to convert the `.elf` file to a `.hex` file.
|
||||
1. Your new target `.hex` binary will be located in a folder called `bin`, which can be found at the top left of the page.
|
||||
|
||||
|
||||
NOTE: You can use this method to build your forks as well. Just paste in the link to your fork and follow the rest of the steps.
|
||||
|
||||
You are done!
|
||||
|
|
@ -1,3 +1,4 @@
|
|||
|
||||
main_sources(COMMON_SRC
|
||||
main.c
|
||||
|
||||
|
|
@ -485,6 +486,8 @@ main_sources(COMMON_SRC
|
|||
io/rangefinder.h
|
||||
io/rangefinder_msp.c
|
||||
io/rangefinder_benewake.c
|
||||
io/rangefinder_usd1_v0.c
|
||||
io/rangefinder_nanoradar.c
|
||||
io/rangefinder_fake.c
|
||||
io/opflow.h
|
||||
io/opflow_cxof.c
|
||||
|
|
|
|||
|
|
@ -1308,10 +1308,16 @@ static void writeSlowFrame(void)
|
|||
static void loadSlowState(blackboxSlowState_t *slow)
|
||||
{
|
||||
memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
|
||||
// Also log Nav auto selected flight modes rather than just those selected by boxmode
|
||||
if (!IS_RC_MODE_ACTIVE(BOXANGLE) && FLIGHT_MODE(ANGLE_MODE)) {
|
||||
// Also log Nav auto enabled flight modes rather than just those selected by boxmode
|
||||
if (FLIGHT_MODE(ANGLE_MODE)) {
|
||||
slow->flightModeFlags |= (1 << BOXANGLE);
|
||||
}
|
||||
if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
|
||||
slow->flightModeFlags |= (1 << BOXNAVALTHOLD);
|
||||
}
|
||||
if (FLIGHT_MODE(NAV_RTH_MODE)) {
|
||||
slow->flightModeFlags |= (1 << BOXNAVRTH);
|
||||
}
|
||||
if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) {
|
||||
slow->flightModeFlags |= (1 << BOXHEADINGHOLD);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@
|
|||
#endif
|
||||
|
||||
#ifdef __APPLE__
|
||||
#define FASTRAM __attribute__ ((section("__DATA,__.fastram_bss"), aligned(4)))
|
||||
#define FASTRAM __attribute__ ((section("__DATA,__.fastram_bss"), aligned(8)))
|
||||
#else
|
||||
#define FASTRAM __attribute__ ((section(".fastram_bss"), aligned(4)))
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -26,6 +26,12 @@
|
|||
|
||||
//type of elements
|
||||
|
||||
#ifndef __APPLE__
|
||||
#define OSD_ENTRY_ATTR __attribute__((packed))
|
||||
#else
|
||||
#define OSD_ENTRY_ATTR
|
||||
#endif
|
||||
|
||||
typedef enum
|
||||
{
|
||||
OME_Label,
|
||||
|
|
@ -71,7 +77,7 @@ typedef struct
|
|||
const void * const data;
|
||||
const uint8_t type; // from OSD_MenuElement
|
||||
uint8_t flags;
|
||||
} __attribute__((packed)) OSD_Entry;
|
||||
} OSD_ENTRY_ATTR OSD_Entry;
|
||||
|
||||
// Bits in flags
|
||||
#define PRINT_VALUE (1 << 0) // Value has been changed, need to redraw
|
||||
|
|
|
|||
|
|
@ -64,7 +64,7 @@ static inline uint16_t pgIsProfile(const pgRegistry_t* reg) {return (reg->size &
|
|||
#ifdef __APPLE__
|
||||
extern const pgRegistry_t __pg_registry_start[] __asm("section$start$__DATA$__pg_registry");
|
||||
extern const pgRegistry_t __pg_registry_end[] __asm("section$end$__DATA$__pg_registry");
|
||||
#define PG_REGISTER_ATTRIBUTES __attribute__ ((section("__DATA,__pg_registry"), used, aligned(4)))
|
||||
#define PG_REGISTER_ATTRIBUTES __attribute__ ((section("__DATA,__pg_registry"), used, aligned(8)))
|
||||
|
||||
extern const uint8_t __pg_resetdata_start[] __asm("section$start$__DATA$__pg_resetdata");
|
||||
extern const uint8_t __pg_resetdata_end[] __asm("section$end$__DATA$__pg_resetdata");
|
||||
|
|
|
|||
|
|
@ -43,6 +43,7 @@
|
|||
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/serial_tcp.h"
|
||||
#include "target/SITL/serial_proxy.h"
|
||||
|
||||
static const struct serialPortVTable tcpVTable[];
|
||||
static tcpPort_t tcpPorts[SERIAL_PORT_COUNT];
|
||||
|
|
@ -118,6 +119,23 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
|
|||
return port;
|
||||
}
|
||||
|
||||
void tcpReceiveBytes( tcpPort_t *port, const uint8_t* buffer, ssize_t recvSize ) {
|
||||
for (ssize_t i = 0; i < recvSize; i++) {
|
||||
if (port->serialPort.rxCallback) {
|
||||
port->serialPort.rxCallback((uint16_t)buffer[i], port->serialPort.rxCallbackData);
|
||||
} else {
|
||||
pthread_mutex_lock(&port->receiveMutex);
|
||||
port->serialPort.rxBuffer[port->serialPort.rxBufferHead] = buffer[i];
|
||||
port->serialPort.rxBufferHead = (port->serialPort.rxBufferHead + 1) % port->serialPort.rxBufferSize;
|
||||
pthread_mutex_unlock(&port->receiveMutex);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void tcpReceiveBytesEx( int portIndex, const uint8_t* buffer, ssize_t recvSize ) {
|
||||
tcpReceiveBytes( &tcpPorts[portIndex], buffer, recvSize );
|
||||
}
|
||||
|
||||
int tcpReceive(tcpPort_t *port)
|
||||
{
|
||||
char addrbuf[IPADDRESS_PRINT_BUFLEN];
|
||||
|
|
@ -162,22 +180,12 @@ int tcpReceive(tcpPort_t *port)
|
|||
return 0;
|
||||
}
|
||||
|
||||
for (ssize_t i = 0; i < recvSize; i++) {
|
||||
|
||||
if (port->serialPort.rxCallback) {
|
||||
port->serialPort.rxCallback((uint16_t)buffer[i], port->serialPort.rxCallbackData);
|
||||
} else {
|
||||
pthread_mutex_lock(&port->receiveMutex);
|
||||
port->serialPort.rxBuffer[port->serialPort.rxBufferHead] = buffer[i];
|
||||
port->serialPort.rxBufferHead = (port->serialPort.rxBufferHead + 1) % port->serialPort.rxBufferSize;
|
||||
pthread_mutex_unlock(&port->receiveMutex);
|
||||
}
|
||||
}
|
||||
|
||||
if (recvSize < 0) {
|
||||
recvSize = 0;
|
||||
}
|
||||
|
||||
tcpReceiveBytes( port, buffer, recvSize );
|
||||
|
||||
return (int)recvSize;
|
||||
}
|
||||
|
||||
|
|
@ -240,9 +248,21 @@ void tcpWritBuf(serialPort_t *instance, const void *data, int count)
|
|||
send(port->clientSocketFd, data, count, 0);
|
||||
}
|
||||
|
||||
int getTcpPortIndex(const serialPort_t *instance) {
|
||||
for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
|
||||
if ( &(tcpPorts[i].serialPort) == instance) return i;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void tcpWrite(serialPort_t *instance, uint8_t ch)
|
||||
{
|
||||
tcpWritBuf(instance, (void*)&ch, 1);
|
||||
|
||||
int index = getTcpPortIndex(instance);
|
||||
if ( !serialFCProxy && serialProxyIsConnected() && (index == (serialUartIndex-1)) ) {
|
||||
serialProxyWriteData( (unsigned char *)&ch, 1);
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
|
||||
|
|
@ -263,6 +283,10 @@ uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
|
|||
return count;
|
||||
}
|
||||
|
||||
uint32_t tcpRXBytesFree(int portIndex) {
|
||||
return tcpPorts[portIndex].serialPort.rxBufferSize - tcpTotalRxBytesWaiting( &tcpPorts[portIndex].serialPort);
|
||||
}
|
||||
|
||||
uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
|
||||
{
|
||||
UNUSED(instance);
|
||||
|
|
@ -272,7 +296,6 @@ uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
|
|||
bool isTcpTransmitBufferEmpty(const serialPort_t *instance)
|
||||
{
|
||||
UNUSED(instance);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -26,6 +26,8 @@
|
|||
#include <netinet/in.h>
|
||||
#include <netdb.h>
|
||||
|
||||
#include "drivers/serial.h"
|
||||
|
||||
#define BASE_IP_ADDRESS 5760
|
||||
#define TCP_BUFFER_SIZE 2048
|
||||
#define TCP_MAX_PACKET_SIZE 65535
|
||||
|
|
@ -50,5 +52,7 @@ typedef struct
|
|||
|
||||
serialPort_t *tcpOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, void *rxCallbackData, uint32_t baudRate, portMode_t mode, portOptions_t options);
|
||||
|
||||
void tcpSend(tcpPort_t *port);
|
||||
int tcpReceive(tcpPort_t *port);
|
||||
extern void tcpSend(tcpPort_t *port);
|
||||
extern int tcpReceive(tcpPort_t *port);
|
||||
extern void tcpReceiveBytesEx( int portIndex, const uint8_t* buffer, ssize_t recvSize );
|
||||
extern uint32_t tcpRXBytesFree(int portIndex);
|
||||
|
|
@ -1171,7 +1171,7 @@ static void cliRxRange(char *cmdline)
|
|||
ptr = cmdline;
|
||||
i = fastA2I(ptr);
|
||||
if (i >= 0 && i < NON_AUX_CHANNEL_COUNT) {
|
||||
int rangeMin, rangeMax;
|
||||
int rangeMin = 0, rangeMax = 0;
|
||||
|
||||
ptr = nextArg(ptr);
|
||||
if (ptr) {
|
||||
|
|
|
|||
|
|
@ -342,6 +342,10 @@ static void updateArmingStatus(void)
|
|||
DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
|
||||
}
|
||||
|
||||
if (ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED) && !IS_RC_MODE_ACTIVE(BOXARM)) {
|
||||
DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
|
||||
}
|
||||
|
||||
/* CHECK: Arming switch */
|
||||
// If arming is disabled and the ARM switch is on
|
||||
// Note that this should be last check so all other blockers could be cleared correctly
|
||||
|
|
|
|||
|
|
@ -60,7 +60,6 @@
|
|||
#include "drivers/pwm_esc_detect.h"
|
||||
#include "drivers/pwm_mapping.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/serial_softserial.h"
|
||||
|
|
@ -147,6 +146,10 @@
|
|||
|
||||
#include "telemetry/telemetry.h"
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
#include "target/SITL/serial_proxy.h"
|
||||
#endif
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
#include "hardware_revision.h"
|
||||
#endif
|
||||
|
|
@ -223,6 +226,10 @@ void init(void)
|
|||
flashDeviceInitialized = flashInit();
|
||||
#endif
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
serialProxyInit();
|
||||
#endif
|
||||
|
||||
initEEPROM();
|
||||
ensureEEPROMContainsValidData();
|
||||
suspendRxSignal();
|
||||
|
|
|
|||
|
|
@ -1652,6 +1652,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
sbufWriteU8(dst, ezTune()->aggressiveness);
|
||||
sbufWriteU8(dst, ezTune()->rate);
|
||||
sbufWriteU8(dst, ezTune()->expo);
|
||||
sbufWriteU8(dst, ezTune()->snappiness);
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
|
@ -3260,7 +3261,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
|
||||
case MSP2_INAV_EZ_TUNE_SET:
|
||||
{
|
||||
if (dataSize == 10) {
|
||||
|
||||
if (dataSize < 10 || dataSize > 11) {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
|
||||
ezTuneMutable()->enabled = sbufReadU8(src);
|
||||
ezTuneMutable()->filterHz = sbufReadU16(src);
|
||||
ezTuneMutable()->axisRatio = sbufReadU8(src);
|
||||
|
|
@ -3271,10 +3276,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
|||
ezTuneMutable()->rate = sbufReadU8(src);
|
||||
ezTuneMutable()->expo = sbufReadU8(src);
|
||||
|
||||
ezTuneUpdate();
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
if (dataSize == 11) {
|
||||
ezTuneMutable()->snappiness = sbufReadU8(src);
|
||||
}
|
||||
ezTuneUpdate();
|
||||
}
|
||||
break;
|
||||
|
||||
|
|
|
|||
|
|
@ -93,6 +93,10 @@
|
|||
|
||||
#include "config/feature.h"
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
#include "target/SITL/serial_proxy.h"
|
||||
#endif
|
||||
|
||||
void taskHandleSerial(timeUs_t currentTimeUs)
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
|
|
@ -421,6 +425,10 @@ void fcTasksInit(void)
|
|||
#if defined(USE_SMARTPORT_MASTER)
|
||||
setTaskEnabled(TASK_SMARTPORT_MASTER, true);
|
||||
#endif
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
serialProxyStart();
|
||||
#endif
|
||||
}
|
||||
|
||||
cfTask_t cfTasks[TASK_COUNT] = {
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@ tables:
|
|||
values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "FAKE"]
|
||||
enum: accelerationSensor_e
|
||||
- name: rangefinder_hardware
|
||||
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO"]
|
||||
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"]
|
||||
enum: rangefinderType_e
|
||||
- name: mag_hardware
|
||||
values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]
|
||||
|
|
@ -1555,6 +1555,12 @@ groups:
|
|||
field: expo
|
||||
min: 0
|
||||
max: 200
|
||||
- name: ez_snappiness
|
||||
description: "EzTune snappiness"
|
||||
default_value: 0
|
||||
field: snappiness
|
||||
min: 0
|
||||
max: 100
|
||||
|
||||
- name: PG_RPM_FILTER_CONFIG
|
||||
headers: ["flight/rpm_filter.h"]
|
||||
|
|
@ -2266,9 +2272,10 @@ groups:
|
|||
min: 0
|
||||
max: 255
|
||||
- name: inav_use_gps_no_baro
|
||||
description: "Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed."
|
||||
field: use_gps_no_baro
|
||||
type: bool
|
||||
default_value: OFF
|
||||
default_value: ON
|
||||
- name: inav_allow_dead_reckoning
|
||||
description: "Defines if INAV will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation"
|
||||
default_value: OFF
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@
|
|||
|
||||
#include "rx/rx.h"
|
||||
|
||||
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0);
|
||||
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 1);
|
||||
|
||||
PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune,
|
||||
.enabled = SETTING_EZ_ENABLED_DEFAULT,
|
||||
|
|
@ -48,6 +48,7 @@ PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune,
|
|||
.aggressiveness = SETTING_EZ_AGGRESSIVENESS_DEFAULT,
|
||||
.rate = SETTING_EZ_RATE_DEFAULT,
|
||||
.expo = SETTING_EZ_EXPO_DEFAULT,
|
||||
.snappiness = SETTING_EZ_SNAPPINESS_DEFAULT,
|
||||
);
|
||||
|
||||
#define EZ_TUNE_PID_RP_DEFAULT { 40, 75, 23, 100 }
|
||||
|
|
@ -142,5 +143,8 @@ void ezTuneUpdate(void) {
|
|||
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100);
|
||||
((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100);
|
||||
|
||||
//D-Boost snappiness
|
||||
pidProfileMutable()->dBoostMin = scaleRangef(ezTune()->snappiness, 0, 100, 1.0f, 0.0f);
|
||||
|
||||
}
|
||||
}
|
||||
|
|
@ -36,6 +36,7 @@ typedef struct ezTuneSettings_s {
|
|||
uint8_t aggressiveness;
|
||||
uint8_t rate;
|
||||
uint8_t expo;
|
||||
uint8_t snappiness;
|
||||
} ezTuneSettings_t;
|
||||
|
||||
PG_DECLARE_PROFILE(ezTuneSettings_t, ezTune);
|
||||
|
|
|
|||
|
|
@ -728,7 +728,7 @@ bool areMotorsRunning(void)
|
|||
return false;
|
||||
}
|
||||
|
||||
uint16_t getMaxThrottle() {
|
||||
uint16_t getMaxThrottle(void) {
|
||||
|
||||
static uint16_t throttle = 0;
|
||||
|
||||
|
|
|
|||
|
|
@ -79,7 +79,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance)
|
|||
}
|
||||
}
|
||||
|
||||
void activateMixerConfig(){
|
||||
void activateMixerConfig(void){
|
||||
currentMixerProfileIndex = getConfigMixerProfile();
|
||||
currentMixerConfig = *mixerConfig();
|
||||
nextMixerProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT;
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@
|
|||
|
||||
uint8_t getBfCharacter(uint8_t ch, uint8_t page)
|
||||
{
|
||||
uint16_t ech = ch | (page << 8);
|
||||
uint16_t ech = ch | ((page & 0x3)<< 8) ;
|
||||
|
||||
if (ech >= 0x20 && ech <= 0x5F) { // ASCII range
|
||||
return ch;
|
||||
|
|
|
|||
|
|
@ -100,9 +100,8 @@ static timeMs_t sendSubFrameMs = 0;
|
|||
static uint8_t currentOsdMode; // HDZero screen mode can change across layouts
|
||||
|
||||
static uint8_t screen[SCREENSIZE];
|
||||
static BITARRAY_DECLARE(fontPage, SCREENSIZE); // font page for each character on the screen
|
||||
static uint8_t attrs[SCREENSIZE]; // font page, blink and other attributes
|
||||
static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character on the screen
|
||||
static BITARRAY_DECLARE(blinkChar, SCREENSIZE); // Does the character blink?
|
||||
static bool screenCleared;
|
||||
static uint8_t screenRows, screenCols;
|
||||
static videoSystem_e osdVideoSystem;
|
||||
|
|
@ -158,6 +157,22 @@ static uint8_t determineHDZeroOsdMode(void)
|
|||
return HD_3016;
|
||||
}
|
||||
|
||||
|
||||
uint8_t setAttrPage(uint8_t origAttr, uint8_t page)
|
||||
{
|
||||
return (origAttr & ~DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK) | (page & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK);
|
||||
}
|
||||
|
||||
uint8_t setAttrBlink(uint8_t origAttr, uint8_t blink)
|
||||
{
|
||||
return (origAttr & ~DISPLAYPORT_MSP_ATTR_BLINK_MASK) | ((blink << DISPLAYPORT_MSP_ATTR_BLINK) & DISPLAYPORT_MSP_ATTR_BLINK_MASK);
|
||||
}
|
||||
|
||||
uint8_t setAttrVersion(uint8_t origAttr, uint8_t version)
|
||||
{
|
||||
return (origAttr & ~DISPLAYPORT_MSP_ATTR_VERSION_MASK) | ((version << DISPLAYPORT_MSP_ATTR_VERSION) & DISPLAYPORT_MSP_ATTR_VERSION_MASK);
|
||||
}
|
||||
|
||||
static int setDisplayMode(displayPort_t *displayPort)
|
||||
{
|
||||
if (osdVideoSystem == VIDEO_SYSTEM_HDZERO) {
|
||||
|
|
@ -171,9 +186,8 @@ static int setDisplayMode(displayPort_t *displayPort)
|
|||
static void init(void)
|
||||
{
|
||||
memset(screen, SYM_BLANK, sizeof(screen));
|
||||
BITARRAY_CLR_ALL(fontPage);
|
||||
memset(attrs, 0, sizeof(attrs));
|
||||
BITARRAY_CLR_ALL(dirty);
|
||||
BITARRAY_CLR_ALL(blinkChar);
|
||||
}
|
||||
|
||||
static int clearScreen(displayPort_t *displayPort)
|
||||
|
|
@ -204,9 +218,8 @@ static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint1
|
|||
}
|
||||
|
||||
*c = screen[pos];
|
||||
if (bitArrayGet(fontPage, pos)) {
|
||||
*c |= 0x100;
|
||||
}
|
||||
uint8_t page = getAttrPage(attrs[pos]);
|
||||
*c |= page << 8;
|
||||
|
||||
if (attr) {
|
||||
*attr = TEXT_ATTRIBUTES_NONE;
|
||||
|
|
@ -219,11 +232,12 @@ static int setChar(const uint16_t pos, const uint16_t c, textAttributes_t attr)
|
|||
{
|
||||
if (pos < SCREENSIZE) {
|
||||
uint8_t ch = c & 0xFF;
|
||||
bool page = (c >> 8);
|
||||
if (screen[pos] != ch || bitArrayGet(fontPage, pos) != page) {
|
||||
uint8_t page = (c >> 8) & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK;
|
||||
if (screen[pos] != ch || getAttrPage(attrs[pos]) != page) {
|
||||
screen[pos] = ch;
|
||||
(page) ? bitArraySet(fontPage, pos) : bitArrayClr(fontPage, pos);
|
||||
(TEXT_ATTRIBUTES_HAVE_BLINK(attr)) ? bitArraySet(blinkChar, pos) : bitArrayClr(blinkChar, pos);
|
||||
attrs[pos] = setAttrPage(attrs[pos], page);
|
||||
uint8_t blink = (TEXT_ATTRIBUTES_HAVE_BLINK(attr)) ? 1 : 0;
|
||||
attrs[pos] = setAttrBlink(attrs[pos], blink);
|
||||
bitArraySet(dirty, pos);
|
||||
}
|
||||
}
|
||||
|
|
@ -287,8 +301,8 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
|
|||
uint8_t col = pos % COLS;
|
||||
uint8_t attributes = 0;
|
||||
int endOfLine = row * COLS + screenCols;
|
||||
bool page = bitArrayGet(fontPage, pos);
|
||||
bool blink = bitArrayGet(blinkChar, pos);
|
||||
uint8_t page = getAttrPage(attrs[pos]);
|
||||
uint8_t blink = getAttrBlink(attrs[pos]);
|
||||
|
||||
uint8_t len = 4;
|
||||
do {
|
||||
|
|
@ -299,7 +313,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
|
|||
if (bitArrayGet(dirty, pos)) {
|
||||
next = pos;
|
||||
}
|
||||
} while (next == pos && next < endOfLine && bitArrayGet(fontPage, next) == page && bitArrayGet(blinkChar, next) == blink);
|
||||
} while (next == pos && next < endOfLine && getAttrPage(attrs[next]) == page && getAttrBlink(attrs[next]) == blink);
|
||||
|
||||
if (!isBfCompatibleVideoSystem(osdConfig())) {
|
||||
attributes |= (page << DISPLAYPORT_MSP_ATTR_FONTPAGE);
|
||||
|
|
@ -525,7 +539,7 @@ void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
|
|||
}
|
||||
}
|
||||
|
||||
mspPort_t *getMspOsdPort()
|
||||
mspPort_t *getMspOsdPort(void)
|
||||
{
|
||||
if (mspPort.port) {
|
||||
return &mspPort;
|
||||
|
|
|
|||
|
|
@ -27,13 +27,25 @@
|
|||
#include "drivers/osd.h"
|
||||
#include "msp/msp_serial.h"
|
||||
|
||||
// MSP displayport V2 attribute byte bit functions
|
||||
#define DISPLAYPORT_MSP_ATTR_FONTPAGE 0 // Select bank of 256 characters as per displayPortSeverity_e
|
||||
#define DISPLAYPORT_MSP_ATTR_BLINK 6 // Device local blink
|
||||
#define DISPLAYPORT_MSP_ATTR_VERSION 7 // Format indicator; must be zero for V2 (and V1)
|
||||
|
||||
#define DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK 0x3
|
||||
#define DISPLAYPORT_MSP_ATTR_BLINK_MASK (1 << DISPLAYPORT_MSP_ATTR_BLINK)
|
||||
#define DISPLAYPORT_MSP_ATTR_VERSION_MASK (1 << DISPLAYPORT_MSP_ATTR_VERSION)
|
||||
|
||||
typedef struct displayPort_s displayPort_t;
|
||||
|
||||
displayPort_t *mspOsdDisplayPortInit(const videoSystem_e videoSystem);
|
||||
void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn);
|
||||
mspPort_t *getMspOsdPort(void);
|
||||
|
||||
// MSP displayport V2 attribute byte bit functions
|
||||
#define DISPLAYPORT_MSP_ATTR_FONTPAGE 0 // Select bank of 256 characters as per displayPortSeverity_e
|
||||
#define DISPLAYPORT_MSP_ATTR_BLINK 6 // Device local blink
|
||||
#define DISPLAYPORT_MSP_ATTR_VERSION 7 // Format indicator; must be zero for V2 (and V1)
|
||||
#define getAttrPage(attr) (attr & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK)
|
||||
#define getAttrBlink(attr) ((attr & DISPLAYPORT_MSP_ATTR_BLINK_MASK) >> DISPLAYPORT_MSP_ATTR_BLINK)
|
||||
#define getAttrVersion(attr) ((attr & DISPLAYPORT_MSP_ATTR_VERSION_MASK) >> DISPLAYPORT_MSP_ATTR_VERSION)
|
||||
|
||||
uint8_t setAttrPage(uint8_t origAttr, uint8_t page);
|
||||
uint8_t setAttrBlink(uint8_t origAttr, uint8_t page);
|
||||
uint8_t setAttrVersion(uint8_t origAttr, uint8_t page);
|
||||
|
|
@ -5404,6 +5404,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
|
|||
static uint8_t statsCurrentPage = 0;
|
||||
static bool statsDisplayed = false;
|
||||
static bool statsAutoPagingEnabled = true;
|
||||
static bool isThrottleHigh = false;
|
||||
|
||||
// Detect arm/disarm
|
||||
if (armState != ARMING_FLAG(ARMED)) {
|
||||
|
|
@ -5429,6 +5430,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
|
|||
statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false;
|
||||
osdShowStats(statsSinglePageCompatible, statsCurrentPage);
|
||||
osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);
|
||||
isThrottleHigh = checkStickPosition(THR_HI);
|
||||
}
|
||||
|
||||
armState = ARMING_FLAG(ARMED);
|
||||
|
|
@ -5494,7 +5496,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
// Handle events when either "Splash", "Armed" or "Stats" screens are displayed.
|
||||
if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) {
|
||||
if (currentTimeUs > resumeRefreshAt || (OSD_RESUME_UPDATES_STICK_COMMAND && !isThrottleHigh)) {
|
||||
// Time elapsed or canceled by stick commands.
|
||||
// Exit to normal OSD operation.
|
||||
displayClearScreen(osdDisplayPort);
|
||||
|
|
@ -5503,6 +5505,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
|
|||
} else {
|
||||
// Continue "Splash", "Armed" or "Stats" screens.
|
||||
displayHeartbeat(osdDisplayPort);
|
||||
isThrottleHigh = checkStickPosition(THR_HI);
|
||||
}
|
||||
|
||||
return;
|
||||
|
|
@ -5721,7 +5724,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
|
|||
|
||||
else {
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (canFwLandCanceld()) {
|
||||
if (canFwLandingBeCancelled()) {
|
||||
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS);
|
||||
} else if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) {
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -122,8 +122,8 @@ int8_t radarGetNearestPOI(void)
|
|||
*/
|
||||
void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, uint8_t poiType, uint16_t poiSymbol, int16_t poiP1, int16_t poiP2)
|
||||
{
|
||||
int poi_x;
|
||||
int poi_y;
|
||||
int poi_x = -1;
|
||||
int poi_y = -1;
|
||||
uint8_t center_x;
|
||||
uint8_t center_y;
|
||||
bool poi_is_oos = 0;
|
||||
|
|
|
|||
|
|
@ -31,6 +31,8 @@
|
|||
|
||||
extern virtualRangefinderVTable_t rangefinderMSPVtable;
|
||||
extern virtualRangefinderVTable_t rangefinderBenewakeVtable;
|
||||
extern virtualRangefinderVTable_t rangefinderUSD1Vtable;
|
||||
extern virtualRangefinderVTable_t rangefinderNanoradarVtable; //NRA15/NRA24
|
||||
extern virtualRangefinderVTable_t rangefinderFakeVtable;
|
||||
|
||||
void mspRangefinderReceiveNewData(uint8_t * bufferPtr);
|
||||
|
|
|
|||
167
src/main/io/rangefinder_nanoradar.c
Normal file
167
src/main/io/rangefinder_nanoradar.c
Normal file
|
|
@ -0,0 +1,167 @@
|
|||
/*
|
||||
* This file is part of INAV Project.
|
||||
*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
|
||||
* You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*
|
||||
* Alternatively, the contents of this file may be used under the terms
|
||||
* of the GNU General Public License Version 3, as described below:
|
||||
*
|
||||
* This file is free software: you may copy, redistribute and/or modify
|
||||
* it under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or (at your
|
||||
* option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
|
||||
* Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see http://www.gnu.org/licenses/.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <ctype.h>
|
||||
|
||||
#include "platform.h"
|
||||
#include "io/serial.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#if defined(USE_RANGEFINDER_NANORADAR)
|
||||
#include "drivers/rangefinder/rangefinder_virtual.h"
|
||||
|
||||
#define NANORADAR_HDR 0xAA // Header Byte
|
||||
#define NANORADAR_END 0x55
|
||||
|
||||
#define NANORADAR_COMMAND_TARGET_INFO 0x70C
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint8_t header0;
|
||||
uint8_t header1;
|
||||
uint8_t commandH;
|
||||
uint8_t commandL;
|
||||
uint8_t index; // Target ID
|
||||
uint8_t rcs; // The section of radar reflection
|
||||
uint8_t rangeH; // Target distance high 8 bi
|
||||
uint8_t rangeL; // Target distance low 8 bit
|
||||
uint8_t rsvd1;
|
||||
uint8_t info; // VrelH Rsvd1 RollCount
|
||||
uint8_t vrelL;
|
||||
uint8_t SNR; // Signal-Noise Ratio
|
||||
uint8_t end0;
|
||||
uint8_t end1;
|
||||
} nanoradarPacket_t;
|
||||
|
||||
#define NANORADAR_PACKET_SIZE sizeof(nanoradarPacket_t)
|
||||
#define NANORADAR_TIMEOUT_MS 2000 // 2s
|
||||
|
||||
static bool hasNewData = false;
|
||||
static bool hasEverData = false;
|
||||
static serialPort_t * serialPort = NULL;
|
||||
static serialPortConfig_t * portConfig;
|
||||
static uint8_t buffer[NANORADAR_PACKET_SIZE];
|
||||
static unsigned bufferPtr;
|
||||
|
||||
static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
|
||||
static timeMs_t lastProtocolActivityMs;
|
||||
|
||||
static bool nanoradarRangefinderDetect(void)
|
||||
{
|
||||
portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
|
||||
if (!portConfig) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void nanoradarRangefinderInit(void)
|
||||
{
|
||||
if (!portConfig) {
|
||||
return;
|
||||
}
|
||||
|
||||
serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
|
||||
if (!serialPort) {
|
||||
return;
|
||||
}
|
||||
|
||||
lastProtocolActivityMs = 0;
|
||||
}
|
||||
|
||||
static void nanoradarRangefinderUpdate(void)
|
||||
{
|
||||
|
||||
nanoradarPacket_t *nanoradarPacket = (nanoradarPacket_t *)buffer;
|
||||
while (serialRxBytesWaiting(serialPort) > 0) {
|
||||
uint8_t c = serialRead(serialPort);
|
||||
|
||||
if (bufferPtr < NANORADAR_PACKET_SIZE) {
|
||||
buffer[bufferPtr++] = c;
|
||||
}
|
||||
|
||||
if ((bufferPtr == 1) && (nanoradarPacket->header0 != NANORADAR_HDR)) {
|
||||
bufferPtr = 0;
|
||||
continue;
|
||||
}
|
||||
|
||||
if ((bufferPtr == 2) && (nanoradarPacket->header1 != NANORADAR_HDR)) {
|
||||
bufferPtr = 0;
|
||||
continue;
|
||||
}
|
||||
|
||||
//only target info packet we are interested
|
||||
if (bufferPtr == 4) {
|
||||
uint16_t command = nanoradarPacket->commandH + (nanoradarPacket->commandL * 0x100);
|
||||
|
||||
if (command != NANORADAR_COMMAND_TARGET_INFO) {
|
||||
bufferPtr = 0;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// Check for complete packet
|
||||
if (bufferPtr == NANORADAR_PACKET_SIZE) {
|
||||
if (nanoradarPacket->end0 == NANORADAR_END && nanoradarPacket->end1 == NANORADAR_END) {
|
||||
// Valid packet
|
||||
hasNewData = true;
|
||||
hasEverData = true;
|
||||
lastProtocolActivityMs = millis();
|
||||
|
||||
sensorData = ((nanoradarPacket->rangeH * 0x100) + nanoradarPacket->rangeL);
|
||||
}
|
||||
|
||||
// Prepare for new packet
|
||||
bufferPtr = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static int32_t nanoradarRangefinderGetDistance(void)
|
||||
{
|
||||
int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE;
|
||||
|
||||
if (hasNewData) {
|
||||
hasNewData = false;
|
||||
return altitude;
|
||||
}
|
||||
else {
|
||||
//keep last value for timeout, because radar sends data only if change
|
||||
if ((millis() - lastProtocolActivityMs) < NANORADAR_TIMEOUT_MS) {
|
||||
return altitude;
|
||||
}
|
||||
|
||||
return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA;
|
||||
}
|
||||
}
|
||||
|
||||
virtualRangefinderVTable_t rangefinderNanoradarVtable = {
|
||||
.detect = nanoradarRangefinderDetect,
|
||||
.init = nanoradarRangefinderInit,
|
||||
.update = nanoradarRangefinderUpdate,
|
||||
.read = nanoradarRangefinderGetDistance
|
||||
};
|
||||
|
||||
#endif
|
||||
136
src/main/io/rangefinder_usd1_v0.c
Normal file
136
src/main/io/rangefinder_usd1_v0.c
Normal file
|
|
@ -0,0 +1,136 @@
|
|||
/*
|
||||
* This file is part of INAV Project.
|
||||
*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
|
||||
* You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*
|
||||
* Alternatively, the contents of this file may be used under the terms
|
||||
* of the GNU General Public License Version 3, as described below:
|
||||
*
|
||||
* This file is free software: you may copy, redistribute and/or modify
|
||||
* it under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or (at your
|
||||
* option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
|
||||
* Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see http://www.gnu.org/licenses/.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <ctype.h>
|
||||
|
||||
|
||||
#include "platform.h"
|
||||
#include "io/serial.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#if defined(USE_RANGEFINDER_USD1_V0)
|
||||
#include "drivers/rangefinder/rangefinder_virtual.h"
|
||||
|
||||
#define USD1_HDR_V0 72 // Header Byte for beta V0 of USD1_Serial (0x48)
|
||||
|
||||
#define USD1_PACKET_SIZE 3
|
||||
#define USD1_KEEP_DATA_TIMEOUT 2000 // 2s
|
||||
|
||||
|
||||
static serialPort_t * serialPort = NULL;
|
||||
static serialPortConfig_t * portConfig;
|
||||
|
||||
static bool hasNewData = false;
|
||||
static bool hasEverData = false;
|
||||
static uint8_t lineBuf[USD1_PACKET_SIZE];
|
||||
static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
|
||||
static timeMs_t lastProtocolActivityMs;
|
||||
|
||||
static bool usd1RangefinderDetect(void)
|
||||
{
|
||||
portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
|
||||
if (!portConfig) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void usd1RangefinderInit(void)
|
||||
{
|
||||
if (!portConfig) {
|
||||
return;
|
||||
}
|
||||
|
||||
serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
|
||||
if (!serialPort) {
|
||||
return;
|
||||
}
|
||||
|
||||
lastProtocolActivityMs = 0;
|
||||
}
|
||||
|
||||
static void usd1RangefinderUpdate(void)
|
||||
{
|
||||
float sum = 0;
|
||||
uint16_t count = 0;
|
||||
uint8_t index = 0;
|
||||
|
||||
while (serialRxBytesWaiting(serialPort) > 0) {
|
||||
uint8_t c = serialRead(serialPort);
|
||||
|
||||
if (c == USD1_HDR_V0 && index == 0) {
|
||||
lineBuf[index] = c;
|
||||
index = 1;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (index > 0) {
|
||||
lineBuf[index] = c;
|
||||
index++;
|
||||
if (index == 3) {
|
||||
index = 0;
|
||||
sum += (float)((lineBuf[2]&0x7F) * 128 + (lineBuf[1]&0x7F));
|
||||
count++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (count == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
hasNewData = true;
|
||||
hasEverData = true;
|
||||
lastProtocolActivityMs = millis();
|
||||
sensorData = (int32_t)(2.5f * sum / (float)count);
|
||||
}
|
||||
|
||||
static int32_t usd1RangefinderGetDistance(void)
|
||||
{
|
||||
int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE;
|
||||
|
||||
if (hasNewData) {
|
||||
hasNewData = false;
|
||||
return altitude;
|
||||
}
|
||||
else {
|
||||
//keep last value for timeout, because radar sends data only if change
|
||||
if ((millis() - lastProtocolActivityMs) < USD1_KEEP_DATA_TIMEOUT) {
|
||||
return altitude;
|
||||
}
|
||||
|
||||
return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA;
|
||||
}
|
||||
}
|
||||
|
||||
virtualRangefinderVTable_t rangefinderUSD1Vtable = {
|
||||
.detect = usd1RangefinderDetect,
|
||||
.init = usd1RangefinderInit,
|
||||
.update = usd1RangefinderUpdate,
|
||||
.read = usd1RangefinderGetDistance
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -27,6 +27,11 @@
|
|||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
#include "target/SITL/serial_proxy.h"
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef SOFTSERIAL_LOOPBACK
|
||||
serialPort_t *loopbackPort;
|
||||
#endif
|
||||
|
|
@ -65,6 +70,9 @@ int main(void)
|
|||
loopbackInit();
|
||||
|
||||
while (true) {
|
||||
#if defined(SITL_BUILD)
|
||||
serialProxyProcess();
|
||||
#endif
|
||||
scheduler();
|
||||
processLoopback();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -328,8 +328,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState);
|
||||
|
|
@ -356,6 +356,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
|
|||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState);
|
||||
#endif
|
||||
|
||||
|
|
@ -423,7 +424,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_HOLD_INFINIT,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
|
|
@ -438,7 +439,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_HOLD_INFINIT,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
|
|
@ -585,7 +586,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK] = NAV_STATE_RTH_TRACKBACK,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
}
|
||||
},
|
||||
|
|
@ -594,7 +595,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, // allow pos adjustment while climbind to safe alt
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, // allow pos adjustment while climbing to safe alt
|
||||
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_RTH_CLIMB,
|
||||
.mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT,
|
||||
|
|
@ -640,7 +641,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
|
||||
[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,
|
||||
|
|
@ -651,18 +652,18 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
|
||||
[NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
|
||||
.timeoutMs = 500,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
|
||||
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_LAND_SETTLE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME] = NAV_STATE_RTH_LOITER_ABOVE_HOME,
|
||||
[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,
|
||||
|
|
@ -672,16 +673,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_RTH_HOVER_ABOVE_HOME] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME,
|
||||
[NAV_STATE_RTH_LOITER_ABOVE_HOME] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
|
||||
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LOITER_ABOVE_HOME,
|
||||
[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,
|
||||
|
|
@ -695,7 +696,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_RTH_LANDING,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
|
||||
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
|
||||
.mwError = MW_NAV_ERROR_LANDING,
|
||||
|
|
@ -708,7 +709,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME] = NAV_STATE_RTH_LOITER_ABOVE_HOME,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -716,7 +717,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_RTH_FINISHING,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
|
||||
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
|
||||
.mwError = MW_NAV_ERROR_LANDING,
|
||||
|
|
@ -827,7 +828,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME, // There is no state for timed hold?
|
||||
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_HOLD_TIMED,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
|
|
@ -848,7 +849,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
|
||||
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
|
||||
.mwError = MW_NAV_ERROR_LANDING,
|
||||
|
|
@ -886,7 +887,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
|
||||
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_WP_ENROUTE,
|
||||
.mwError = MW_NAV_ERROR_FINISH,
|
||||
|
|
@ -925,7 +926,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
|
||||
.stateFlags = NAV_CTL_HOLD | NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
|
||||
.mapToFlightModes = 0,
|
||||
.mwState = MW_NAV_STATE_EMERGENCY_LANDING,
|
||||
.mwError = MW_NAV_ERROR_LANDING,
|
||||
|
|
@ -943,7 +944,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
|
||||
.stateFlags = NAV_CTL_HOLD | NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
|
||||
.mapToFlightModes = 0,
|
||||
.mwState = MW_NAV_STATE_LANDED,
|
||||
.mwError = MW_NAV_ERROR_LANDING,
|
||||
|
|
@ -1052,7 +1053,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
|
||||
.mapToFlightModes = NAV_FW_AUTOLAND,
|
||||
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
|
|
@ -1073,7 +1074,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_LOITER,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_LOITER,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
|
||||
.mapToFlightModes = NAV_FW_AUTOLAND,
|
||||
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
|
|
@ -1107,6 +1108,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED] = NAV_STATE_FW_LANDING_FINISHED,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
|
||||
}
|
||||
},
|
||||
|
|
@ -1128,6 +1130,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED] = NAV_STATE_FW_LANDING_FINISHED,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
|
||||
}
|
||||
},
|
||||
|
|
@ -1142,10 +1145,25 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_FLARE, // re-process the state
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED] = NAV_STATE_FW_LANDING_FINISHED,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_FW_LANDING_FINISHED] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_FINISHED,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_REQUIRE_ANGLE,
|
||||
.mapToFlightModes = NAV_FW_AUTOLAND,
|
||||
.mwState = MW_NAV_STATE_LANDED,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_FINISHED, // re-process the state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_FW_LANDING_ABORT] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_ABORT,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_ABORT,
|
||||
|
|
@ -1246,7 +1264,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
|
|||
}
|
||||
|
||||
// Prepare position controller if idle or current Mode NOT active in position hold state
|
||||
if (previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME &&
|
||||
if (previousState != NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_LOITER_ABOVE_HOME &&
|
||||
previousState != NAV_STATE_RTH_LANDING && previousState != NAV_STATE_WAYPOINT_RTH_LAND &&
|
||||
previousState != NAV_STATE_WAYPOINT_FINISHED && previousState != NAV_STATE_WAYPOINT_HOLD_TIME)
|
||||
{
|
||||
|
|
@ -1427,9 +1445,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
|
||||
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
|
||||
return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
|
||||
} else {
|
||||
if (rthTrackBackIsActive() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING
|
||||
}
|
||||
else {
|
||||
// Switch to RTH trackback
|
||||
bool trackbackActive = navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON ||
|
||||
(navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated);
|
||||
|
||||
if (trackbackActive && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
|
||||
rthTrackBackUpdate(true); // save final trackpoint for altitude and max trackback distance reference
|
||||
posControl.flags.rthTrackbackActive = true;
|
||||
calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition());
|
||||
|
|
@ -1496,8 +1519,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
initializeRTHSanityChecker();
|
||||
}
|
||||
|
||||
// Save initial home distance for future use
|
||||
// Save initial home distance and direction for future use
|
||||
posControl.rthState.rthInitialDistance = posControl.homeDistance;
|
||||
posControl.activeWaypoint.bearing = posControl.homeDirection;
|
||||
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
|
||||
|
||||
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
|
||||
|
|
@ -1583,7 +1607,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
|
||||
|
||||
if (isWaypointReached(tmpHomePos, 0)) {
|
||||
if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
|
||||
// Successfully reached position target - update XYZ-position
|
||||
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
|
||||
|
|
@ -1592,7 +1616,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
|
||||
posControl.rthState.rthLinearDescentActive = false;
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
|
||||
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING
|
||||
} else {
|
||||
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
|
|
@ -1606,7 +1630,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
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_LOITER_PRIOR_TO_LANDING(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
|
|
@ -1648,7 +1672,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
|
|||
}
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState)
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
|
|
@ -1657,7 +1681,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
|
|||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
|
||||
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LOITER);
|
||||
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
|
|
@ -1665,9 +1689,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
|
|||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState)
|
||||
{
|
||||
#ifndef USE_FW_AUTOLAND
|
||||
UNUSED(previousState);
|
||||
#endif
|
||||
|
||||
//On ROVER and BOAT we immediately switch to the next event
|
||||
if (!STATE(ALTITUDE_CONTROL)) {
|
||||
|
|
@ -1680,7 +1702,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
|
||||
/* 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() || (previousState != NAV_STATE_WAYPOINT_REACHED && !validateRTHSanityChecker())) {
|
||||
if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || (FLIGHT_MODE(NAV_RTH_MODE) && !validateRTHSanityChecker())) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
|
|
@ -1699,7 +1721,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
shIdx = posControl.safehomeState.index;
|
||||
missionFwLandConfigStartIdx = MAX_SAFE_HOMES;
|
||||
#endif
|
||||
if (previousState == NAV_STATE_WAYPOINT_REACHED && missionIdx >= 0) {
|
||||
if (FLIGHT_MODE(NAV_WP_MODE) && missionIdx >= 0) {
|
||||
approachSettingIdx = missionFwLandConfigStartIdx + missionIdx;
|
||||
} else if (shIdx >= 0) {
|
||||
approachSettingIdx = shIdx;
|
||||
|
|
@ -1707,7 +1729,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
|
||||
if (!posControl.fwLandState.landAborted && approachSettingIdx >= 0 && (fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading2 != 0)) {
|
||||
|
||||
if (previousState == NAV_STATE_WAYPOINT_REACHED) {
|
||||
if (FLIGHT_MODE(NAV_WP_MODE)) {
|
||||
posControl.fwLandState.landPos = posControl.activeWaypoint.pos;
|
||||
posControl.fwLandState.landWp = true;
|
||||
} else {
|
||||
|
|
@ -2006,8 +2028,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
|
|||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (posControl.fwLandState.landAborted) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
|
||||
|
|
@ -2016,21 +2036,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig
|
|||
|
||||
const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (landEvent == NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
|
||||
} else
|
||||
#endif
|
||||
if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
|
||||
} else if (landEvent == NAV_FSM_EVENT_SUCCESS) {
|
||||
// Landing controller returned success - invoke RTH finishing state and finish the waypoint
|
||||
if (landEvent == NAV_FSM_EVENT_SUCCESS) {
|
||||
// Landing controller returned success - invoke RTH finish states and finish the waypoint
|
||||
navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState);
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
else {
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
navOnEnteringState_NAV_STATE_RTH_FINISHED(previousState);
|
||||
}
|
||||
|
||||
return landEvent;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState)
|
||||
|
|
@ -2341,6 +2353,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (STATE(LANDING_DETECTED)) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED;
|
||||
}
|
||||
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
|
@ -2349,12 +2365,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
|
|||
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
|
||||
}
|
||||
|
||||
if (isLandingDetected()) {
|
||||
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
|
||||
//disarm(DISARM_LANDING);
|
||||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
}
|
||||
|
||||
if (getLandAltitude() <= fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) {
|
||||
resetPositionController();
|
||||
posControl.cruise.course = posControl.fwLandState.landingDirection;
|
||||
|
|
@ -2391,6 +2401,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (STATE(LANDING_DETECTED)) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED;
|
||||
}
|
||||
|
||||
if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
|
||||
}
|
||||
|
|
@ -2400,12 +2414,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
|
|||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
if (isLandingDetected()) {
|
||||
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
|
||||
//disarm(DISARM_LANDING);
|
||||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
}
|
||||
|
||||
setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
|
@ -2414,16 +2422,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (isLandingDetected()) {
|
||||
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
|
||||
//disarm(DISARM_LANDING);
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
if (STATE(LANDING_DETECTED)) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED;
|
||||
}
|
||||
|
||||
setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
|
|
@ -2540,7 +2556,7 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
|
|||
posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
|
||||
break;
|
||||
|
||||
case RTH_HOME_FINAL_HOVER:
|
||||
case RTH_HOME_FINAL_LOITER:
|
||||
if (navConfig()->general.rth_home_altitude) {
|
||||
posControl.rthState.homeTmpWaypoint.z = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
|
||||
}
|
||||
|
|
@ -2825,30 +2841,28 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
|
|||
|
||||
/*-----------------------------------------------------------
|
||||
* Check if waypoint is/was reached.
|
||||
* waypointBearing stores initial bearing to waypoint
|
||||
* 'waypointBearing' stores initial bearing to waypoint.
|
||||
*-----------------------------------------------------------*/
|
||||
bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBearing)
|
||||
{
|
||||
posControl.wpDistance = calculateDistanceToDestination(waypointPos);
|
||||
|
||||
// Airplane will do a circular loiter at hold waypoints and might never approach them closer than waypoint_radius
|
||||
// Check within 10% margin of circular loiter radius
|
||||
if (STATE(AIRPLANE) && isNavHoldPositionActive() && posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)) {
|
||||
return true;
|
||||
}
|
||||
// Check if waypoint was missed based on bearing to waypoint exceeding given angular limit relative to initial waypoint bearing.
|
||||
// Default angular limit = 100 degs with a reduced limit of 60 degs used if fixed wing waypoint turn smoothing option active
|
||||
uint16_t relativeBearingTargetAngle = 10000;
|
||||
|
||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) {
|
||||
// If WP turn smoothing CUT option used WP is reached when start of turn is initiated
|
||||
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) {
|
||||
if (STATE(AIRPLANE) && posControl.flags.wpTurnSmoothingActive) {
|
||||
// If WP mode turn smoothing CUT option used waypoint is reached when start of turn is initiated
|
||||
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) {
|
||||
posControl.flags.wpTurnSmoothingActive = false;
|
||||
return true;
|
||||
}
|
||||
// Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw
|
||||
// Same method for turn smoothing option but relative bearing set at 60 degrees
|
||||
uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000;
|
||||
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearing) {
|
||||
return true;
|
||||
relativeBearingTargetAngle = 6000;
|
||||
}
|
||||
|
||||
|
||||
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearingTargetAngle) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return posControl.wpDistance <= (navConfig()->general.waypoint_radius);
|
||||
|
|
@ -3333,9 +3347,6 @@ void updateLandingStatus(timeMs_t currentTimeMs)
|
|||
}
|
||||
resetLandingDetector();
|
||||
|
||||
if (!IS_RC_MODE_ACTIVE(BOXARM)) {
|
||||
DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -3966,16 +3977,20 @@ bool isLastMissionWaypoint(void)
|
|||
/* Checks if Nav hold position is active */
|
||||
bool isNavHoldPositionActive(void)
|
||||
{
|
||||
// WP mode last WP hold and Timed hold positions
|
||||
if (FLIGHT_MODE(NAV_WP_MODE)) {
|
||||
return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
|
||||
/* If the current Nav state isn't flagged as a hold point (NAV_CTL_HOLD) then
|
||||
* waypoints are assumed to be hold points by default unless excluded as defined here */
|
||||
|
||||
if (navGetCurrentStateFlags() & NAV_CTL_HOLD) {
|
||||
return true;
|
||||
}
|
||||
// RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode
|
||||
// Also hold position during emergency landing if position valid
|
||||
return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||
|
||||
FLIGHT_MODE(NAV_POSHOLD_MODE) ||
|
||||
(posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) ||
|
||||
navigationIsExecutingAnEmergencyLanding();
|
||||
|
||||
// No hold required for basic WP type unless it's the last mission waypoint
|
||||
if (FLIGHT_MODE(NAV_WP_MODE)) {
|
||||
return posControl.waypointList[posControl.activeWaypointIndex].action != NAV_WP_ACTION_WAYPOINT || isLastMissionWaypoint();
|
||||
}
|
||||
|
||||
// No hold required for Trackback WPs or for fixed wing autoland WPs not flagged as hold points (returned above if they are)
|
||||
return !FLIGHT_MODE(NAV_FW_AUTOLAND) && !posControl.flags.rthTrackbackActive;
|
||||
}
|
||||
|
||||
float getActiveSpeed(void)
|
||||
|
|
@ -4814,9 +4829,9 @@ void abortForcedRTH(void)
|
|||
|
||||
rthState_e getStateOfForcedRTH(void)
|
||||
{
|
||||
/* If forced RTH activated and in AUTO_RTH or EMERG state */
|
||||
/* If forced RTH activated and in AUTO_RTH, EMERG state or FW Auto Landing */
|
||||
if (posControl.flags.forcedRTHActivated && ((navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT)) || FLIGHT_MODE(NAV_FW_AUTOLAND))) {
|
||||
if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
|
||||
if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED || posControl.navState == NAV_STATE_FW_LANDING_FINISHED) {
|
||||
return RTH_HAS_LANDED;
|
||||
}
|
||||
else {
|
||||
|
|
@ -5060,12 +5075,9 @@ void resetFwAutolandApproach(int8_t idx)
|
|||
}
|
||||
}
|
||||
|
||||
bool canFwLandCanceld(void)
|
||||
bool canFwLandingBeCancelled(void)
|
||||
{
|
||||
return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER
|
||||
|| posControl.navState == NAV_STATE_FW_LANDING_LOITER
|
||||
|| posControl.navState == NAV_STATE_FW_LANDING_APPROACH
|
||||
|| posControl.navState == NAV_STATE_FW_LANDING_GLIDE;
|
||||
return FLIGHT_MODE(NAV_FW_AUTOLAND) && posControl.navState != NAV_STATE_FW_LANDING_FLARE;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -703,7 +703,7 @@ uint8_t getActiveWpNumber(void);
|
|||
int32_t navigationGetHomeHeading(void);
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
bool canFwLandCanceld(void);
|
||||
bool canFwLandingBeCancelled(void);
|
||||
#endif
|
||||
|
||||
/* Compatibility data */
|
||||
|
|
|
|||
|
|
@ -274,9 +274,12 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
|||
// Detemine if a circular loiter is required.
|
||||
// For waypoints only use circular loiter when angular visibility is > 30 degs, otherwise head straight toward target
|
||||
#define TAN_15DEG 0.26795f
|
||||
needToCalculateCircularLoiter = isNavHoldPositionActive() &&
|
||||
(distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
|
||||
(distanceToActualTarget > 50.0f);
|
||||
|
||||
bool loiterApproachActive = isNavHoldPositionActive() &&
|
||||
distanceToActualTarget <= (navLoiterRadius / TAN_15DEG) &&
|
||||
distanceToActualTarget > 50.0f;
|
||||
needToCalculateCircularLoiter = loiterApproachActive || (navGetCurrentStateFlags() & NAV_CTL_HOLD);
|
||||
|
||||
//if vtol landing is required, fly straight to homepoint
|
||||
if ((posControl.navState == NAV_STATE_RTH_HEAD_HOME) && navigationRTHAllowsLanding() && checkMixerATRequired(MIXERAT_REQUEST_LAND)){
|
||||
needToCalculateCircularLoiter = false;
|
||||
|
|
|
|||
|
|
@ -166,16 +166,18 @@ typedef enum {
|
|||
NAV_FSM_EVENT_STATE_SPECIFIC_3, // State-specific event
|
||||
NAV_FSM_EVENT_STATE_SPECIFIC_4, // State-specific event
|
||||
NAV_FSM_EVENT_STATE_SPECIFIC_5, // State-specific event
|
||||
NAV_FSM_EVENT_STATE_SPECIFIC_6, // State-specific event
|
||||
|
||||
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT = NAV_FSM_EVENT_STATE_SPECIFIC_1,
|
||||
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,
|
||||
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_1,
|
||||
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_2,
|
||||
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_3,
|
||||
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_1,
|
||||
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_2,
|
||||
NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
|
||||
NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1,
|
||||
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1,
|
||||
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,
|
||||
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
|
||||
NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4,
|
||||
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_5,
|
||||
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_6,
|
||||
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT = NAV_FSM_EVENT_STATE_SPECIFIC_6,
|
||||
NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4,
|
||||
NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_5,
|
||||
|
||||
NAV_FSM_EVENT_COUNT,
|
||||
} navigationFSMEvent_t;
|
||||
|
||||
|
|
@ -198,7 +200,7 @@ typedef enum {
|
|||
NAV_PERSISTENT_ID_RTH_INITIALIZE = 8,
|
||||
NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT = 9,
|
||||
NAV_PERSISTENT_ID_RTH_HEAD_HOME = 10,
|
||||
NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING = 11,
|
||||
NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING = 11,
|
||||
NAV_PERSISTENT_ID_RTH_LANDING = 12,
|
||||
NAV_PERSISTENT_ID_RTH_FINISHING = 13,
|
||||
NAV_PERSISTENT_ID_RTH_FINISHED = 14,
|
||||
|
|
@ -229,7 +231,7 @@ typedef enum {
|
|||
NAV_PERSISTENT_ID_CRUISE_ADJUSTING = 34,
|
||||
|
||||
NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35,
|
||||
NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36,
|
||||
NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME = 36,
|
||||
NAV_PERSISTENT_ID_UNUSED_4 = 37, // was NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME
|
||||
NAV_PERSISTENT_ID_RTH_TRACKBACK = 38,
|
||||
|
||||
|
|
@ -242,6 +244,7 @@ typedef enum {
|
|||
NAV_PERSISTENT_ID_FW_LANDING_GLIDE = 45,
|
||||
NAV_PERSISTENT_ID_FW_LANDING_FLARE = 46,
|
||||
NAV_PERSISTENT_ID_FW_LANDING_ABORT = 47,
|
||||
NAV_PERSISTENT_ID_FW_LANDING_FINISHED = 48,
|
||||
} navigationPersistentId_e;
|
||||
|
||||
typedef enum {
|
||||
|
|
@ -259,8 +262,8 @@ typedef enum {
|
|||
NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
|
||||
NAV_STATE_RTH_TRACKBACK,
|
||||
NAV_STATE_RTH_HEAD_HOME,
|
||||
NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
|
||||
NAV_STATE_RTH_HOVER_ABOVE_HOME,
|
||||
NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
|
||||
NAV_STATE_RTH_LOITER_ABOVE_HOME,
|
||||
NAV_STATE_RTH_LANDING,
|
||||
NAV_STATE_RTH_FINISHING,
|
||||
NAV_STATE_RTH_FINISHED,
|
||||
|
|
@ -294,6 +297,7 @@ typedef enum {
|
|||
NAV_STATE_FW_LANDING_APPROACH,
|
||||
NAV_STATE_FW_LANDING_GLIDE,
|
||||
NAV_STATE_FW_LANDING_FLARE,
|
||||
NAV_STATE_FW_LANDING_FINISHED,
|
||||
NAV_STATE_FW_LANDING_ABORT,
|
||||
|
||||
NAV_STATE_MIXERAT_INITIALIZE,
|
||||
|
|
@ -328,9 +332,10 @@ typedef enum {
|
|||
|
||||
/* Additional flags */
|
||||
NAV_CTL_LAND = (1 << 14),
|
||||
NAV_AUTO_WP_DONE = (1 << 15), //Waypoint mission reached the last waypoint and is idling
|
||||
NAV_AUTO_WP_DONE = (1 << 15), // Waypoint mission reached the last waypoint and is idling
|
||||
|
||||
NAV_MIXERAT = (1 << 16), //MIXERAT in progress
|
||||
NAV_MIXERAT = (1 << 16), // MIXERAT in progress
|
||||
NAV_CTL_HOLD = (1 << 17), // Nav loiter active at position
|
||||
} navigationFSMStateFlags_t;
|
||||
|
||||
typedef struct {
|
||||
|
|
@ -397,7 +402,7 @@ typedef enum {
|
|||
RTH_HOME_ENROUTE_INITIAL, // Initial position for RTH approach
|
||||
RTH_HOME_ENROUTE_PROPORTIONAL, // Prorpotional position for RTH approach
|
||||
RTH_HOME_ENROUTE_FINAL, // Final position for RTH approach
|
||||
RTH_HOME_FINAL_HOVER, // Final hover altitude (if rth_home_altitude is set)
|
||||
RTH_HOME_FINAL_LOITER, // Final loiter altitude (if rth_home_altitude is set)
|
||||
RTH_HOME_FINAL_LAND, // Home position and altitude
|
||||
} rthTargetMode_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -31,15 +31,14 @@
|
|||
|
||||
static uint16_t channels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
static bool hasNewData = false;
|
||||
static uint16_t rssi = 0;
|
||||
|
||||
static uint16_t rxSimReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan)
|
||||
{
|
||||
static uint16_t rxSimReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) {
|
||||
UNUSED(rxRuntimeConfigPtr);
|
||||
return channels[chan];
|
||||
}
|
||||
|
||||
void rxSimSetChannelValue(uint16_t* values, uint8_t count)
|
||||
{
|
||||
void rxSimSetChannelValue(uint16_t* values, uint8_t count) {
|
||||
for (size_t i = 0; i < count; i++) {
|
||||
channels[i] = values[i];
|
||||
}
|
||||
|
|
@ -47,10 +46,11 @@ void rxSimSetChannelValue(uint16_t* values, uint8_t count)
|
|||
hasNewData = true;
|
||||
}
|
||||
|
||||
static uint8_t rxSimFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
static uint8_t rxSimFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) {
|
||||
UNUSED(rxRuntimeConfig);
|
||||
|
||||
lqTrackerSet(rxRuntimeConfig->lqTracker, rssi);
|
||||
|
||||
if (!hasNewData) {
|
||||
return RX_FRAME_PENDING;
|
||||
}
|
||||
|
|
@ -59,8 +59,7 @@ static uint8_t rxSimFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
|
|||
return RX_FRAME_COMPLETE;
|
||||
}
|
||||
|
||||
void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) {
|
||||
UNUSED(rxConfig);
|
||||
|
||||
rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT;
|
||||
|
|
@ -68,4 +67,8 @@ void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
|||
rxRuntimeConfig->rcReadRawFn = rxSimReadRawRC;
|
||||
rxRuntimeConfig->rcFrameStatusFn = rxSimFrameStatus;
|
||||
}
|
||||
|
||||
void rxSimSetRssi(uint16_t value) {
|
||||
rssi = value;
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -20,4 +20,5 @@
|
|||
#include "rx/rx.h"
|
||||
|
||||
void rxSimSetChannelValue(uint16_t* values, uint8_t count);
|
||||
void rxSimSetRssi(uint16_t value);
|
||||
void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
|
||||
|
|
|
|||
|
|
@ -130,6 +130,22 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
|
|||
rangefinderHardware = RANGEFINDER_BENEWAKE;
|
||||
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case RANGEFINDER_USD1_V0:
|
||||
#if defined(USE_RANGEFINDER_USD1_V0)
|
||||
if (virtualRangefinderDetect(dev, &rangefinderUSD1Vtable)) {
|
||||
rangefinderHardware = RANGEFINDER_USD1_V0;
|
||||
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case RANGEFINDER_NANORADAR:
|
||||
#if defined(USE_RANGEFINDER_NANORADAR)
|
||||
if (virtualRangefinderDetect(dev, &rangefinderNanoradarVtable)) {
|
||||
rangefinderHardware = RANGEFINDER_NANORADAR;
|
||||
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
|
|
|
|||
|
|
@ -32,6 +32,8 @@ typedef enum {
|
|||
RANGEFINDER_TOF10102I2C = 7,
|
||||
RANGEFINDER_FAKE = 8,
|
||||
RANGEFINDER_TERARANGER_EVO = 9,
|
||||
RANGEFINDER_USD1_V0 = 10,
|
||||
RANGEFINDER_NANORADAR = 11,
|
||||
} rangefinderType_e;
|
||||
|
||||
typedef struct rangefinderConfig_s {
|
||||
|
|
|
|||
|
|
@ -30,13 +30,22 @@
|
|||
#include "drivers/pinio.h"
|
||||
|
||||
timerHardware_t timerHardware[] = {
|
||||
// DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6
|
||||
// DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM,
|
||||
// DMA2_ST6
|
||||
|
||||
#ifdef FLYWOOF745
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST2
|
||||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA2_ST7
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M3 , DMA1_ST7
|
||||
DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4
|
||||
DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M5 , DMA2_ST2
|
||||
#else /* FLYWOOF745NANO */
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST7
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA1_ST2
|
||||
DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 , DMA2_ST2
|
||||
DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4
|
||||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M5 , DMA2_ST7
|
||||
#endif
|
||||
DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // M6 , DMA1_ST1
|
||||
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // M7 , DMA1_ST4
|
||||
DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // M8 , DMA1_ST5
|
||||
|
|
|
|||
|
|
@ -57,7 +57,4 @@ void targetConfiguration(void)
|
|||
|
||||
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP;
|
||||
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200;
|
||||
|
||||
// To improve backwards compatibility with INAV versions 6.x and older
|
||||
timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS;
|
||||
}
|
||||
|
|
|
|||
784
src/main/target/SITL/serial_proxy.c
Normal file
784
src/main/target/SITL/serial_proxy.c
Normal file
|
|
@ -0,0 +1,784 @@
|
|||
/*
|
||||
* This file is part of INAV Project.
|
||||
*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
|
||||
* You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*
|
||||
* Alternatively, the contents of this file may be used under the terms
|
||||
* of the GNU General Public License Version 3, as described below:
|
||||
*
|
||||
* This file is free software: you may copy, redistribute and/or modify
|
||||
* it under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or (at your
|
||||
* option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
|
||||
* Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see http://www.gnu.org/licenses/.
|
||||
*/
|
||||
|
||||
#include "serial_proxy.h"
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/select.h>
|
||||
#include <time.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <unistd.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "drivers/time.h"
|
||||
#include "msp/msp_serial.h"
|
||||
#include "msp/msp_protocol.h"
|
||||
#include "common/crc.h"
|
||||
#include "rx/sim.h"
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#ifdef __FreeBSD__
|
||||
# define __BSD_VISIBLE 1
|
||||
#endif
|
||||
|
||||
#ifdef __linux__
|
||||
#include <asm/termbits.h>
|
||||
#ifndef TCGETS2
|
||||
#include <asm-generic/ioctls.h>
|
||||
#endif
|
||||
#else
|
||||
#include <termios.h>
|
||||
#endif
|
||||
|
||||
#ifdef __APPLE__
|
||||
#include <IOKit/serial/ioss.h>
|
||||
#endif
|
||||
|
||||
#include "drivers/serial_tcp.h"
|
||||
|
||||
#define SYM_BEGIN '$'
|
||||
#define SYM_PROTO_V1 'M'
|
||||
#define SYM_PROTO_V2 'X'
|
||||
#define SYM_FROM_MWC '>'
|
||||
#define SYM_TO_MWC '<'
|
||||
#define SYM_UNSUPPORTED '!'
|
||||
|
||||
#define JUMBO_FRAME_MIN_SIZE 255
|
||||
#define MAX_MSP_MESSAGE 1024
|
||||
#define RX_CONFIG_SIZE 24
|
||||
|
||||
typedef enum {
|
||||
DS_IDLE,
|
||||
DS_PROTO_IDENTIFIER,
|
||||
DS_DIRECTION_V1,
|
||||
DS_DIRECTION_V2,
|
||||
DS_FLAG_V2,
|
||||
DS_PAYLOAD_LENGTH_V1,
|
||||
DS_PAYLOAD_LENGTH_JUMBO_LOW,
|
||||
DS_PAYLOAD_LENGTH_JUMBO_HIGH,
|
||||
DS_PAYLOAD_LENGTH_V2_LOW,
|
||||
DS_PAYLOAD_LENGTH_V2_HIGH,
|
||||
DS_CODE_V1,
|
||||
DS_CODE_JUMBO_V1,
|
||||
DS_CODE_V2_LOW,
|
||||
DS_CODE_V2_HIGH,
|
||||
DS_PAYLOAD_V1,
|
||||
DS_PAYLOAD_V2,
|
||||
DS_CHECKSUM_V1,
|
||||
DS_CHECKSUM_V2,
|
||||
} TDecoderState;
|
||||
|
||||
static TDecoderState decoderState = DS_IDLE;
|
||||
|
||||
typedef enum {
|
||||
RXC_IDLE = 0,
|
||||
RXC_RQ = 1,
|
||||
RXC_DONE = 2
|
||||
} TRXConfigState;
|
||||
|
||||
static TRXConfigState rxConfigState = RXC_IDLE;
|
||||
|
||||
static int message_length_expected;
|
||||
static unsigned char message_buffer[MAX_MSP_MESSAGE];
|
||||
static int message_length_received;
|
||||
static int unsupported;
|
||||
static int code;
|
||||
static int message_direction;
|
||||
static uint8_t message_checksum;
|
||||
static int reqCount = 0;
|
||||
static uint16_t rssi = 0;
|
||||
static uint8_t rxConfigBuffer[RX_CONFIG_SIZE];
|
||||
|
||||
static timeMs_t lastWarning = 0;
|
||||
|
||||
int serialUartIndex = -1;
|
||||
char serialPort[64] = "";
|
||||
int serialBaudRate = 115200;
|
||||
OptSerialStopBits_e serialStopBits = OPT_SERIAL_STOP_BITS_ONE; //0:None|1:One|2:OnePointFive|3:Two
|
||||
OptSerialParity_e serialParity = OPT_SERIAL_PARITY_NONE;
|
||||
bool serialFCProxy = false;
|
||||
|
||||
#define FC_PROXY_REQUEST_PERIOD_MIN_MS 20 //inav can handle 100 msp messages per second max
|
||||
#define FC_PROXY_REQUEST_PERIOD_TIMEOUT_MS 200
|
||||
#define FC_PROXY_REQUEST_PERIOD_RSSI_MS 300
|
||||
#define SERIAL_BUFFER_SIZE 256
|
||||
|
||||
#if defined(__CYGWIN__)
|
||||
#include <windows.h>
|
||||
static HANDLE hSerial;
|
||||
#else
|
||||
static int fd;
|
||||
#endif
|
||||
|
||||
static bool connected = false;
|
||||
static bool started = false;
|
||||
|
||||
static timeMs_t nextProxyRequestTimeout = 0;
|
||||
static timeMs_t nextProxyRequestMin = 0;
|
||||
static timeMs_t nextProxyRequestRssi = 0;
|
||||
|
||||
static timeMs_t lastVisit = 0;
|
||||
|
||||
#if !defined(__CYGWIN__)
|
||||
#if !defined( __linux__) && !defined(__APPLE__)
|
||||
static int rate_to_constant(int baudrate)
|
||||
{
|
||||
#ifdef __FreeBSD__
|
||||
return baudrate;
|
||||
#else
|
||||
#define B(x) case x: return B##x
|
||||
switch (baudrate) {
|
||||
B(50);
|
||||
B(75);
|
||||
B(110);
|
||||
B(134);
|
||||
B(150);
|
||||
B(200);
|
||||
B(300);
|
||||
B(600);
|
||||
B(1200);
|
||||
B(1800);
|
||||
B(2400);
|
||||
B(4800);
|
||||
B(9600);
|
||||
B(19200);
|
||||
B(38400);
|
||||
B(57600);
|
||||
B(115200);
|
||||
B(230400);
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
#undef B
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
static void close_serial(void)
|
||||
{
|
||||
#ifdef __linux__
|
||||
ioctl(fd, TCFLSH, TCIOFLUSH);
|
||||
#else
|
||||
tcflush(fd, TCIOFLUSH);
|
||||
#endif
|
||||
close(fd);
|
||||
}
|
||||
|
||||
static int set_fd_speed(void)
|
||||
{
|
||||
int res = -1;
|
||||
#ifdef __linux__
|
||||
// Just user BOTHER for everything (allows non-standard speeds)
|
||||
struct termios2 t;
|
||||
if ((res = ioctl(fd, TCGETS2, &t)) != -1) {
|
||||
t.c_cflag &= ~CBAUD;
|
||||
t.c_cflag |= BOTHER;
|
||||
t.c_ospeed = t.c_ispeed = serialBaudRate;
|
||||
res = ioctl(fd, TCSETS2, &t);
|
||||
}
|
||||
#elif __APPLE__
|
||||
speed_t speed = serialBaudRate;
|
||||
res = ioctl(fd, IOSSIOSPEED, &speed);
|
||||
#else
|
||||
int speed = rate_to_constant(serialBaudRate);
|
||||
struct termios term;
|
||||
if (tcgetattr(fd, &term)) return -1;
|
||||
if (speed == 0) {
|
||||
res = -1;
|
||||
} else {
|
||||
if (cfsetispeed(&term, speed) != -1) {
|
||||
cfsetospeed(&term, speed);
|
||||
res = tcsetattr(fd, TCSANOW, &term);
|
||||
}
|
||||
if (res != -1) {
|
||||
memset(&term, 0, sizeof(term));
|
||||
res = (tcgetattr(fd, &term));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
return res;
|
||||
}
|
||||
|
||||
static int set_attributes(void)
|
||||
{
|
||||
struct termios tio;
|
||||
memset (&tio, 0, sizeof(tio));
|
||||
int res = -1;
|
||||
#ifdef __linux__
|
||||
res = ioctl(fd, TCGETS, &tio);
|
||||
#else
|
||||
res = tcgetattr(fd, &tio);
|
||||
#endif
|
||||
if (res != -1) {
|
||||
// cfmakeraw ...
|
||||
tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
|
||||
tio.c_oflag &= ~OPOST;
|
||||
tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
|
||||
tio.c_cflag &= ~(CSIZE | PARENB);
|
||||
tio.c_cflag |= CS8 | CREAD | CLOCAL;
|
||||
tio.c_cc[VTIME] = 0;
|
||||
tio.c_cc[VMIN] = 0;
|
||||
|
||||
switch (serialStopBits) {
|
||||
case OPT_SERIAL_STOP_BITS_ONE:
|
||||
tio.c_cflag &= ~CSTOPB;
|
||||
break;
|
||||
case OPT_SERIAL_STOP_BITS_TWO:
|
||||
tio.c_cflag |= CSTOPB;
|
||||
break;
|
||||
case OPT_SERIAL_STOP_BITS_INVALID:
|
||||
break;
|
||||
}
|
||||
|
||||
switch (serialParity) {
|
||||
case OPT_SERIAL_PARITY_EVEN:
|
||||
tio.c_cflag |= PARENB;
|
||||
tio.c_cflag &= ~PARODD;
|
||||
break;
|
||||
case OPT_SERIAL_PARITY_NONE:
|
||||
tio.c_cflag &= ~PARENB;
|
||||
tio.c_cflag &= ~PARODD;
|
||||
break;
|
||||
case OPT_SERIAL_PARITY_ODD:
|
||||
tio.c_cflag |= PARENB;
|
||||
tio.c_cflag |= PARODD;
|
||||
break;
|
||||
case OPT_SERIAL_PARITY_INVALID:
|
||||
break;
|
||||
}
|
||||
#ifdef __linux__
|
||||
res = ioctl(fd, TCSETS, &tio);
|
||||
#else
|
||||
res = tcsetattr(fd, TCSANOW, &tio);
|
||||
#endif
|
||||
}
|
||||
if (res != -1) {
|
||||
res = set_fd_speed();
|
||||
}
|
||||
return res;
|
||||
}
|
||||
#endif
|
||||
|
||||
void serialProxyInit(void)
|
||||
{
|
||||
char portName[64 + 20];
|
||||
if ( strlen(serialPort) < 1) {
|
||||
return;
|
||||
}
|
||||
connected = false;
|
||||
|
||||
#if defined(__CYGWIN__)
|
||||
sprintf(portName, "\\\\.\\%s", serialPort);
|
||||
|
||||
hSerial = CreateFile(portName,
|
||||
GENERIC_READ | GENERIC_WRITE,
|
||||
0,
|
||||
NULL,
|
||||
OPEN_EXISTING,
|
||||
FILE_ATTRIBUTE_NORMAL,
|
||||
NULL);
|
||||
|
||||
if (hSerial == INVALID_HANDLE_VALUE) {
|
||||
if (GetLastError() == ERROR_FILE_NOT_FOUND) {
|
||||
fprintf(stderr, "[SERIALPROXY] ERROR: Sserial port was not attached. Reason: %s not available.\n", portName);
|
||||
} else {
|
||||
fprintf(stderr, "[SERIALPROXY] ERROR: Can not connect to serial port, unknown error.\n");
|
||||
}
|
||||
return;
|
||||
} else {
|
||||
DCB dcbSerialParams = { 0 };
|
||||
if (!GetCommState(hSerial, &dcbSerialParams)) {
|
||||
fprintf(stderr, "[SERIALPROXY] ALERT: failed to get current serial parameters!\n");
|
||||
} else {
|
||||
dcbSerialParams.BaudRate = serialBaudRate;
|
||||
dcbSerialParams.ByteSize = 8;
|
||||
|
||||
// Disable software flow control (XON/XOFF)
|
||||
dcbSerialParams.fOutX = FALSE;
|
||||
dcbSerialParams.fInX = FALSE;
|
||||
|
||||
// Disable hardware flow control (RTS/CTS)
|
||||
dcbSerialParams.fRtsControl = RTS_CONTROL_DISABLE;
|
||||
dcbSerialParams.fDtrControl = DTR_CONTROL_DISABLE;
|
||||
|
||||
// Disable any special processing of bytes
|
||||
dcbSerialParams.fBinary = TRUE;
|
||||
|
||||
switch (serialStopBits) {
|
||||
case OPT_SERIAL_STOP_BITS_ONE:
|
||||
dcbSerialParams.StopBits = ONESTOPBIT;
|
||||
break;
|
||||
case OPT_SERIAL_STOP_BITS_TWO:
|
||||
dcbSerialParams.StopBits = TWOSTOPBITS;
|
||||
break;
|
||||
case OPT_SERIAL_STOP_BITS_INVALID:
|
||||
break;
|
||||
}
|
||||
|
||||
switch (serialParity) {
|
||||
case OPT_SERIAL_PARITY_EVEN:
|
||||
dcbSerialParams.Parity = EVENPARITY;
|
||||
break;
|
||||
case OPT_SERIAL_PARITY_NONE:
|
||||
dcbSerialParams.Parity = NOPARITY;
|
||||
break;
|
||||
case OPT_SERIAL_PARITY_ODD:
|
||||
dcbSerialParams.Parity = ODDPARITY;
|
||||
break;
|
||||
case OPT_SERIAL_PARITY_INVALID:
|
||||
break;
|
||||
}
|
||||
|
||||
if (!SetCommState(hSerial, &dcbSerialParams)) {
|
||||
fprintf(stderr, "[SERIALPROXY] ALERT: Could not set Serial Port parameters\n");
|
||||
} else {
|
||||
COMMTIMEOUTS comTimeOut;
|
||||
comTimeOut.ReadIntervalTimeout = MAXDWORD;
|
||||
comTimeOut.ReadTotalTimeoutMultiplier = 0;
|
||||
comTimeOut.ReadTotalTimeoutConstant = 0;
|
||||
comTimeOut.WriteTotalTimeoutMultiplier = 0;
|
||||
comTimeOut.WriteTotalTimeoutConstant = 0;
|
||||
SetCommTimeouts(hSerial, &comTimeOut);
|
||||
}
|
||||
}
|
||||
}
|
||||
#else
|
||||
strcpy(portName, serialPort); // because, windows ...
|
||||
fd = open(serialPort, O_RDWR | O_NOCTTY);
|
||||
if (fd != -1) {
|
||||
int res = 1;
|
||||
res = set_attributes();
|
||||
if (res == -1) {
|
||||
fprintf(stderr, "[SERIALPROXY] ALERT: Failed to configure device: %s %s\n", portName, strerror(errno));
|
||||
close(fd);
|
||||
fd = -1;
|
||||
}
|
||||
} else {
|
||||
fprintf(stderr, "[SERIALPROXY] ERROR: Can not connect to serial port %s\n", portName);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
connected = true;
|
||||
|
||||
if ( !serialFCProxy ) {
|
||||
fprintf(stderr, "[SERIALPROXY] Connected %s to UART%d\n", portName, serialUartIndex);
|
||||
} else {
|
||||
fprintf(stderr, "[SERIALPROXY] Using proxy flight controller on %s\n", portName);
|
||||
}
|
||||
}
|
||||
|
||||
void serialProxyStart(void)
|
||||
{
|
||||
started = true;
|
||||
}
|
||||
|
||||
void serialProxyClose(void)
|
||||
{
|
||||
if (connected) {
|
||||
connected = false;
|
||||
#if defined(__CYGWIN__)
|
||||
CloseHandle(hSerial);
|
||||
#else
|
||||
close_serial();
|
||||
#endif
|
||||
started = false;
|
||||
nextProxyRequestTimeout = 0;
|
||||
nextProxyRequestMin = 0;
|
||||
nextProxyRequestRssi = 0;
|
||||
lastWarning = 0;
|
||||
lastVisit = 0;
|
||||
}
|
||||
}
|
||||
|
||||
static bool canOutputWarning(void)
|
||||
{
|
||||
if ( millis() > lastWarning ) {
|
||||
lastWarning = millis() + 5000;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
int serialProxyReadData(unsigned char *buffer, unsigned int nbChar)
|
||||
{
|
||||
if (!connected) return 0;
|
||||
|
||||
#if defined(__CYGWIN__)
|
||||
COMSTAT status;
|
||||
DWORD errors;
|
||||
DWORD bytesRead;
|
||||
|
||||
ClearCommError(hSerial, &errors, &status);
|
||||
if (status.cbInQue > 0) {
|
||||
unsigned int toRead = (status.cbInQue > nbChar) ? nbChar : status.cbInQue;
|
||||
if (ReadFile(hSerial, buffer, toRead, &bytesRead, NULL) && (bytesRead != 0)) {
|
||||
return bytesRead;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
#else
|
||||
if (nbChar == 0) return 0;
|
||||
int bytesRead = read(fd, buffer, nbChar);
|
||||
return bytesRead;
|
||||
#endif
|
||||
}
|
||||
|
||||
bool serialProxyWriteData(unsigned char *buffer, unsigned int nbChar)
|
||||
{
|
||||
if (!connected) return false;
|
||||
|
||||
#if defined(__CYGWIN__)
|
||||
COMSTAT status;
|
||||
DWORD errors;
|
||||
DWORD bytesSent;
|
||||
if (!WriteFile(hSerial, (void *)buffer, nbChar, &bytesSent, 0)) {
|
||||
ClearCommError(hSerial, &errors, &status);
|
||||
if ( canOutputWarning() ) {
|
||||
fprintf(stderr, "[SERIALPROXY] ERROR: Can not write to serial port\n");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
if ( bytesSent != nbChar ) {
|
||||
if ( canOutputWarning() ) {
|
||||
fprintf(stderr, "[SERIALPROXY] ERROR: Can not write to serial port\n");
|
||||
}
|
||||
}
|
||||
#else
|
||||
ssize_t l = write(fd, buffer, nbChar);
|
||||
if ( l != (ssize_t)nbChar ) {
|
||||
if ( canOutputWarning() ) {
|
||||
fprintf(stderr, "[SERIALPROXY] ERROR: Can not write to serial port\n");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
bool serialProxyIsConnected(void)
|
||||
{
|
||||
return connected;
|
||||
}
|
||||
|
||||
static void mspSendCommand(int cmd, unsigned char *data, int dataLen)
|
||||
{
|
||||
uint8_t msgBuf[MAX_MSP_MESSAGE] = { '$', 'X', '<' };
|
||||
int msgLen = 3;
|
||||
|
||||
mspHeaderV2_t *hdrV2 = (mspHeaderV2_t *)&msgBuf[msgLen];
|
||||
hdrV2->flags = 0;
|
||||
hdrV2->cmd = cmd;
|
||||
hdrV2->size = dataLen;
|
||||
msgLen += sizeof(mspHeaderV2_t);
|
||||
|
||||
for ( int i = 0; i < dataLen; i++ ) {
|
||||
msgBuf[msgLen++] = data[i];
|
||||
}
|
||||
|
||||
uint8_t crc = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
|
||||
crc = crc8_dvb_s2_update(crc, data, dataLen);
|
||||
msgBuf[msgLen] = crc;
|
||||
msgLen++;
|
||||
|
||||
serialProxyWriteData((unsigned char *)&msgBuf, msgLen);
|
||||
}
|
||||
|
||||
static void mspRequestChannels(void)
|
||||
{
|
||||
mspSendCommand(MSP_RC, NULL, 0);
|
||||
}
|
||||
|
||||
static void mspRequestRssi(void)
|
||||
{
|
||||
mspSendCommand(MSP_ANALOG, NULL, 0);
|
||||
}
|
||||
|
||||
static void requestRXConfigState(void)
|
||||
{
|
||||
mspSendCommand(MSP_RX_CONFIG, NULL, 0);
|
||||
rxConfigState = RXC_RQ;
|
||||
fprintf(stderr, "[SERIALPROXY] Requesting RX config from proxy FC...\n");
|
||||
}
|
||||
|
||||
static void processMessage(void)
|
||||
{
|
||||
if ( code == MSP_RC ) {
|
||||
if ( reqCount > 0 ) reqCount--;
|
||||
int count = message_length_received / 2;
|
||||
if ( count <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
|
||||
uint16_t *channels = (uint16_t *)(&message_buffer[0]);
|
||||
//AETR => AERT
|
||||
uint v = channels[2];
|
||||
channels[2] = channels[3];
|
||||
channels[3] = v;
|
||||
if ( rssi > 0 ) {
|
||||
rxSimSetChannelValue(channels, count);
|
||||
}
|
||||
}
|
||||
} else if ( code == MSP_ANALOG ) {
|
||||
if ( reqCount > 0 ) reqCount--;
|
||||
if ( message_length_received >= 7 ) {
|
||||
rssi = *((uint16_t *)(&message_buffer[3]));
|
||||
rxSimSetRssi( rssi );
|
||||
}
|
||||
} else if ( code == MSP_RX_CONFIG ) {
|
||||
memcpy( &(rxConfigBuffer[0]), &(message_buffer[0]), RX_CONFIG_SIZE );
|
||||
*((uint16_t *) & (rxConfigBuffer[1])) = 2500; //maxcheck
|
||||
*((uint16_t *) & (rxConfigBuffer[5])) = 500; //mincheck
|
||||
|
||||
mspSendCommand( MSP_SET_RX_CONFIG, rxConfigBuffer, RX_CONFIG_SIZE );
|
||||
rxConfigState = RXC_DONE;
|
||||
fprintf(stderr, "[SERIALPROXY] Setting RX config on proxy FC...\n");
|
||||
}
|
||||
}
|
||||
|
||||
static void decodeProxyMessages(unsigned char *data, int len)
|
||||
{
|
||||
for (int i = 0; i < len; i++) {
|
||||
switch (decoderState) {
|
||||
case DS_IDLE: // sync char 1
|
||||
if (data[i] == SYM_BEGIN) {
|
||||
decoderState = DS_PROTO_IDENTIFIER;
|
||||
}
|
||||
break;
|
||||
|
||||
case DS_PROTO_IDENTIFIER: // sync char 2
|
||||
switch (data[i]) {
|
||||
case SYM_PROTO_V1:
|
||||
decoderState = DS_DIRECTION_V1;
|
||||
break;
|
||||
case SYM_PROTO_V2:
|
||||
decoderState = DS_DIRECTION_V2;
|
||||
break;
|
||||
default:
|
||||
//unknown protocol
|
||||
decoderState = DS_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case DS_DIRECTION_V1: // direction (should be >)
|
||||
|
||||
case DS_DIRECTION_V2:
|
||||
unsupported = 0;
|
||||
switch (data[i]) {
|
||||
case SYM_FROM_MWC:
|
||||
message_direction = 1;
|
||||
break;
|
||||
case SYM_TO_MWC:
|
||||
message_direction = 0;
|
||||
break;
|
||||
case SYM_UNSUPPORTED:
|
||||
unsupported = 1;
|
||||
break;
|
||||
}
|
||||
decoderState = decoderState == DS_DIRECTION_V1 ? DS_PAYLOAD_LENGTH_V1 : DS_FLAG_V2;
|
||||
break;
|
||||
|
||||
case DS_FLAG_V2:
|
||||
// Ignored for now
|
||||
decoderState = DS_CODE_V2_LOW;
|
||||
break;
|
||||
|
||||
case DS_PAYLOAD_LENGTH_V1:
|
||||
message_length_expected = data[i];
|
||||
|
||||
if (message_length_expected == JUMBO_FRAME_MIN_SIZE) {
|
||||
decoderState = DS_CODE_JUMBO_V1;
|
||||
} else {
|
||||
message_length_received = 0;
|
||||
decoderState = DS_CODE_V1;
|
||||
}
|
||||
break;
|
||||
|
||||
case DS_PAYLOAD_LENGTH_V2_LOW:
|
||||
message_length_expected = data[i];
|
||||
decoderState = DS_PAYLOAD_LENGTH_V2_HIGH;
|
||||
break;
|
||||
|
||||
case DS_PAYLOAD_LENGTH_V2_HIGH:
|
||||
message_length_expected |= data[i] << 8;
|
||||
message_length_received = 0;
|
||||
if (message_length_expected <= MAX_MSP_MESSAGE) {
|
||||
decoderState = message_length_expected > 0 ? DS_PAYLOAD_V2 : DS_CHECKSUM_V2;
|
||||
} else {
|
||||
//too large payload
|
||||
decoderState = DS_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case DS_CODE_V1:
|
||||
case DS_CODE_JUMBO_V1:
|
||||
code = data[i];
|
||||
if (message_length_expected > 0) {
|
||||
// process payload
|
||||
if (decoderState == DS_CODE_JUMBO_V1) {
|
||||
decoderState = DS_PAYLOAD_LENGTH_JUMBO_LOW;
|
||||
} else {
|
||||
decoderState = DS_PAYLOAD_V1;
|
||||
}
|
||||
} else {
|
||||
// no payload
|
||||
decoderState = DS_CHECKSUM_V1;
|
||||
}
|
||||
break;
|
||||
|
||||
case DS_CODE_V2_LOW:
|
||||
code = data[i];
|
||||
decoderState = DS_CODE_V2_HIGH;
|
||||
break;
|
||||
|
||||
case DS_CODE_V2_HIGH:
|
||||
code |= data[i] << 8;
|
||||
decoderState = DS_PAYLOAD_LENGTH_V2_LOW;
|
||||
break;
|
||||
|
||||
case DS_PAYLOAD_LENGTH_JUMBO_LOW:
|
||||
message_length_expected = data[i];
|
||||
decoderState = DS_PAYLOAD_LENGTH_JUMBO_HIGH;
|
||||
break;
|
||||
|
||||
case DS_PAYLOAD_LENGTH_JUMBO_HIGH:
|
||||
message_length_expected |= data[i] << 8;
|
||||
message_length_received = 0;
|
||||
decoderState = DS_PAYLOAD_V1;
|
||||
break;
|
||||
|
||||
case DS_PAYLOAD_V1:
|
||||
case DS_PAYLOAD_V2:
|
||||
message_buffer[message_length_received] = data[i];
|
||||
message_length_received++;
|
||||
|
||||
if (message_length_received >= message_length_expected) {
|
||||
decoderState = decoderState == DS_PAYLOAD_V1 ? DS_CHECKSUM_V1 : DS_CHECKSUM_V2;
|
||||
}
|
||||
break;
|
||||
|
||||
case DS_CHECKSUM_V1:
|
||||
if (message_length_expected >= JUMBO_FRAME_MIN_SIZE) {
|
||||
message_checksum = JUMBO_FRAME_MIN_SIZE;
|
||||
} else {
|
||||
message_checksum = message_length_expected;
|
||||
}
|
||||
message_checksum ^= code;
|
||||
if (message_length_expected >= JUMBO_FRAME_MIN_SIZE) {
|
||||
message_checksum ^= message_length_expected & 0xFF;
|
||||
message_checksum ^= (message_length_expected & 0xFF00) >> 8;
|
||||
}
|
||||
for (int ii = 0; ii < message_length_received; ii++) {
|
||||
message_checksum ^= message_buffer[ii];
|
||||
}
|
||||
if (message_checksum == data[i]) {
|
||||
processMessage();
|
||||
}
|
||||
decoderState = DS_IDLE;
|
||||
break;
|
||||
|
||||
case DS_CHECKSUM_V2:
|
||||
message_checksum = 0;
|
||||
message_checksum = crc8_dvb_s2(message_checksum, 0); // flag
|
||||
message_checksum = crc8_dvb_s2(message_checksum, code & 0xFF);
|
||||
message_checksum = crc8_dvb_s2(message_checksum, (code & 0xFF00) >> 8);
|
||||
message_checksum = crc8_dvb_s2(message_checksum, message_length_expected & 0xFF);
|
||||
message_checksum = crc8_dvb_s2(message_checksum, (message_length_expected & 0xFF00) >> 8);
|
||||
for (int ii = 0; ii < message_length_received; ii++) {
|
||||
message_checksum = crc8_dvb_s2(message_checksum, message_buffer[ii]);
|
||||
}
|
||||
if (message_checksum == data[i]) {
|
||||
processMessage();
|
||||
}
|
||||
decoderState = DS_IDLE;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void serialProxyProcess(void)
|
||||
{
|
||||
|
||||
if (!started) return;
|
||||
if (!connected) return;
|
||||
|
||||
if ((millis() - lastVisit) > 500) {
|
||||
if ( lastVisit > 0 ) {
|
||||
fprintf(stderr, "timeout=%dms\n", millis() - lastVisit);
|
||||
}
|
||||
}
|
||||
lastVisit = millis();
|
||||
|
||||
if ( serialFCProxy ) {
|
||||
//first we have to change FC min_check and max_check thresholds to avoid activating stick commands on proxy FC
|
||||
if ( rxConfigState == RXC_IDLE ) {
|
||||
requestRXConfigState();
|
||||
} else if ( rxConfigState == RXC_DONE ) {
|
||||
if ( (nextProxyRequestTimeout <= millis()) || ((reqCount == 0) && (nextProxyRequestMin <= millis()))) {
|
||||
nextProxyRequestTimeout = millis() + FC_PROXY_REQUEST_PERIOD_TIMEOUT_MS;
|
||||
nextProxyRequestMin = millis() + FC_PROXY_REQUEST_PERIOD_MIN_MS;
|
||||
mspRequestChannels();
|
||||
reqCount++;
|
||||
}
|
||||
|
||||
if ( nextProxyRequestRssi <= millis()) {
|
||||
nextProxyRequestRssi = millis() + FC_PROXY_REQUEST_PERIOD_RSSI_MS;
|
||||
mspRequestRssi();
|
||||
reqCount++;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned char buf[SERIAL_BUFFER_SIZE];
|
||||
int count = serialProxyReadData(buf, SERIAL_BUFFER_SIZE);
|
||||
if (count == 0) return;
|
||||
decodeProxyMessages(buf, count);
|
||||
|
||||
} else {
|
||||
|
||||
if ( serialUartIndex == -1 ) return;
|
||||
unsigned char buf[SERIAL_BUFFER_SIZE];
|
||||
uint32_t avail = tcpRXBytesFree(serialUartIndex - 1);
|
||||
if ( avail == 0 ) return;
|
||||
if (avail > SERIAL_BUFFER_SIZE) avail = SERIAL_BUFFER_SIZE;
|
||||
|
||||
int count = serialProxyReadData(buf, avail);
|
||||
if (count == 0) return;
|
||||
|
||||
tcpReceiveBytesEx( serialUartIndex - 1, buf, count);
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
64
src/main/target/SITL/serial_proxy.h
Normal file
64
src/main/target/SITL/serial_proxy.h
Normal file
|
|
@ -0,0 +1,64 @@
|
|||
/*
|
||||
* This file is part of INAV Project.
|
||||
*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
|
||||
* You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*
|
||||
* Alternatively, the contents of this file may be used under the terms
|
||||
* of the GNU General Public License Version 3, as described below:
|
||||
*
|
||||
* This file is free software: you may copy, redistribute and/or modify
|
||||
* it under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or (at your
|
||||
* option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
|
||||
* Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see http://www.gnu.org/licenses/.
|
||||
*/
|
||||
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
|
||||
#include <platform.h>
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
|
||||
typedef enum
|
||||
{
|
||||
OPT_SERIAL_STOP_BITS_ONE,
|
||||
OPT_SERIAL_STOP_BITS_TWO,
|
||||
OPT_SERIAL_STOP_BITS_INVALID
|
||||
} OptSerialStopBits_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
OPT_SERIAL_PARITY_EVEN,
|
||||
OPT_SERIAL_PARITY_NONE,
|
||||
OPT_SERIAL_PARITY_ODD,
|
||||
OPT_SERIAL_PARITY_INVALID
|
||||
} OptSerialParity_e;
|
||||
|
||||
|
||||
extern int serialUartIndex; ///1 for UART1
|
||||
extern char serialPort[64];
|
||||
extern int serialBaudRate;
|
||||
extern OptSerialStopBits_e serialStopBits;
|
||||
extern OptSerialParity_e serialParity;
|
||||
extern bool serialFCProxy;
|
||||
|
||||
extern void serialProxyInit(void);
|
||||
extern void serialProxyStart(void);
|
||||
extern void serialProxyProcess(void);
|
||||
extern void serialProxyClose(void);
|
||||
extern bool serialProxyIsConnected(void);
|
||||
extern bool serialProxyWriteData(unsigned char *buffer, unsigned int nbChar);
|
||||
|
||||
#endif
|
||||
|
|
@ -56,6 +56,8 @@
|
|||
#include "target/SITL/sim/realFlight.h"
|
||||
#include "target/SITL/sim/xplane.h"
|
||||
|
||||
#include "target/SITL/serial_proxy.h"
|
||||
|
||||
// More dummys
|
||||
const int timerHardwareCount = 0;
|
||||
timerHardware_t timerHardware[1];
|
||||
|
|
@ -170,6 +172,28 @@ bool parseMapping(char* mapStr)
|
|||
return true;
|
||||
}
|
||||
|
||||
OptSerialStopBits_e parseStopBits(const char* optarg){
|
||||
if ( strcmp(optarg, "One") == 0 ) {
|
||||
return OPT_SERIAL_STOP_BITS_ONE;
|
||||
} else if ( strcmp(optarg, "Two") == 0 ) {
|
||||
return OPT_SERIAL_STOP_BITS_TWO;
|
||||
} else {
|
||||
return OPT_SERIAL_STOP_BITS_INVALID;
|
||||
}
|
||||
}
|
||||
|
||||
OptSerialParity_e parseParity(const char* optarg){
|
||||
if ( strcmp(optarg, "Even") == 0 ) {
|
||||
return OPT_SERIAL_PARITY_EVEN;
|
||||
} else if ( strcmp(optarg, "None") == 0 ) {
|
||||
return OPT_SERIAL_PARITY_NONE;
|
||||
} else if ( strcmp(optarg, "Odd") == 0 ) {
|
||||
return OPT_SERIAL_PARITY_ODD;
|
||||
} else {
|
||||
return OPT_SERIAL_PARITY_INVALID;
|
||||
}
|
||||
}
|
||||
|
||||
void printCmdLineOptions(void)
|
||||
{
|
||||
printVersion();
|
||||
|
|
@ -179,6 +203,12 @@ void printCmdLineOptions(void)
|
|||
fprintf(stderr, "--simip=[ip] IP-Address oft the simulator host. If not specified localhost (127.0.0.1) is used.\n");
|
||||
fprintf(stderr, "--simport=[port] Port oft the simulator host.\n");
|
||||
fprintf(stderr, "--useimu Use IMU sensor data from the simulator instead of using attitude data from the simulator directly (experimental, not recommended).\n");
|
||||
fprintf(stderr, "--serialuart=[uart] UART number on which serial receiver is configured in SITL, f.e. 3 for UART3\n");
|
||||
fprintf(stderr, "--serialport=[serialport] Host's serial port to which serial receiver/proxy FC is connected, f.e. COM3, /dev/ttyACM3\n");
|
||||
fprintf(stderr, "--baudrate=[baudrate] Serial receiver baudrate (default: 115200).\n");
|
||||
fprintf(stderr, "--stopbits=[None|One|Two] Serial receiver stopbits (default: One).\n");
|
||||
fprintf(stderr, "--parity=[Even|None|Odd] Serial receiver parity (default: None).\n");
|
||||
fprintf(stderr, "--fcproxy Use inav/betaflight FC as a proxy for serial receiver.\n");
|
||||
fprintf(stderr, "--chanmap=[mapstring] Channel mapping. Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n");
|
||||
fprintf(stderr, " The mapstring has the following format: M(otor)|S(servo)<INAV-OUT>-<RECEIVER-OUT>,... All numbers must have two digits\n");
|
||||
fprintf(stderr, " For example: Map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n");
|
||||
|
|
@ -203,6 +233,12 @@ void parseArguments(int argc, char *argv[])
|
|||
{"help", no_argument, 0, 'h'},
|
||||
{"path", required_argument, 0, 'e'},
|
||||
{"version", no_argument, 0, 'v'},
|
||||
{"serialuart", required_argument, 0, '0'},
|
||||
{"serialport", required_argument, 0, '1'},
|
||||
{"baudrate", required_argument, 0, '2'},
|
||||
{"stopbits", required_argument, 0, '3'},
|
||||
{"parity", required_argument, 0, '4'},
|
||||
{"fcproxy", no_argument, 0, '5'},
|
||||
{NULL, 0, NULL, 0}
|
||||
};
|
||||
|
||||
|
|
@ -245,6 +281,50 @@ void parseArguments(int argc, char *argv[])
|
|||
case 'v':
|
||||
printVersion();
|
||||
exit(0);
|
||||
case '0':
|
||||
serialUartIndex = atoi(optarg);
|
||||
if ( (serialUartIndex<1) || (serialUartIndex>8) ) {
|
||||
fprintf(stderr, "[serialuart] Invalid argument\n.");
|
||||
exit(0);
|
||||
}
|
||||
break;
|
||||
case '1':
|
||||
if ( (strlen(optarg)<1) || (strlen(optarg)>63) ) {
|
||||
fprintf(stderr, "[serialport] Invalid argument\n.");
|
||||
exit(0);
|
||||
} else {
|
||||
strcpy( serialPort, optarg );
|
||||
}
|
||||
break;
|
||||
case '2':
|
||||
serialBaudRate = atoi(optarg);
|
||||
if ( serialBaudRate < 1200 )
|
||||
{
|
||||
fprintf(stderr, "[baudrate] Invalid argument\n.");
|
||||
exit(0);
|
||||
}
|
||||
break;
|
||||
case '3':
|
||||
serialStopBits = parseStopBits(optarg);
|
||||
if ( serialStopBits == OPT_SERIAL_STOP_BITS_INVALID )
|
||||
{
|
||||
fprintf(stderr, "[stopbits] Invalid argument\n.");
|
||||
exit(0);
|
||||
}
|
||||
break;
|
||||
|
||||
case '4':
|
||||
serialParity = parseParity(optarg);
|
||||
if ( serialParity== OPT_SERIAL_PARITY_INVALID )
|
||||
{
|
||||
fprintf(stderr, "[parity] Invalid argument\n.");
|
||||
exit(0);
|
||||
}
|
||||
break;
|
||||
case '5':
|
||||
serialFCProxy = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
printCmdLineOptions();
|
||||
exit(0);
|
||||
|
|
@ -304,6 +384,7 @@ void systemReset(void)
|
|||
#else
|
||||
closefrom(3);
|
||||
#endif
|
||||
serialProxyClose();
|
||||
execvp(c_argv[0], c_argv); // restart
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -32,9 +32,9 @@ timerHardware_t timerHardware[] = {
|
|||
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3
|
||||
DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4
|
||||
DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // S5
|
||||
DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
|
||||
DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
|
||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
|
||||
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7
|
||||
DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
|
||||
#endif
|
||||
|
||||
#ifdef ZEEZF7V2
|
||||
|
|
@ -42,10 +42,10 @@ timerHardware_t timerHardware[] = {
|
|||
DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2
|
||||
DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3
|
||||
DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4
|
||||
DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5
|
||||
DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
|
||||
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5
|
||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
|
||||
DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // S7
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
|
||||
DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
|
||||
#endif
|
||||
|
||||
#ifdef ZEEZF7
|
||||
|
|
|
|||
|
|
@ -84,6 +84,8 @@
|
|||
#define USE_RANGEFINDER_US42
|
||||
#define USE_RANGEFINDER_TOF10120_I2C
|
||||
#define USE_RANGEFINDER_TERARANGER_EVO_I2C
|
||||
#define USE_RANGEFINDER_USD1_V0
|
||||
#define USE_RANGEFINDER_NANORADAR
|
||||
|
||||
// Allow default optic flow boards
|
||||
#define USE_OPFLOW
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue