Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-12 19:10:27 +03:00
Code Issues Wiki Activity

- Bugfixes

Browse source 
- Add magnetometer and rnagefinder
- Add joystick interface
- Configuator integration
- Update docs
This commit is contained in:
Scavanger 2023-03-05 00:22:12 -03:00
parent 1af0e6116b
commit 154ea341f4
34 changed files with 706 additions and 256 deletions
Download patch file Download diff file Expand all files Collapse all files

30
.github/workflows/ci.yml vendored
View file

@ -43,6 +43,36 @@ jobs:
name: ${{ env.BUILD_NAME }}.zip
path: ./build/*.hex
build-SITL:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3
- name: Install dependencies
run: sudo apt-get update && sudo apt-get -y install ninja-build
- name: Setup environment
env:
ACTIONS_ALLOW_UNSECURE_COMMANDS: true
run: |
# This is the hash of the commit for the PR
# when the action is triggered by PR, empty otherwise
COMMIT_ID=${{ github.event.pull_request.head.sha }}
# This is the hash of the commit when triggered by push
# but the hash of refs/pull/<n>/merge, which is different
# from the hash of the latest commit in the PR, that's
# why we try github.event.pull_request.head.sha first
COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
- name: Build SITL
run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
- name: Upload artifacts
uses: actions/upload-artifact@v2-preview
with:
name: ${{ env.BUILD_NAME }}_SITL.zip
path: ./build_SITL/*_SITL
test:
needs: [build]
runs-on: ubuntu-latest

2
CMakeLists.txt
View file

@ -44,8 +44,8 @@ else()
include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}.cmake")
else()
set(CMAKE_TOOLCHAIN_FILE "${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}.cmake")
include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake")
endif()
include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake")
endif()
project(INAV VERSION 6.0.0)

10
cmake/host-checks.cmake
View file

@ -1,10 +0,0 @@
set(gcc_host_req_ver 10)
execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion
OUTPUT_STRIP_TRAILING_WHITESPACE
OUTPUT_VARIABLE version)
message("-- found GCC ${version}")
if (NOT gcc_host_req_ver STREQUAL version)
message(FATAL_ERROR "-- expecting GCC version ${gcc_host_req_ver}, but got version ${version} instead")
endif()

8
docs/SITL/RealFlight.md
View file

@ -2,10 +2,14 @@
Supported are RealFlight 9.5S and RealFlight Evolution, NOT RealFlight-X.
RealFlight is very well suited to simulate the model flight specific aspects. Autolaunch and the mixers can be set in INAV.
RealFlight is very well suited to simulate the model flight specific aspects. Autolaunch and the mixers can be used.
However, since the sceneries do not correspond to a real environment, the GPS data must be "faked". The position is always shown somewhere in southern Nevada ;).
GPS data and flight modes work fine though, only for missions with waypoints it is of course not ideal.
## Joystick
In the settings, calibrate the joystick, set it up and assign the axes in the same order as in INAV.
Channel 1 (Aileron) in RealFlight is Cannel 1 (Aileron in INAV) and so on.
## General settings
Under Settings / Physics / Quality Switch on "RealFlight Link enabled".
As a command line option for SITL, the port does not need to be specified, the port is fixed.
@ -17,4 +21,4 @@ In the model editor under "Electronis" all mixers should be deleted and the serv
In the "Radio" tab also deactivate Expo and low rates: "Activadd when: Never".
Configure the model in the same way as a real model would be set up in INAV including Mixer, Expo, etc. depending on the selected model in RealFlight.
Then adjust the channelmap (see command line option) accordingly.
Then adjust the channelmap im the Configurator or via command line accordingly.

76
docs/SITL/SITL.md
View file

@ -3,7 +3,7 @@
![INAV-SIM-OSD](assets/INAV-SIM-OSD.png)
## ATTENTION!
SITL is currently still under development, its use is still a bit cumbersome.
SITL is currently still under development.
SITL (Software in the loop) allows to run INAV completely in software on the PC without using a flight controller and simulate complete FPV flights.
For this, INAV is compiled with a normal PC compiler.
@ -13,46 +13,23 @@ Currently supported are
- RealFlight https://www.realflight.com/
- X-Plane https://www.x-plane.com/
INAV SITL communicates for sensor data and control directly with the corresponding simulator, see the documentation of the individual simulators and the command line options.
## Command line
The following SITL specific command line options are available:
If SITL is started without command line options, only the Configurator can be used.
```--help``` Displays help for the command line options.
```--sim=[sim]``` Select the simulator. xp = X-Plane, rf = RealFlight. Example: ```--sim=xp```
```--simip=[ip]``` IP address of the simulator, if you specify a simulator with "--sim" and omit this option localhost (127.0.0.1) will be used. Example: ```--simip=172.65.21.15```
```--simport=[port]``` Port number of the simulator, not necessary for all simulators. Example: ```--simport=4900```
```--useimu``` Use IMU sensor data from the simulator instead of using attitude data directly from the simulator. Not recommended, use only for debugging.
```--chanmap=[chanmap]``` The channelmap to map the motor and servo outputs from INAV to the virtual receiver channel or control surfaces around simulator.
Syntax: (M(otor)|S(ervo)<INAV-OUT>-<RECEIVER_OUT>),..., all numbers must have two digits.
Example:
To assign motor1 to virtual receiver channel 1, servo 1 to channel 2, and servo2 to channel 3:
```--chanmap:M01-01,S01-02,S02-03```
Please also read the documentation of the individual simulators.
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.
## Sensors
The following sensors are emulated:
- IMU (Gyro, Accelerometer)
- GPS
- Pitot
- barometer
- Magnetometer (Compass)
- Rangefinder
- Barometer
- Battery (current and voltage), depending on simulator
![SITL-Fake-Sensors](assets/SITL-Fake-Sensors.png)
Magnetometer (compass) is not yet supported, therefore no support for copters.
Select "FAKE" as type for all mentioned, so that they receive the data from the simulator.
## Serial ports
## Serial ports+
UARTs are replaced by TCP starting with port 5760 ascending. UART 1 port 5760, UART2 6761, ...
By default, UART1 and UART2 are available as MSP connections.
To connect the Configurator to SITL: Select TCP and connect to ```127.0.0.1:5760``` (if SITL is running on the same machine).
@ -64,8 +41,17 @@ The assignment and status of user UART/TCP connections is displayed on the conso
All other interfaces (I2C, SPI, etc.) are not emulated.
## Remote control
At the moment only direct input via UART/TCP is supported.
Joystick (via simulator) or serial receiver via USB/Serial interface are supported.
### 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.
### 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.
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 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.
@ -78,7 +64,7 @@ For this you need a FT232 module. With FT-Prog (https://ftdichip.com/utilities/)
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```
Note: Telemetry via return channel through the receiver is not supported by SITL.
Note: Telemetry via return channel through the receiver is not supported by SITL (yet).
## OSD
For the OSD the program INAV-Sim-OSD is available: https://github.com/Scavanger/INAV-SIM-OSD.
@ -86,6 +72,33 @@ 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.
## Command line
The command line options are only necessary if the SITL executable is started by hand, e.g. when debugging.
For normal use, please use the SITL tab in the configurator.
The following SITL specific command line options are available:
If SITL is started without command line options, only the Configurator can be used.
```--path``` Full 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```
```--sim=[sim]``` Select the simulator. xp = X-Plane, rf = RealFlight. Example: ```--sim=xp```
```--simip=[ip]``` IP address of the simulator, if you specify a simulator with "--sim" and omit this option localhost (127.0.0.1) will be used. Example: ```--simip=172.65.21.15```
```--simport=[port]``` Port number of the simulator, not necessary for all simulators. Example: ```--simport=4900```
```--useimu``` Use IMU sensor data from the simulator instead of using attitude data directly from the simulator. Not recommended, use only for debugging.
```--chanmap=[chanmap]``` The channelmap to map the motor and servo outputs from INAV to the virtual receiver channel or control surfaces around simulator.
Syntax: (M(otor)|S(ervo)<INAV-OUT>-<RECEIVER_OUT>),..., all numbers must have two digits.
Example:
To assign motor1 to virtual receiver channel 1, servo 1 to channel 2, and servo2 to channel 3:
```--chanmap:M01-01,S01-02,S02-03```
Please also read the documentation of the individual simulators.
```--help``` Displays help for the command line options.
## Running SITL
It is recommended to start the tools in the following order:
1. Simulator, aircraft should be ready for take-off
@ -94,7 +107,6 @@ It is recommended to start the tools in the following order:
4. serial redirect for RC input
## Compile
GCC 10 is required, GCC 11 gives an error message (bug in GCC?!?).
### Linux:
Almost like normal, ruby, cmake and make are also required.

24
docs/SITL/X-Plane.md
View file

@ -2,14 +2,32 @@
Tested on X-Plane 11, 12 should(!) work but not tested.
X-Plane is not a model flight simulator, but is based on real world data and is therefore suitable
GPS missions with waypoints.
X-Plane is not a model flight simulator, but is based on real world data and is therefore suitable for GPS missions with waypoints.
## Aircraft
It is recommended to use the "AR Wing" of the INAV HITL project: https://github.com/RomanLut/INAV-X-Plane-HITL
## General settings
In Settings / Network select "Accept incoming connections".
The port can be found under "UDP PORTS", "Port we receive on". If no connection is established, the port can be changed.
You may want to incease the "Flight model per frame" value under "General"
## Joystick
In the settings, calibrate the joystick, set it up and assign the axes as follows:
| INAV | X-Plane |
|------|---------|
| Roll | Roll |
| Pitch | Pitch |
| Throttle | Throttle |
| Yaw | Yaw |
| Channel 5 | Prop |
| Channel 6 | Mixture |
| Channel 7 | Collective |
| Channel 8 | Thrust vector |
Reverse axis in X-Plane if necessary.
## Channelmap:
The assignment of the "virtual receiver" is fixed:
1 - Throttle
@ -17,6 +35,6 @@ The assignment of the "virtual receiver" is fixed:
3 - Pitch
4 - Yaw
The internal mixer (e.g. for wings only) cannot be deactivated without further ado, therefore always select "Aircraft with tail" in INAV.
The internal mixer (e.g. for flying wings) cannot be deactivated without further ado, therefore always select "Aircraft with tail" in INAV.
For the standard Aircraft preset the channelmap is:
```--chanmap=M01-01,S01-03,S03-02,S04-04```

4
src/main/CMakeLists.txt
View file

@ -406,6 +406,9 @@ main_sources(COMMON_SRC
rx/srxl2.h
rx/sumd.c
rx/sumd.h
rx/sim.c
rx/sim.h
scheduler/scheduler.c
scheduler/scheduler.h
@ -467,6 +470,7 @@ main_sources(COMMON_SRC
io/rangefinder.h
io/rangefinder_msp.c
io/rangefinder_benewake.c
io/rangefinder_fake.c
io/opflow.h
io/opflow_cxof.c
io/opflow_msp.c

10
src/main/common/time.c
View file

@ -35,6 +35,10 @@
#include "fc/settings.h"
#ifdef SITL_BUILD
#include <time.h>
#endif
// For the "modulo 4" arithmetic to work, we need a leap base year
#define REFERENCE_YEAR 2000
// Offset (seconds) from the UNIX epoch (1970-01-01) to 2000-01-01
@ -310,11 +314,16 @@ bool rtcHasTime(void)
bool rtcGet(rtcTime_t *t)
{
#ifdef SITL_BUILD
*t = (rtcTime_t)(time(NULL) * 1000);
return true;
#else
if (!rtcHasTime()) {
return false;
}
*t = started + millis();
return true;
#endif
}
bool rtcSet(rtcTime_t *t)
@ -323,6 +332,7 @@ bool rtcSet(rtcTime_t *t)
return true;
}
bool rtcGetDateTime(dateTime_t *dt)
{
rtcTime_t t;

4
src/main/config/config_streamer.h
View file

@ -57,3 +57,7 @@ int config_streamer_flush(config_streamer_t *c);
int config_streamer_finish(config_streamer_t *c);
int config_streamer_status(config_streamer_t *c);
#if defined(CONFIG_IN_FILE)
bool configFileSetPath(char* path);
#endif

36
src/main/config/config_streamer_file.c
View file

@ -16,6 +16,7 @@
*/
#include <string.h>
#include <dirent.h>
#include "platform.h"
#include "drivers/system.h"
#include "config/config_streamer.h"
@ -26,21 +27,32 @@
#include <stdio.h>
#include <errno.h>
#define FLASH_PAGE_SIZE (0x400)
static FILE *eepromFd = NULL;
static bool streamerLocked = true;
static char eepromPath[260] = EEPROM_FILENAME;
bool configFileSetPath(char* path)
{
if(!path || strlen(path) > 260) {
return false;
}
strcpy(eepromPath, path);
return true;
}
void config_streamer_impl_unlock(void)
{
if (eepromFd != NULL) {
fprintf(stderr, "[EEPROM] Unable to load %s\n", EEPROM_FILENAME);
fprintf(stderr, "[EEPROM] Unable to load %s\n", eepromPath);
return;
}
// open or create
eepromFd = fopen(EEPROM_FILENAME,"r+");
eepromFd = fopen(eepromPath,"r+");
if (eepromFd != NULL) {
// obtain file size:
fseek(eepromFd , 0 , SEEK_END);
@ -49,16 +61,16 @@ void config_streamer_impl_unlock(void)
size_t n = fread(eepromData, 1, sizeof(eepromData), eepromFd);
if (n == size) {
printf("[EEPROM] Loaded '%s' (%ld of %ld bytes)\n", EEPROM_FILENAME, size, sizeof(eepromData));
fprintf(stderr,"[EEPROM] Loaded '%s' (%ld of %ld bytes)\n", eepromPath, size, sizeof(eepromData));
streamerLocked = false;
} else {
fprintf(stderr, "[EEPROM] Failed to load '%s'\n", EEPROM_FILENAME);
fprintf(stderr, "[EEPROM] Failed to load '%s'\n", eepromPath);
}
} else {
printf("[EEPROM] Created '%s', size = %ld\n", EEPROM_FILENAME, sizeof(eepromData));
printf("[EEPROM] Created '%s', size = %ld\n", eepromPath, sizeof(eepromData));
streamerLocked = false;
if ((eepromFd = fopen(EEPROM_FILENAME, "w+")) == NULL) {
fprintf(stderr, "[EEPROM] Failed to create '%s'\n", EEPROM_FILENAME);
if ((eepromFd = fopen(eepromPath, "w+")) == NULL) {
fprintf(stderr, "[EEPROM] Failed to create '%s'\n", eepromPath);
streamerLocked = true;
}
if (fwrite(eepromData, sizeof(eepromData), 1, eepromFd) != 1) {
@ -66,8 +78,6 @@ void config_streamer_impl_unlock(void)
streamerLocked = true;
}
}
}
void config_streamer_impl_lock(void)
@ -78,7 +88,7 @@ void config_streamer_impl_lock(void)
fwrite(eepromData, 1, sizeof(eepromData), eepromFd);
fclose(eepromFd);
eepromFd = NULL;
printf("[EEPROM] Saved '%s'\n", EEPROM_FILENAME);
fprintf(stderr, "[EEPROM] Saved '%s'\n", eepromPath);
streamerLocked = false;
} else {
fprintf(stderr, "[EEPROM] Unlock error\n");
@ -93,9 +103,9 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer
if ((c->address >= (uintptr_t)eepromData) && (c->address < (uintptr_t)ARRAYEND(eepromData))) {
*((uint32_t*)c->address) = *buffer;
printf("[EEPROM] Program word %p = %08x\n", (void*)c->address, *((uint32_t*)c->address));
fprintf(stderr, "[EEPROM] Program word %p = %08x\n", (void*)c->address, *((uint32_t*)c->address));
} else {
printf("[EEPROM] Program word %p out of range!\n", (void*)c->address);
fprintf(stderr, "[EEPROM] Program word %p out of range!\n", (void*)c->address);
}
c->address += CONFIG_STREAMER_BUFFER_SIZE;

46
src/main/drivers/accgyro/accgyro_fake.c
View file

@ -32,59 +32,25 @@
#ifdef USE_IMU_FAKE
#if defined(SITL_BUILD)
#define VOLATILE volatile
static pthread_mutex_t gyroMutex;
static pthread_mutex_t accMutex;
#define LOCK(mutex) (pthread_mutex_lock(mutex))
#define UNLOCK(mutex) (pthread_mutex_unlock(mutex))
#define GYROLOCK (pthread_mutex_lock(&gyroMutex))
#define GYROUNLOCK (pthread_mutex_unlock(&gyroMutex))
#define ACCLOCK (pthread_mutex_lock(&accMutex))
#define ACCUNLOCK (pthread_mutex_unlock(&accMutex))
#else
#define VOLATILE
#define GYROLOCK
#define GYROUNLOCK
#define ACCLOCK
#define ACCUNLOCK
#endif
static VOLATILE int16_t fakeGyroADC[XYZ_AXIS_COUNT];
static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
static void fakeGyroInit(gyroDev_t *gyro)
{
UNUSED(gyro);
#if defined(SITL_BUILD)
pthread_mutex_init(&gyroMutex, NULL);
#endif
//ENABLE_STATE(ACCELEROMETER_CALIBRATED);
}
void fakeGyroSet(int16_t x, int16_t y, int16_t z)
{
GYROLOCK;
fakeGyroADC[X] = x;
fakeGyroADC[Y] = y;
fakeGyroADC[Z] = z;
GYROUNLOCK;
}
static bool fakeGyroRead(gyroDev_t *gyro)
{
GYROLOCK;
gyro->gyroADCRaw[X] = fakeGyroADC[X];
gyro->gyroADCRaw[Y] = fakeGyroADC[Y];
gyro->gyroADCRaw[Z] = fakeGyroADC[Z];
GYROUNLOCK;
return true;
}
@ -112,33 +78,25 @@ bool fakeGyroDetect(gyroDev_t *gyro)
return true;
}
static VOLATILE int16_t fakeAccData[XYZ_AXIS_COUNT];
static int16_t fakeAccData[XYZ_AXIS_COUNT];
static void fakeAccInit(accDev_t *acc)
{
#if defined(SITL_BUILD)
pthread_mutex_init(&accMutex, NULL);
#endif
acc->acc_1G = 9806;
}
void fakeAccSet(int16_t x, int16_t y, int16_t z)
{
ACCLOCK;
fakeAccData[X] = x;
fakeAccData[Y] = y;
fakeAccData[Z] = z;
ACCUNLOCK;
}
static bool fakeAccRead(accDev_t *acc)
{
ACCLOCK;
acc->ADCRaw[X] = fakeAccData[X];
acc->ADCRaw[Y] = fakeAccData[Y];
acc->ADCRaw[Z] = fakeAccData[Z];
ACCUNLOCK;
return true;
}

9
src/main/drivers/rangefinder/rangefinder_virtual.c
View file

@ -58,18 +58,15 @@ static int32_t virtualRangefinderGetDistance(rangefinderDev_t * dev)
return highLevelDeviceVTable->read();
}
#define VIRTUAL_MAX_RANGE_CM 250
#define VIRTUAL_DETECTION_CONE_DECIDEGREES 900
bool virtualRangefinderDetect(rangefinderDev_t * dev, const virtualRangefinderVTable_t * vtable)
{
if (vtable && vtable->detect()) {
highLevelDeviceVTable = vtable;
dev->delayMs = RANGEFINDER_VIRTUAL_TASK_PERIOD_MS;
dev->maxRangeCm = VIRTUAL_MAX_RANGE_CM;
dev->detectionConeDeciDegrees = VIRTUAL_DETECTION_CONE_DECIDEGREES;
dev->detectionConeExtendedDeciDegrees = VIRTUAL_DETECTION_CONE_DECIDEGREES;
dev->maxRangeCm = RANGEFINDER_VIRTUAL_MAX_RANGE_CM;
dev->detectionConeDeciDegrees = RANGEFINDER_VIRTUAL_DETECTION_CONE_DECIDEGREES;
dev->detectionConeExtendedDeciDegrees = RANGEFINDER_VIRTUAL_DETECTION_CONE_DECIDEGREES;
dev->init = &virtualRangefinderInit;
dev->update = &virtualRangefinderUpdate;

2
src/main/drivers/rangefinder/rangefinder_virtual.h
View file

@ -26,6 +26,8 @@
#include "drivers/rangefinder/rangefinder.h"
#define RANGEFINDER_VIRTUAL_MAX_RANGE_CM 250
#define RANGEFINDER_VIRTUAL_DETECTION_CONE_DECIDEGREES 900
#define RANGEFINDER_VIRTUAL_TASK_PERIOD_MS 30
typedef struct virtualRangefinderVTable_s {

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

@ -3417,7 +3417,11 @@ static void cliStatus(char *cmdline)
cliPrint("OSD: ");
#if defined(USE_OSD)
displayPort_t *osdDisplayPort = osdGetDisplayPort();
cliPrintf("%s [%u x %u]", osdDisplayPort->displayPortType, osdDisplayPort->cols, osdDisplayPort->rows);
if (osdDisplayPort) {
cliPrintf("%s [%u x %u]", osdDisplayPort->displayPortType, osdDisplayPort->cols, osdDisplayPort->rows);
} else {
cliPrintf("no OSD detected");
}
#else
cliPrint("not used");
#endif

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

@ -161,11 +161,22 @@ bool areSensorsCalibrating(void)
int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
{
int16_t stickDeflection;
int16_t stickDeflection = 0;
stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500);
#if defined(SITL_BUILD) // Workaround due to strange bug in GCC > 10.2 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=108914
const int16_t value = rawData - PWM_RANGE_MIDDLE;
if (value < -500) {
stickDeflection = -500;
} else if (value > 500) {
stickDeflection = 500;
} else {
stickDeflection = value;
}
#else
stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500);
#endif
stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500);
return rcLookup(stickDeflection, rate);
}

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

@ -46,11 +46,7 @@
#include "drivers/compass/compass_msp.h"
#include "drivers/barometer/barometer_msp.h"
#include "drivers/pitotmeter/pitotmeter_msp.h"
#include "drivers/accgyro/accgyro_fake.h"
#include "drivers/barometer/barometer_fake.h"
#include "drivers/compass/compass_fake.h"
#include "sensors/battery_sensor_fake.h"
#include "drivers/pitotmeter/pitotmeter_fake.h"
#include "drivers/bus_i2c.h"
#include "drivers/display.h"
#include "drivers/flash.h"

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

@ -7,7 +7,7 @@ tables:
values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270", "FAKE"]
enum: accelerationSensor_e
- name: rangefinder_hardware
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C"]
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE"]
enum: rangefinderType_e
- name: mag_hardware
values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]
@ -22,7 +22,7 @@ tables:
values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"]
enum: pitotSensor_e
- name: receiver_type
values: ["NONE", "SERIAL", "MSP"]
values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"]
enum: rxReceiverType_e
- name: serial_rx
values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK"]

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

@ -31,5 +31,7 @@
extern virtualRangefinderVTable_t rangefinderMSPVtable;
extern virtualRangefinderVTable_t rangefinderBenewakeVtable;
extern virtualRangefinderVTable_t rangefinderFakeVtable;
void mspRangefinderReceiveNewData(uint8_t * bufferPtr);
void mspRangefinderReceiveNewData(uint8_t * bufferPtr);
void fakeRangefindersSetData(int32_t data);

86
src/main/io/rangefinder_fake.c Normal file
View file

@ -0,0 +1,86 @@
/*
* 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 <stdint.h>
#include <ctype.h>
#include <math.h>
#include "platform.h"
#include "build/build_config.h"
#include "build/debug.h"
#include "common/maths.h"
#include "io/serial.h"
#if defined(USE_RANGEFINDER_FAKE)
#include "drivers/rangefinder/rangefinder_virtual.h"
#include "drivers/time.h"
#include "io/rangefinder.h"
static bool hasNewData = false;
static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
static bool fakeRangefinderDetect(void)
{
// Always detectable
return true;
}
static void fakeRangefinderInit(void)
{
}
static void fakeRangefinderUpdate(void)
{
}
static int32_t fakeRangefinderGetDistance(void)
{
if (hasNewData) {
hasNewData = false;
return (sensorData > 0) ? sensorData : RANGEFINDER_OUT_OF_RANGE;
}
else {
return RANGEFINDER_NO_NEW_DATA;
}
}
void fakeRangefindersSetData(int32_t data)
{
sensorData = data;
hasNewData = true;
}
virtualRangefinderVTable_t rangefinderFakeVtable = {
.detect = fakeRangefinderDetect,
.init = fakeRangefinderInit,
.update = fakeRangefinderUpdate,
.read = fakeRangefinderGetDistance
};
#endif

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

@ -241,7 +241,7 @@ void onNewGPSData(void)
/* Use VELNED provided by GPS if available, calculate from coordinates otherwise */
float gpsScaleLonDown = constrainf(cos_approx((ABS(gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f);
if (positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelNE) {
if (!ARMING_FLAG(SIMULATOR_MODE_SITL) && positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelNE) {
posEstimator.gps.vel.x = gpsSol.velNED[X];
posEstimator.gps.vel.y = gpsSol.velNED[Y];
}

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

@ -63,6 +63,7 @@
#include "rx/sumd.h"
#include "rx/ghst.h"
#include "rx/mavlink.h"
#include "rx/sim.h"
const char rcChannelLetters[] = "AERT";
@ -310,6 +311,12 @@ void rxInit(void)
break;
#endif
#ifdef USE_RX_SIM
case RX_TYPE_SIM:
rxSimInit(rxConfig(), &rxRuntimeConfig);
break;
#endif
default:
case RX_TYPE_NONE:
rxConfigMutable()->receiverType = RX_TYPE_NONE;

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

@ -62,7 +62,8 @@ typedef enum {
typedef enum {
RX_TYPE_NONE = 0,
RX_TYPE_SERIAL,
RX_TYPE_MSP
RX_TYPE_MSP,
RX_TYPE_SIM
} rxReceiverType_e;
typedef enum {

71
src/main/rx/sim.c Normal file
View file

@ -0,0 +1,71 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#ifdef USE_RX_SIM
#include "build/build_config.h"
#include "common/utils.h"
#include "rx/rx.h"
#include "rx/sim.h"
static uint16_t channels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
static bool hasNewData = false;
static uint16_t rxSimReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan)
{
UNUSED(rxRuntimeConfigPtr);
return channels[chan];
}
void rxSimSetChannelValue(uint16_t* values, uint8_t count)
{
for (size_t i = 0; i < count; i++) {
channels[i] = values[i];
}
hasNewData = true;
}
static uint8_t rxSimFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
if (!hasNewData) {
return RX_FRAME_PENDING;
}
hasNewData = false;
return RX_FRAME_COMPLETE;
}
void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxConfig);
rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT;
rxRuntimeConfig->rxSignalTimeout = DELAY_5_HZ;
rxRuntimeConfig->rcReadRawFn = rxSimReadRawRC;
rxRuntimeConfig->rcFrameStatusFn = rxSimFrameStatus;
}
#endif

23
src/main/rx/sim.h Normal file
View file

@ -0,0 +1,23 @@
/*
* This file is part of INAV.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "rx/rx.h"
void rxSimSetChannelValue(uint16_t* values, uint8_t count);
void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);

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

@ -520,11 +520,6 @@ void accUpdate(void)
if (!ARMING_FLAG(SIMULATOR_MODE_SITL)) {
performAcclerationCalibration();
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
}

9
src/main/sensors/rangefinder.c
View file

@ -141,7 +141,14 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
}
#endif
break;
case RANGEFINDER_FAKE:
#if defined(USE_RANGEFINDER_FAKE)
if(virtualRangefinderDetect(dev, &rangefinderFakeVtable)) {
rangefinderHardware = RANGEFINDER_FAKE;
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
}
#endif
break;
case RANGEFINDER_NONE:
rangefinderHardware = RANGEFINDER_NONE;
break;

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

@ -30,6 +30,7 @@ typedef enum {
RANGEFINDER_VL53L1X = 5,
RANGEFINDER_US42 = 6,
RANGEFINDER_TOF10102I2C = 7,
RANGEFINDER_FAKE = 8,
} rangefinderType_e;
typedef struct rangefinderConfig_s {

96
src/main/target/SITL/sim/realFlight.c
View file

@ -45,14 +45,19 @@
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "drivers/pitotmeter/pitotmeter_fake.h"
#include "drivers/compass/compass_fake.h"
#include "drivers/rangefinder/rangefinder_virtual.h"
#include "io/rangefinder.h"
#include "common/utils.h"
#include "common/maths.h"
#include "flight/mixer.h"
#include "flight/servos.h"
#include "flight/imu.h"
#include "io/gps.h"
#include "rx/sim.h"
#define RF_PORT 18083
#define RF_MAX_CHANNEL_COUNT 12
// RealFlight scenerys doesn't represent real landscapes, so fake some nice coords: Area 51 ;)
#define FAKE_LAT 37.277127f
#define FAKE_LON -115.799669f
@ -212,6 +217,33 @@ static char* getStringFromResponse(const char* response, const char* elementName
return ret;
}
static bool getChannelValues(const char* response, uint16_t* channelValues)
{
if (!response) {
return false;
}
const char* channelValueTag = "<m-channelValues-0to1 xsi:type=\"SOAP-ENC:Array\" SOAP-ENC:arrayType=\"xsd:double[12]\">";
char* pos = strstr(response, channelValueTag);
if (!pos){
return false;
}
pos += strlen(channelValueTag);
for (size_t i = 0; i < RF_MAX_CHANNEL_COUNT; i++) {
char* end = strstr(pos, "</");
if (!end) {
return false;
}
channelValues[i] = FLOAT_0_1_TO_PWM((float)atof(pos + 6));
pos = end + 7;
}
return true;
}
static void fakeCoords(double posX, double posY, double distanceX, double distanceY, double *lat, double *lon)
{
double m = 1 / (2 * (double)M_PIf / 360 * EARTH_RADIUS) / 1000;
@ -224,7 +256,6 @@ static double convertAzimuth(double azimuth)
if (azimuth < 0) {
azimuth += 360;
}
return 360 - fmod(azimuth + 90, 360.0f);
}
@ -246,8 +277,8 @@ static void exchangeData(void)
//rfValues.m_currentPhysicsTime_SEC = getDoubleFromResponse(response, "m-currentPhysicsTime-SEC");
//rfValues.m_currentPhysicsSpeedMultiplier = getDoubleFromResponse(response, "m-currentPhysicsSpeedMultiplier");
rfValues.m_airspeed_MPS = getDoubleFromResponse(response, "m-airspeed-MPS");
rfValues.m_altitudeASL_MTR = getDoubleFromResponse(response, "<m-altitudeASL-MTR");
//rfValues.m_altitudeAGL_MTR = getDoubleFromResponse(response, "<m-altitudeAGL-MTR");
rfValues.m_altitudeASL_MTR = getDoubleFromResponse(response, "m-altitudeASL-MTR");
rfValues.m_altitudeAGL_MTR = getDoubleFromResponse(response, "m-altitudeAGL-MTR");
rfValues.m_groundspeed_MPS = getDoubleFromResponse(response, "m-groundspeed-MPS");
rfValues.m_pitchRate_DEGpSEC = getDoubleFromResponse(response, "m-pitchRate-DEGpSEC");
rfValues.m_rollRate_DEGpSEC = getDoubleFromResponse(response, "m-rollRate-DEGpSEC");
@ -289,6 +320,11 @@ static void exchangeData(void)
//rfValues.m_flightAxisControllerIsActive= getBoolFromResponse(response, "m-flightAxisControllerIsActive");
rfValues.m_currentAircraftStatus = getStringFromResponse(response, "m-currentAircraftStatus");
uint16_t channelValues[RF_MAX_CHANNEL_COUNT];
getChannelValues(response, channelValues);
rxSimSetChannelValue(channelValues, RF_MAX_CHANNEL_COUNT);
double lat, lon;
fakeCoords(FAKE_LAT, FAKE_LON, rfValues.m_aircraftPositionX_MTR, -rfValues.m_aircraftPositionY_MTR, &lat, &lon);
@ -308,12 +344,18 @@ static void exchangeData(void)
0
);
int32_t altitudeOverGround = (int32_t)round(rfValues.m_altitudeAGL_MTR * 100);
if (altitudeOverGround > 0 && altitudeOverGround <= RANGEFINDER_VIRTUAL_MAX_RANGE_CM) {
fakeRangefindersSetData(altitudeOverGround);
} else {
fakeRangefindersSetData(-1);
}
const int16_t roll_inav = (int16_t)round(rfValues.m_roll_DEG * 10);
const int16_t pitch_inav = (int16_t)round(-rfValues.m_inclination_DEG * 10);
const int16_t yaw_inav = course;
if (!useImu) {
imuSetAttitudeRPY(
(int16_t)round(rfValues.m_roll_DEG * 10),
(int16_t)round(-rfValues.m_inclination_DEG * 10),
course
);
imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav);
imuUpdateAttitude(micros());
}
@ -326,26 +368,38 @@ static void exchangeData(void)
accY = 0;
accZ = (int16_t)(GRAVITY_MSS * 1000.0f);
} else {
accX = clampToInt16(rfValues.m_accelerationBodyAX_MPS2 * 1000);
accY = clampToInt16(-rfValues.m_accelerationBodyAY_MPS2 * 1000);
accZ = clampToInt16(-rfValues.m_accelerationBodyAZ_MPS2 * 1000);
accX = constrainToInt16(rfValues.m_accelerationBodyAX_MPS2 * 1000);
accY = constrainToInt16(-rfValues.m_accelerationBodyAY_MPS2 * 1000);
accZ = constrainToInt16(-rfValues.m_accelerationBodyAZ_MPS2 * 1000);
}
fakeAccSet(accX, accY, accZ);
fakeGyroSet(
clampToInt16(rfValues.m_rollRate_DEGpSEC * (double)16.0),
clampToInt16(-rfValues.m_pitchRate_DEGpSEC * (double)16.0),
clampToInt16(rfValues.m_yawRate_DEGpSEC * (double)16.0)
constrainToInt16(rfValues.m_rollRate_DEGpSEC * (double)16.0),
constrainToInt16(-rfValues.m_pitchRate_DEGpSEC * (double)16.0),
constrainToInt16(rfValues.m_yawRate_DEGpSEC * (double)16.0)
);
fakeBaroSet(altitudeToPressure(altitude), DEGREES_TO_CENTIDEGREES(21));
fakePitotSetAirspeed(rfValues.m_airspeed_MPS * 100);
// TODO fakeMag
fakeBattSensorSetVbat((uint16_t)round(rfValues.m_batteryVoltage_VOLTS * 100));
fakeBattSensorSetAmperage((uint16_t)round(rfValues.m_batteryCurrentDraw_AMPS * 100));
fakeBattSensorSetAmperage((uint16_t)round(rfValues.m_batteryCurrentDraw_AMPS * 100));
fpQuaternion_t quat;
fpVector3_t north;
north.x = 1.0f;
north.y = 0;
north.z = 0;
computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav);
transformVectorEarthToBody(&north, &quat);
fakeMagSet(
constrainToInt16(north.x * 16000.0f),
constrainToInt16(north.y * 16000.0f),
constrainToInt16(north.z * 16000.0f)
);
ENABLE_STATE(COMPASS_CALIBRATED);
}
static void* soapWorker(void* arg)
@ -357,14 +411,14 @@ static void* soapWorker(void* arg)
startRequest("RestoreOriginalControllerDevice", "<RestoreOriginalControllerDevice><a>1</a><b>2</b></RestoreOriginalControllerDevice>");
endRequest();
startRequest("InjectUAVControllerInterface", "<InjectUAVControllerInterface><a>1</a><b>2</b></InjectUAVControllerInterface>");
endRequest();
endRequest();
exchangeData();
ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
isInitalised = true;
}
exchangeData();
exchangeData();
unlockMainPID();
}
@ -414,7 +468,7 @@ bool simRealFlightInit(char* ip, uint8_t* mapping, uint8_t mapCount, bool imu)
// Wait until the connection is established, the interface has been initialised
// and the first valid packet has been received to avoid problems with the startup calibration.
while (!isInitalised) {
// ...
delay(250);
}
return true;

44
src/main/target/SITL/sim/simHelper.c
View file

@ -26,22 +26,42 @@
#include <stdio.h>
#include <math.h>
#include "common/quaternion.h"
#include "target/SITL/sim/simHelper.h"
inline double clampd(double value, double min, double max)
inline int16_t constrainToInt16(double value)
{
if (value < min) {
value = min;
}
if (value > max) {
value = max;
}
return value;
return (int16_t)round(constrain(value, INT16_MIN, INT16_MAX));
}
inline int16_t clampToInt16(double value)
// Move to quaternion.h ?
void computeQuaternionFromRPY(fpQuaternion_t *quat, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
{
return (int16_t)round(clampd(value, INT16_MIN, INT16_MAX));
if (initialRoll > 1800) initialRoll -= 3600;
if (initialPitch > 1800) initialPitch -= 3600;
if (initialYaw > 1800) initialYaw -= 3600;
const float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
const float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
const float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
const float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
const float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
const float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
quat->q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
quat->q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
quat->q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
quat->q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
}
void transformVectorEarthToBody(fpVector3_t *v, const fpQuaternion_t *quat)
{
// HACK: This is needed to correctly transform from NED (sensor frame) to NEU (navigation)
v->y = -v->y;
// From earth frame to body frame
quaternionRotateVector(v, v, quat);
}

10
src/main/target/SITL/sim/simHelper.h
View file

@ -23,11 +23,15 @@
*/
#include <stdint.h>
#include "common/maths.h"
#include "common/quaternion.h"
#define EARTH_RADIUS ((double)6378.137)
#define DEG2RAD(deg) (deg * (double)M_PIf / (double)180.0)
#define PWM_TO_FLOAT_0_1(x) (((int)x - 1000) / 1000.0f)
#define PWM_TO_FLOAT_MINUS_1_1(x) (((int)x - 1500) / 500.0f)
#define FLOAT_0_1_TO_PWM(x) ((uint16_t)(x * 1000.0f) + 1000.0f)
#define FLOAT_MINUS_1_1_TO_PWM(x) ((uint16_t)((x + 1.0f) / 2.0f * 1000.0f) + 1000.0f)
double clampd(double value, double min, double max);
int16_t clampToInt16(double value);
int16_t constrainToInt16(double value);
void transformVectorEarthToBody(fpVector3_t *v, const fpQuaternion_t *quat);
void computeQuaternionFromRPY(fpQuaternion_t *quat, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw);

16
src/main/target/SITL/sim/simple_soap_client.c
View file

@ -47,7 +47,7 @@ bool soapClientConnect(soap_client_t *client, const char *address, int port)
}
int one = 1;
if (setsockopt(client->sockedFd, SOL_SOCKET, SO_REUSEADDR, &one,sizeof(one)) < 0) {
if (setsockopt(client->sockedFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) < 0) {
return false;
}
@ -69,6 +69,8 @@ void soapClientClose(soap_client_t *client)
{
close(client->sockedFd);
memset(client, 0, sizeof(soap_client_t));
client->isConnected = false;
client->isInitalised = false;
}
void soapClientSendRequestVa(soap_client_t *client, const char* action, const char *fmt, va_list va)
@ -78,11 +80,15 @@ void soapClientSendRequestVa(soap_client_t *client, const char* action, const ch
}
char* requestBody;
vasprintf(&requestBody, fmt, va);
if (vasprintf(&requestBody, fmt, va) < 0) {
return;
}
char* request;
asprintf(&request, "POST / HTTP/1.1\r\nsoapaction: %s\r\ncontent-length: %u\r\ncontent-type: text/xml;charset='UTF-8'\r\nConnection: Keep-Alive\r\n\r\n<soap:Envelope xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xmlns:xsd=\"http://www.w3.org/2001/XMLSchema\" xmlns:soap=\"http://schemas.xmlsoap.org/soap/envelope/\"><soap:Body>%s</soap:Body></soap:Envelope>",
action, (unsigned)strlen(requestBody), requestBody);
if (asprintf(&request, "POST / HTTP/1.1\r\nsoapaction: %s\r\ncontent-length: %u\r\ncontent-type: text/xml;charset='UTF-8'\r\nConnection: Keep-Alive\r\n\r\n<soap:Envelope xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xmlns:xsd=\"http://www.w3.org/2001/XMLSchema\" xmlns:soap=\"http://schemas.xmlsoap.org/soap/envelope/\"><soap:Body>%s</soap:Body></soap:Envelope>",
action, (unsigned)strlen(requestBody), requestBody) < 0) {
return;
}
send(client->sockedFd, request, strlen(request), 0);
}

199
src/main/target/SITL/sim/xplane.c
View file

@ -45,14 +45,19 @@
#include "sensors/battery_sensor_fake.h"
#include "sensors/acceleration.h"
#include "drivers/pitotmeter/pitotmeter_fake.h"
#include "drivers/compass/compass_fake.h"
#include "drivers/rangefinder/rangefinder_virtual.h"
#include "io/rangefinder.h"
#include "common/utils.h"
#include "common/maths.h"
#include "flight/mixer.h"
#include "flight/servos.h"
#include "flight/imu.h"
#include "io/gps.h"
#include "rx/sim.h"
#define XP_PORT 49000
#define XPLANE_JOYSTICK_AXIS_COUNT 8
static uint8_t pwmMapping[XP_MAX_PWM_OUTS];
@ -67,6 +72,7 @@ static bool useImu = false;
static float lattitude = 0;
static float longitude = 0;
static float elevation = 0;
static float agl = 0;
static float local_vx = 0;
static float local_vy = 0;
static float local_vz = 0;
@ -83,13 +89,27 @@ static float gyro_x = 0;
static float gyro_y = 0;
static float gyro_z = 0;
static float barometer = 0;
static bool hasJoystick = false;
static float joystickRaw[XPLANE_JOYSTICK_AXIS_COUNT];
enum
{
RF_ASSIGNMENT_PITCH,
RF_ASSIGNMENT_ROLL,
RF_ASSIGNMENT_YAW,
RF_ASSIGNMENT_THROTLE,
RF_ASSIGNMENT_PROP,
RF_ASSIGNMENT_MIXTURE,
RF_ASSIGNMENT_COLLECTIVE,
RF_ASSIGNMENT_THRUST_VECTOR
};
typedef enum
{
DREF_LATITUDE,
DREF_LONGITUDE,
DREF_ELEVATION,
DREF_AGL,
DREF_LOCAL_VX,
DREF_LOCAL_VY,
DREF_LOCAL_VZ,
@ -106,7 +126,16 @@ typedef enum
DREF_POS_Q,
DREF_POS_R,
DREF_POS_BARO_CURRENT_INHG,
DREF_COUNT
DREF_COUNT,
DREF_HAS_JOYSTICK,
DREF_JOYSTICK_VALUES_ROll,
DREF_JOYSTICK_VALUES_PITCH,
DREF_JOYSTICK_VALUES_THROTTLE,
DREF_JOYSTICK_VALUES_YAW,
DREF_JOYSTICK_VALUES_PROP,
DREF_JOYSTICK_VALUES_MIXTURE,
DREF_JOYSTICK_VALUES_COLLECTIVE,
DREF_JOYSTICK_VALUES_THRUST_VECTOR,
} dref_t;
uint32_t xint2uint32 (uint8_t * buf)
@ -168,7 +197,7 @@ static void* listenWorker(void* arg)
if (y > 2) {
break;
}
if (pwmMapping[i] & 0x80) { // Motor)
if (pwmMapping[i] & 0x80) { // Motor
motorValue = PWM_TO_FLOAT_0_1(motor[pwmMapping[i] & 0x7f]);
} else {
yokeValues[y] = PWM_TO_FLOAT_MINUS_1_1(servo[pwmMapping[i]]);
@ -184,11 +213,11 @@ static void* listenWorker(void* arg)
recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, (struct sockaddr*)&remoteAddr, &slen);
if (recvLen < 0 && errno != EWOULDBLOCK) {
break;
continue;
}
if (strncmp((char*)buf, "RREF", 4) != 0) {
break;
continue;
}
for (int i = 5; i < recvLen; i += 8) {
@ -208,6 +237,10 @@ static void* listenWorker(void* arg)
case DREF_ELEVATION:
elevation = value;
break;
case DREF_AGL:
agl = value;
break;
case DREF_LOCAL_VX:
local_vx = value;
@ -273,10 +306,45 @@ static void* listenWorker(void* arg)
barometer = value;
break;
case DREF_HAS_JOYSTICK:
hasJoystick = value >= 1 ? true : false;
break;
case DREF_JOYSTICK_VALUES_ROll:
joystickRaw[0] = value;
break;
case DREF_JOYSTICK_VALUES_PITCH:
joystickRaw[1] = value;
break;
case DREF_JOYSTICK_VALUES_THROTTLE:
joystickRaw[2] = value;
break;
case DREF_JOYSTICK_VALUES_YAW:
joystickRaw[3] = value;
break;
case DREF_JOYSTICK_VALUES_PROP:
joystickRaw[4] = value;
break;
case DREF_JOYSTICK_VALUES_MIXTURE:
joystickRaw[5] = value;
break;
case DREF_JOYSTICK_VALUES_COLLECTIVE:
joystickRaw[6] = value;
break;
case DREF_JOYSTICK_VALUES_THRUST_VECTOR:
joystickRaw[7] = value;
break;
default:
break;
}
}
if (hpath < 0) {
@ -287,6 +355,20 @@ static void* listenWorker(void* arg)
yaw += 3600;
}
if (hasJoystick) {
uint16_t channelValues[XPLANE_JOYSTICK_AXIS_COUNT];
channelValues[0] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_ROLL]);
channelValues[1] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_PITCH]);
channelValues[2] = FLOAT_0_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_THROTLE]);
channelValues[3] = FLOAT_MINUS_1_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_YAW]);
channelValues[4] = FLOAT_0_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_PROP]);
channelValues[5] = FLOAT_0_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_MIXTURE]);
channelValues[6] = FLOAT_0_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_COLLECTIVE]);
channelValues[7] = FLOAT_0_1_TO_PWM(joystickRaw[RF_ASSIGNMENT_THRUST_VECTOR]);
rxSimSetChannelValue(channelValues, XPLANE_JOYSTICK_AXIS_COUNT);
}
gpsFakeSet(
GPS_FIX_3D,
16,
@ -300,26 +382,33 @@ static void* listenWorker(void* arg)
0, //(int16_t)round(-local_vy * 100),
0
);
const int32_t altitideOverGround = (int32_t)round(agl * 100);
if (altitideOverGround > 0 && altitideOverGround <= RANGEFINDER_VIRTUAL_MAX_RANGE_CM) {
fakeRangefindersSetData(altitideOverGround);
} else {
fakeRangefindersSetData(-1);
}
const int16_t roll_inav = roll * 10;
const int16_t pitch_inav = -pitch * 10;
const int16_t yaw_inav = yaw * 10;
if (!useImu) {
imuSetAttitudeRPY(
(int16_t)round(roll * 10),
(int16_t)round(-pitch * 10),
(int16_t)round(yaw * 10)
);
imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav);
imuUpdateAttitude(micros());
}
fakeAccSet(
clampToInt16(-accel_x * GRAVITY_MSS * 1000),
clampToInt16(accel_y * GRAVITY_MSS * 1000),
clampToInt16(accel_z * GRAVITY_MSS * 1000)
constrainToInt16(-accel_x * GRAVITY_MSS * 1000),
constrainToInt16(accel_y * GRAVITY_MSS * 1000),
constrainToInt16(accel_z * GRAVITY_MSS * 1000)
);
fakeGyroSet(
clampToInt16(gyro_x * 16.0f),
clampToInt16(-gyro_y * 16.0f),
clampToInt16(-gyro_z * 16.0f)
constrainToInt16(gyro_x * 16.0f),
constrainToInt16(-gyro_y * 16.0f),
constrainToInt16(-gyro_z * 16.0f)
);
fakeBaroSet((int32_t)round(barometer * 3386.39f), DEGREES_TO_CENTIDEGREES(21));
@ -327,8 +416,24 @@ static void* listenWorker(void* arg)
fakeBattSensorSetVbat(16.8 * 100);
fpQuaternion_t quat;
fpVector3_t north;
north.x = 1.0f;
north.y = 0;
north.z = 0;
computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav);
transformVectorEarthToBody(&north, &quat);
fakeMagSet(
constrainToInt16(north.x * 16000.0f),
constrainToInt16(north.y * 16000.0f),
constrainToInt16(north.z * 16000.0f)
);
ENABLE_STATE(COMPASS_CALIBRATED);
if (!initalized) {
ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
// Aircraft can wobble on the runway and prevents calibration of the accelerometer
ENABLE_STATE(ACCELEROMETER_CALIBRATED);
initalized = true;
}
@ -362,42 +467,52 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
return false;
}
if (bind(sockFd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
if (setsockopt(sockFd, SOL_SOCKET, SO_SNDTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) {
return false;
}
bind(sockFd, (struct sockaddr *) &addr, sizeof(addr));
serverAddr.sin_family = AF_INET;
serverAddr.sin_addr.s_addr = inet_addr(ip);
serverAddr.sin_port = htons(port);
registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100);
registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100);
registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100);
registerDref(DREF_LOCAL_VX, "sim/flightmodel/position/local_vx", 100);
registerDref(DREF_LOCAL_VY, "sim/flightmodel/position/local_vy", 100);
registerDref(DREF_LOCAL_VZ, "sim/flightmodel/position/local_vz", 100);
registerDref(DREF_GROUNDSPEED, "sim/flightmodel/position/groundspeed", 100);
registerDref(DREF_TRUE_AIRSPEED, "sim/flightmodel/position/true_airspeed", 100);
registerDref(DREF_POS_PHI, "sim/flightmodel/position/phi", 100);
registerDref(DREF_POS_THETA, "sim/flightmodel/position/theta", 100);
registerDref(DREF_POS_PSI, "sim/flightmodel/position/psi", 100);
registerDref(DREF_POS_HPATH, "sim/flightmodel/position/hpath", 100);
registerDref(DREF_FORCE_G_AXI1, "sim/flightmodel/forces/g_axil", 100);
registerDref(DREF_FORCE_G_SIDE, "sim/flightmodel/forces/g_side", 100);
registerDref(DREF_FORCE_G_NRML, "sim/flightmodel/forces/g_nrml", 100);
registerDref(DREF_POS_P, "sim/flightmodel/position/P", 100);
registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", 100);
registerDref(DREF_POS_R, "sim/flightmodel/position/R", 100);
registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", 100);
if (pthread_create(&listenThread, NULL, listenWorker, NULL) < 0) {
return false;
}
while(!initalized) {
//
while (!initalized) {
registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100);
registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100);
registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100);
registerDref(DREF_AGL, "sim/flightmodel/position/y_agl", 100);
registerDref(DREF_LOCAL_VX, "sim/flightmodel/position/local_vx", 100);
registerDref(DREF_LOCAL_VY, "sim/flightmodel/position/local_vy", 100);
registerDref(DREF_LOCAL_VZ, "sim/flightmodel/position/local_vz", 100);
registerDref(DREF_GROUNDSPEED, "sim/flightmodel/position/groundspeed", 100);
registerDref(DREF_TRUE_AIRSPEED, "sim/flightmodel/position/true_airspeed", 100);
registerDref(DREF_POS_PHI, "sim/flightmodel/position/phi", 100);
registerDref(DREF_POS_THETA, "sim/flightmodel/position/theta", 100);
registerDref(DREF_POS_PSI, "sim/flightmodel/position/psi", 100);
registerDref(DREF_POS_HPATH, "sim/flightmodel/position/hpath", 100);
registerDref(DREF_FORCE_G_AXI1, "sim/flightmodel/forces/g_axil", 100);
registerDref(DREF_FORCE_G_SIDE, "sim/flightmodel/forces/g_side", 100);
registerDref(DREF_FORCE_G_NRML, "sim/flightmodel/forces/g_nrml", 100);
registerDref(DREF_POS_P, "sim/flightmodel/position/P", 100);
registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", 100);
registerDref(DREF_POS_R, "sim/flightmodel/position/R", 100);
registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", 100);
registerDref(DREF_HAS_JOYSTICK, "sim/joystick/has_joystick", 100);
registerDref(DREF_JOYSTICK_VALUES_ROll, "sim/joystick/joy_mapped_axis_value[1]", 100);
registerDref(DREF_JOYSTICK_VALUES_PITCH, "sim/joystick/joy_mapped_axis_value[2]", 100);
registerDref(DREF_JOYSTICK_VALUES_THROTTLE, "sim/joystick/joy_mapped_axis_value[3]", 100);
registerDref(DREF_JOYSTICK_VALUES_YAW, "sim/joystick/joy_mapped_axis_value[4]", 100);
registerDref(DREF_JOYSTICK_VALUES_PROP, "sim/joystick/joy_mapped_axis_value[8]", 100);
registerDref(DREF_JOYSTICK_VALUES_MIXTURE, "sim/joystick/joy_mapped_axis_value[9]", 100);
registerDref(DREF_JOYSTICK_VALUES_COLLECTIVE, "sim/joystick/joy_mapped_axis_value[5]", 100);
registerDref(DREF_JOYSTICK_VALUES_THRUST_VECTOR, "sim/joystick/joy_mapped_axis_value[12]", 100);
delay(250);
}
return true;
}

92
src/main/target/SITL/target.c
View file

@ -33,6 +33,7 @@
#include <errno.h>
#include <time.h>
#include <pthread.h>
#include <unistd.h>
#include <platform.h>
#include "target.h"
@ -44,6 +45,7 @@
#include "drivers/pwm_mapping.h"
#include "drivers/timer.h"
#include "drivers/serial.h"
#include "config/config_streamer.h"
#include "target/SITL/sim/realFlight.h"
#include "target/SITL/sim/xplane.h"
@ -66,46 +68,55 @@ static int simPort = 0;
void systemInit(void) {
printf("INAV %d.%d.%d SITL\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL);
fprintf(stderr, "INAV %d.%d.%d SITL\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL);
clock_gettime(CLOCK_MONOTONIC, &start_time);
printf("[SYSTEM] Init...\n");
fprintf(stderr, "[SYSTEM] Init...\n");
pthread_attr_t thAttr;
int policy = 0;
pthread_attr_init(&thAttr);
pthread_attr_getschedpolicy(&thAttr, &policy);
pthread_setschedprio(pthread_self(), sched_get_priority_min(policy));
pthread_attr_destroy(&thAttr);
if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
printf("[SYSTEM] Unable to create mainLoop lock.\n");
fprintf(stderr, "[SYSTEM] Unable to create mainLoop lock.\n");
exit(1);
}
if (sitlSim != SITL_SIM_NONE) {
printf("[SIM] Waiting for connection...\n");
fprintf(stderr, "[SIM] Waiting for connection...\n");
}
switch (sitlSim) {
case SITL_SIM_REALFLIGHT:
if (mappingCount > RF_MAX_PWM_OUTS) {
printf("[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", RF_MAX_PWM_OUTS);
fprintf(stderr, "[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", RF_MAX_PWM_OUTS);
sitlSim = SITL_SIM_NONE;
break;
}
if (simRealFlightInit(simIp, pwmMapping, mappingCount, useImu)) {
printf("[SIM] Connection with RealFlight (%s) successfully established. \n", simIp);
fprintf(stderr, "[SIM] Connection with RealFlight successfully established.\n");
} else {
printf("[SIM] Connection with RealFlight (%s) NOT established. \n", simIp);
fprintf(stderr, "[SIM] Connection with RealFlight NOT established.\n");
}
break;
case SITL_SIM_XPLANE:
if (mappingCount > XP_MAX_PWM_OUTS) {
printf("[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", XP_MAX_PWM_OUTS);
fprintf(stderr, "[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", XP_MAX_PWM_OUTS);
sitlSim = SITL_SIM_NONE;
break;
}
if (simXPlaneInit(simIp, simPort, pwmMapping, mappingCount, useImu)) {
printf("[SIM] Connection with XPlane successfully established. \n");
fprintf(stderr, "[SIM] Connection with X-Plane successfully established.\n");
} else {
printf("[SIM] Connection with XPLane NOT established. \n");
fprintf(stderr, "[SIM] Connection with X-PLane NOT established.\n");
}
break;
default:
printf("[SIM] No interface specified. Configurator only.\n");
fprintf(stderr, "[SIM] No interface specified. Configurator only.\n");
break;
}
@ -149,21 +160,22 @@ bool parseMapping(char* mapStr)
void printCmdLineOptions(void)
{
printf("Avaiable options:\n");
printf("--sim=[rf|xp] Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n");
printf("--simip=[ip] IP-Address oft the simulator host. If not specified localhost (127.0.0.1) is used.");
printf("--simport=[port] Port oft the simulator host.");
printf("--useimu Use IMU sensor data from the simulator instead of using attitude data from the simulator directly (experimental, not recommended).");
printf("--chanmap=[mapstring] Channel mapping, Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n");
printf(" The mapstring has the following format: M(otor)|S(servo)<INAV-OUT>-<RECEIVER_OUT>,)... All numbers must have two digits\n");
printf(" For example map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n");
printf(" --chanmap=M01-01,S01-02,S02-03\n");
fprintf(stderr, "Avaiable options:\n");
fprintf(stderr, "--path=[path] Path and filename of eeprom.bin. If not specified 'eeprom.bin' in program directory is used.\n");
fprintf(stderr, "--sim=[rf|xp] Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n");
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, "--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");
fprintf(stderr, " --chanmap=M01-01,S01-02,S02-03\n");
}
void parseArguments(int argc, char *argv[])
{
int c;
while(1) {
while(true) {
static struct option longOpt[] = {
{"sim", optional_argument, 0, 's'},
{"useimu", optional_argument, 0, 'u'},
@ -171,6 +183,7 @@ void parseArguments(int argc, char *argv[])
{"simip", optional_argument, 0, 'i'},
{"simport", optional_argument, 0, 'p'},
{"help", optional_argument, 0, 'h'},
{"path", optional_argument, 0, 'e'},
{NULL, 0, NULL, 0}
};
@ -185,13 +198,13 @@ void parseArguments(int argc, char *argv[])
} else if (strcmp(optarg, "xp") == 0){
sitlSim = SITL_SIM_XPLANE;
} else {
printf("[SIM] Unsupported simulator %s.\n", optarg);
fprintf(stderr, "[SIM] Unsupported simulator %s.\n", optarg);
}
break;
case 'c':
if (!parseMapping(optarg) && sitlSim != SITL_SIM_NONE) {
printf("[SIM] Invalid channel mapping string.\n");
fprintf(stderr, "[SIM] Invalid channel mapping string.\n");
printCmdLineOptions();
exit(0);
}
@ -205,7 +218,11 @@ void parseArguments(int argc, char *argv[])
case 'i':
simIp = optarg;
break;
case 'e':
if (!configFileSetPath(optarg)){
fprintf(stderr, "[EEPROM] Invalid path, using eeprom file in program directory\n.");
}
break;
case 'h':
printCmdLineOptions();
exit(0);
@ -230,17 +247,6 @@ void unlockMainPID(void)
}
// Replacements for system functions
void microsleep(uint32_t usec) {
struct timespec ts;
ts.tv_sec = 0;
ts.tv_nsec = usec*1000UL;
while (nanosleep(&ts, &ts) == -1 && errno == EINTR) ;
}
void delayMicroseconds_real(uint32_t us) {
microsleep(us);
}
timeUs_t micros(void) {
struct timespec now;
clock_gettime(CLOCK_MONOTONIC, &now);
@ -259,31 +265,31 @@ uint32_t millis(void) {
void delayMicroseconds(timeUs_t us)
{
timeUs_t now = micros();
while (micros() - now < us);
usleep(us);
}
void delay(timeMs_t ms)
{
while (ms--)
delayMicroseconds(1000);
delayMicroseconds(ms * 1000UL);
}
void systemReset(void)
{
printf("[SYSTEM] Reset\n");
fprintf(stderr, "[SYSTEM] Reset\n");
exit(0);
}
void systemResetToBootloader(void)
{
printf("[SYSTEM] Reset to bootloader\n");
fprintf(stderr, "[SYSTEM] Reset to bootloader\n");
exit(0);
}
void failureMode(failureMode_e mode) {
printf("[SYSTEM] Failure mode %d\n", mode);
while (1);
fprintf(stderr, "[SYSTEM] Failure mode %d\n", mode);
while (true) {
delay(1000);
};
}
// Even more dummys and stubs

2
src/main/target/SITL/target.h
View file

@ -65,6 +65,8 @@
#define USE_FAKE_BARO
#define USE_FAKE_MAG
#define USE_GPS_FAKE
#define USE_RANGEFINDER_FAKE
#define USE_RX_SIM
#undef USE_DASHBOARD
#undef USE_TELEMETRY_LTM