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

Merge remote-tracking branch 'origin/development' into dzikuvx-f7-speedup

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2019-02-09 19:59:49 +01:00
commit fbeddfb6a6
82 changed files with 2730 additions and 492 deletions
Download patch file Download diff file Expand all files Collapse all files

2
Makefile
View file

@ -121,7 +121,7 @@ endif
GROUP_1_TARGETS := ALIENFLIGHTF3 ALIENFLIGHTF4 AIRHEROF3 AIRHEROF3_QUAD COLIBRI_RACE LUX_RACE SPARKY REVO SPARKY2 COLIBRI KISSFC FALCORE FF_F35_LIGHTNING FF_FORTINIF4 FF_PIKOF4 FF_PIKOF4OSD
GROUP_2_TARGETS := SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO CLRACINGF4AIR CLRACINGF4AIRV2 BEEROTORF4 BETAFLIGHTF3 BETAFLIGHTF4 PIKOBLX SPRACINGF3NEO
GROUP_3_TARGETS := OMNIBUS AIRBOTF4 BLUEJAYF4 OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4V3 FIREWORKSV2 SPARKY2 MATEKF405 OMNIBUSF7 DYSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 OMNIBUSF7NXT OMNIBUSF7V2 ASGARD32F4
GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ANYFCM7 ALIENFLIGHTNGF7 PIXRACER YUPIF4 YUPIF4MINI YUPIF4R2 YUPIF7 MATEKF405SE MATEKF411 MATEKF722 MATEKF405OSD MATEKF405_SERVOS6
GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ANYFCM7 ALIENFLIGHTNGF7 PIXRACER YUPIF4 YUPIF4MINI YUPIF4R2 YUPIF7 MATEKF405SE MATEKF411 MATEKF722 MATEKF405OSD MATEKF405_SERVOS6 NOX
GROUP_OTHER_TARGETS := $(filter-out $(GROUP_1_TARGETS) $(GROUP_2_TARGETS) $(GROUP_3_TARGETS) $(GROUP_4_TARGETS), $(VALID_TARGETS))
REVISION = $(shell git rev-parse --short HEAD)

9
docs/Board - Matek F411 Wing.md
View file

@ -16,8 +16,13 @@
## Details
* [Full specification](http://www.mateksys.com/?portfolio=f411-wing)
* For Matek F411-WING use `MATEKF411` firmware.
* SBS pad has a built-in inverter and is connected to UART1 RX
* SBUS pad has a built-in inverter and is connected to UART1 RX
## Available TARGETS
* `MATEKF411` if you want to control LEDs and have SS1 TX on ST1 pad.
* `MATEKF411_SFTSRL2`if you want to use two softserials (TX only) at the same time. Eg. Smart Audio + S. Port, Smart Audio + LTM
* `MATEKF411_RSSI` if you want to have analog RSSI input on ST1 pad. SS1 TX will be available on the LED pad.
## Where to buy:

46
docs/Board - NOX.md Executable file
View file

@ -0,0 +1,46 @@
# Board - [NOX](https://inavflight.com/shop/p/NOX)
![Airbot]https://youtu.be/j3zIYFYZ_SQ
![Banggood](https://img.staticbg.com/thumb/view/oaupload/banggood/images/A5/01/79d28a2c-ef4b-4e4f-bab6-14edf66bbb23.jpg)
![Banggood]https://img.staticbg.com/images/oaupload/banggood/images/2E/C5/c14a1e86-4e58-4bc8-85de-8e344cb382b9.jpg)
![Banggood]https://img.staticbg.com/images/oaupload/banggood/images/42/F7/45a68ade-9be1-4fff-afec-bbdd45f0331d.jpg)
## Airbot Specification:
* Betaflight OSD
* STM32F411 MCU
* MPU6000 Gyro - It's also replaceable
* Barometer
* 5V BEC with LC filter (500ma)
* 4000uf capacitors onboard - No Need for bulky caps
* 4in1 ESC
* BLHeli32 - The 32-bit architecture that we've come to expect
* DSHOT 1200
* 35A Per motor
* Telemetry Output functionality
* ESC upgradeable
## Banggood Specification:
* Model: F4 Nox4
* Version: Acro Version / Deluxe Version
* Acro Version: Without Barometer and Blackbox
* Deluxe Version: With Barometer and Blackbox
* CPU: STM32F411C
* MPU: MPU6000
* Input Voltage: Support 2-4S Lipo Input
* Built-In Betaflight OSD
* Built-in 5V @ 3A BEC
* 3.3V
* Built-in LC Filter
* DShot, Proshot ESC
* Support Spektrum 1024 /2048 , SBUS, IBUS, PPM
* Size: 27x27mm
* Mounting Hole: 20x20mm , M2.5
* Weight: 3.3g
* DSM / IBUS/SBUS Receiver, choose UART RX2
* PPM Receiver, don't need choose UART Port
## Where to buy:
* [Airbot](https://store.myairbot.com/noxv2.html)
* [Banggood](https://www.banggood.com/nl/20x20mm-Betaflight-F4-Noxe-MPU6000-Flight-Controller-AIO-OSD-5V-BEC-Built-in-LC-Filter-for-RC-Drone-p-1310419.html)

5
docs/Cli.md
View file

@ -96,9 +96,6 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag
| i2c_speed | 400KHZ | This setting controls the clock speed of I2C bus. 400KHZ is the default that most setups are able to use. Some noise-free setups may be overclocked to 800KHZ. Some sensor chips or setups with long wires may work unreliably at 400KHZ - user can try lowering the clock speed to 200KHZ or even 100KHZ. User need to bear in mind that lower clock speeds might require higher looptimes (lower looptime rate) |
| cpu_underclock | OFF | This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz |
| gyro_sync | OFF | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf |
| acc_task_frequency | 500 | Determines accelerometer task frequency in async_mode = ALL. Depending on UAV type this frequency can be lowered to conserve CPU resources as long as vibrations are not a problem. |
| attitude_task_frequency | 250 | Determines attitude task frequency when async_mode = ALL |
| async_mode | NONE | Enables asynchronous mode processing for gyro/accelerometer and attitude computations. Allowed modes: NONE -> default behavior, all calculations are executed in main PID loop. GYRO -> gyro sampling and filtering is detached from main PID loop. PID loop runs based on looptime while gyro sampling rate is determined by the gyro driver using also the gyro_hardware_lpf setting. ALL -> in this mode, gyro, accelerometer and attitude are running as separate tasks. Accelerometer task frequency is determined by acc_task_frequency, attitude task frequency by attitude_task_frequency. In this mode ANGLE and HORIZON, as well GPS assisted flight modes (including PosHold) performance might be lowered if improper settings are used. |
| min_check | 1100 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. |
| max_check | 1900 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. |
| rssi_channel | 0 | RX channel containing the RSSI signal |
@ -114,7 +111,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag
| 3d_neutral | 1460 | Neutral (stop) throttle value for 3D mode |
| 3d_deadband_throttle | 50 | Throttle signal will be held to a fixed value when throttle is centered with an error margin defined in this parameter. |
| motor_pwm_rate | 400 | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. |
| motor_pwm_protocol | STANDARD | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, BRUSHED |
| motor_pwm_protocol | STANDARD | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED |
| fixed_wing_auto_arm | OFF | Auto-arm fixed wing aircraft on throttle above min_throttle, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. |
| disarm_kill_switch | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. |
| auto_disarm_delay | 5 | Delay before automatic disarming when using stick arming and MOTOR_STOP. This does not apply when using FIXED_WING |

2
docs/Controls.md
View file

@ -41,7 +41,7 @@ The stick positions are combined to activate different functions:
| Save current waypoint mission | LOW | CENTER | HIGH | LOW |
| Load current waypoint mission | LOW | CENTER | HIGH | HIGH |
| Save setting | LOW | LOW | LOW | HIGH |
| Enter OSD Menu (CMS) | CENTER | LOW | HIGH | CENTER |
![Stick Positions](assets/images/StickPositions.png)
## Yaw control

33
docs/Looptime.md
View file

@ -1,33 +0,0 @@
# Looptime / Control loop frequency
Looptime / Control loop frequency (how often PID controller is executed and data is passed to motors) in INAV depends on multiple settings.
More, with asynchronous gyro processing, not all tasks are executed in the same time. Depending on `async_mode` setting, there are following cases:
## `async_mode = NONE`
Gyroscope sampling and filtering, Accelerometer sampling and filtering, Attitude computation and PID control loop are executed as single tasks and processed one after another.
| **gyro_sync** | **gyro_lpf** | **Control Loop Rate** (actual looptime) [us] |
| ---- | ---- | ----- |
| OFF | any | `looptime` |
| ON | != 256HZ | 1000 * `gyro_sync_denom` |
| ON | = 256HZ | 125 * `gyro_sync_denom` |
## `async_mode = GYRO`
In this mode, gyro sampling and filtering is decoupled from PID main loop and executed separately. In this mode, `gyro_sync` is forced and is always **ON**
| **gyro_lpf** | **Control Loop Rate** [us] | Gyro looptime [us] |
| ---- | ----- | ---- |
| != 256HZ | `looptime` | 1000 * `gyro_sync_denom` |
| = 256HZ | `looptime` | 125 * `gyro_sync_denom` |
## `async_mode = ALL`
In this mode, Gyroscope sampling and filtering, Accelerometer sampling and filtering, Attitude computation and PID control loop are decoupled and run as separate tasks.
| **gyro_lpf** | **Control Loop Rate** [us] | Gyro looptime [us] | Accelerometer looptime [us] | Attitude looptime [us] |
| ---- | ----- | ---- | ---- | ---- |
| != 256HZ | `looptime` | 1000 * `gyro_sync_denom` | 1000000 / `acc_task_frequency` | 1000000 / `attitude_task_frequency` |
| = 256HZ | `looptime` | 125 * `gyro_sync_denom` | 1000000 / `acc_task_frequency` | 1000000 / `attitude_task_frequency` |

BIN
docs/assets/images/StickPositions.png
View file

Binary file not shown.
Side by side Swipe Overlay

Before

Width:  |  Height:  |  Size: 208 KiB

After

Width:  |  Height:  |  Size: 717 KiB

Before After
Before After

43
docs/assets/images/StickPositions.svg
View file

@ -14,7 +14,7 @@
viewBox="0 0 8782.4284 4964.1146"
id="svg2"
version="1.1"
inkscape:version="0.92.2 (5c3e80d, 2017-08-06)"
inkscape:version="0.92.4 (5da689c313, 2019-01-14)"
sodipodi:docname="StickPositions.svg"
inkscape:export-filename="C:\Users\stuphi\Dropbox\projects\quad\StickPositions.png"
inkscape:export-xdpi="74.996788"
@ -778,6 +778,17 @@
y="1227.7418"
id="tspan6015"
style="font-size:286.5987854px;line-height:1.25;text-align:end;text-anchor:end">Save Setting</tspan></text>
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;line-height:0%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
x="5630.3838"
y="1642.0621"
id="text4221"><tspan
sodipodi:role="line"
x="5630.3838"
y="1642.0621"
id="tspan4219"
style="font-size:286.5987854px;line-height:1.25;text-align:end;text-anchor:end">Enter OSD Menu (CMS)</tspan></text>
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;line-height:0%;font-family:sans-serif;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
@ -914,5 +925,35 @@
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
inkscape:transform-center-y="-73.935654"
inkscape:transform-center-x="73.935669" />
<rect
ry="0"
y="-1679.5159"
x="-6530.0146"
height="288.94339"
width="288.94339"
id="rect5919-6"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
transform="scale(-1)" />
<path
sodipodi:nodetypes="sssss"
inkscape:connector-curvature="0"
id="path5923-2"
d="m 6352.7615,1369.2446 c 0,-10.5728 14.6765,-19.1438 32.7809,-19.1438 18.1044,0 32.7809,8.571 32.7809,19.1438 0,10.5729 -14.6765,163.5957 -32.7809,163.5957 -18.1044,0 -32.7809,-153.0228 -32.7809,-163.5957 z"
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
<rect
ry="0"
y="1390.7616"
x="-6125.2622"
height="288.94339"
width="288.94339"
id="rect5733-9"
style="display:inline;opacity:1;fill:none;fill-opacity:1;stroke:#808080;stroke-width:11.05660725;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
transform="scale(-1,1)" />
<path
style="display:inline;opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:8.07780743;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
d="m 5814.1209,1502.592 c -10.5728,0 -19.1438,14.6765 -19.1438,32.7809 0,18.1044 8.571,32.7809 19.1438,32.7809 10.5729,0 163.5957,-14.6765 163.5957,-32.7809 0,-18.1044 -153.0228,-32.7809 -163.5957,-32.7809 z"
id="path5747-5"
inkscape:connector-curvature="0"
sodipodi:nodetypes="sssss" />
</g>
</svg>

Side by side Swipe Overlay

Before

Width:  |  Height:  |  Size: 46 KiB

After

Width:  |  Height:  |  Size: 48 KiB

Before After
Before After

2
make/release.mk
View file

@ -19,3 +19,5 @@ RELEASE_TARGETS += MATEKF405 MATEKF405_SERVOS6 MATEKF405OSD MATEKF722 MATEKF722S
RELEASE_TARGETS += FOXEERF405 FOXEERF722DUAL
RELEASE_TARGETS += SPEEDYBEEF4 FRSKYF3 FRSKYF4
RELEASE_TARGETS += NOX

5
make/source.mk
View file

@ -11,6 +11,7 @@ COMMON_SRC = \
common/encoding.c \
common/filter.c \
common/maths.c \
common/calibration.c \
common/memory.c \
common/olc.c \
common/printf.c \
@ -53,6 +54,10 @@ COMMON_SRC = \
drivers/system.c \
drivers/timer.c \
drivers/lights_io.c \
drivers/1-wire.c \
drivers/1-wire/ds_crc.c \
drivers/1-wire/ds2482.c \
drivers/temperature/ds18b20.c \
drivers/temperature/lm75.c \
drivers/pitotmeter_ms4525.c \
fc/cli.c \

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

@ -93,7 +93,7 @@
#define BLACKBOX_INTERVED_CARD_DETECTION 0
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 1);
PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
.device = DEFAULT_BLACKBOX_DEVICE,
@ -353,8 +353,20 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
{"wind", 0, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"wind", 1, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"wind", 2, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"temperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"temperatureSource", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
{"IMUTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
#ifdef USE_BARO
{"baroTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
#endif
#ifdef USE_TEMPERATURE_SENSOR
{"sens0Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens1Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens2Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens3Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens4Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens5Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens6Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
{"sens7Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
#endif
};
typedef enum BlackboxState {
@ -453,8 +465,13 @@ typedef struct blackboxSlowState_s {
uint16_t powerSupplyImpedance;
uint16_t sagCompensatedVBat;
int16_t wind[XYZ_AXIS_COUNT];
int16_t temperature;
uint8_t temperatureSource;
int16_t imuTemperature;
#ifdef USE_BARO
int16_t baroTemperature;
#endif
#ifdef USE_TEMPERATURE_SENSOR
int16_t tempSensorTemperature[MAX_TEMP_SENSORS];
#endif
} __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
//From rc_controls.c
@ -1042,8 +1059,15 @@ static void writeSlowFrame(void)
blackboxWriteSigned16VBArray(slowHistory.wind, XYZ_AXIS_COUNT);
blackboxWriteSignedVB(slowHistory.temperature);
blackboxWriteUnsignedVB(slowHistory.temperatureSource);
blackboxWriteSignedVB(slowHistory.imuTemperature);
#ifdef USE_BARO
blackboxWriteSignedVB(slowHistory.baroTemperature);
#endif
#ifdef USE_TEMPERATURE_SENSOR
blackboxWriteSigned16VBArray(slowHistory.tempSensorTemperature, MAX_TEMP_SENSORS);
#endif
blackboxSlowFrameIterationTimer = 0;
}
@ -1075,11 +1099,24 @@ static void loadSlowState(blackboxSlowState_t *slow)
#else
slow->wind[i] = 0;
#endif
slow->temperature = (int) getCurrentTemperature() * 10;
slow->temperatureSource = getCurrentTemperatureSensorUsed();
}
bool valid_temp;
valid_temp = getIMUTemperature(&slow->imuTemperature);
if (!valid_temp) slow->imuTemperature = TEMPERATURE_INVALID_VALUE;
#ifdef USE_BARO
valid_temp = getBaroTemperature(&slow->baroTemperature);
if (!valid_temp) slow->baroTemperature = TEMPERATURE_INVALID_VALUE;
#endif
#ifdef USE_TEMPERATURE_SENSOR
for (uint8_t index; index < MAX_TEMP_SENSORS; ++index) {
valid_temp = getSensorTemperature(index, slow->tempSensorTemperature + index);
if (!valid_temp) slow->tempSensorTemperature[index] = TEMPERATURE_INVALID_VALUE;
}
#endif
}
/**
@ -1540,7 +1577,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("Firmware date", "%s %s", buildDate, buildTime);
BLACKBOX_PRINT_HEADER_LINE("Log start datetime", "%s", blackboxGetStartDateTime(buf));
BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->name);
BLACKBOX_PRINT_HEADER_LINE("P interval", "%d/%d", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
BLACKBOX_PRINT_HEADER_LINE("P interval", "%u/%u", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro_scale", "0x%x", castFloatBytesToInt(1.0f));
@ -1616,7 +1653,7 @@ static bool blackboxWriteSysinfo(void)
gyroConfig()->gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
gyroConfig()->gyro_soft_notch_cutoff_2);
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", pidProfile()->acc_soft_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", accelerometerConfig()->acc_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware);
BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware);
BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware);
@ -1949,18 +1986,18 @@ void blackboxInit(void)
blackboxSetState(BLACKBOX_STATE_DISABLED);
}
/* FIXME is this really necessary ? Why? */
int max_denom = 4096*1000 / gyroConfig()->looptime;
if (blackboxConfig()->rate_denom > max_denom) {
blackboxConfigMutable()->rate_denom = max_denom;
}
/* Decide on how ofter are we going to log I-frames*/
if (blackboxConfig()->rate_denom <= 32) {
blackboxIFrameInterval = 32;
}
else if (blackboxConfig()->rate_denom <= 64) {
blackboxIFrameInterval = 64;
}
else if (blackboxConfig()->rate_denom <= 128) {
blackboxIFrameInterval = 128;
}
else {
blackboxIFrameInterval = 256;
// Use next higher power of two via GCC builtin
blackboxIFrameInterval = 1 << (32 - __builtin_clz (blackboxConfig()->rate_denom - 1));
}
}
#endif

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

@ -22,8 +22,8 @@
#include "config/parameter_group.h"
typedef struct blackboxConfig_s {
uint8_t rate_num;
uint8_t rate_denom;
uint16_t rate_num;
uint16_t rate_denom;
uint8_t device;
uint8_t invertedCardDetection;
} blackboxConfig_t;

18
src/main/cms/cms_menu_osd.c
View file

@ -245,13 +245,27 @@ static const OSD_Entry menuOsdElemsEntries[] =
OSD_ELEMENT_ENTRY("WIND HOR", OSD_WIND_SPEED_HORIZONTAL),
OSD_ELEMENT_ENTRY("WIND VERT", OSD_WIND_SPEED_VERTICAL),
OSD_ELEMENT_ENTRY("TEMPERATURE", OSD_TEMPERATURE),
OSD_ELEMENT_ENTRY("IMU TEMP", OSD_IMU_TEMPERATURE),
#ifdef USE_BARO
OSD_ELEMENT_ENTRY("BARO TEMP", OSD_BARO_TEMPERATURE),
#endif
#ifdef USE_TEMPERATURE_SENSOR
OSD_ELEMENT_ENTRY("SENSOR 0 TEMP", OSD_TEMP_SENSOR_0_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 1 TEMP", OSD_TEMP_SENSOR_1_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 2 TEMP", OSD_TEMP_SENSOR_2_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 3 TEMP", OSD_TEMP_SENSOR_3_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 4 TEMP", OSD_TEMP_SENSOR_4_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 5 TEMP", OSD_TEMP_SENSOR_5_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 6 TEMP", OSD_TEMP_SENSOR_6_TEMPERATURE),
OSD_ELEMENT_ENTRY("SENSOR 7 TEMP", OSD_TEMP_SENSOR_7_TEMPERATURE),
#endif
OSD_BACK_ENTRY,
OSD_END_ENTRY,
};
#if defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT)
#if defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) && defined(USE_TEMPERATURE_SENSOR)
// All CMS OSD elements should be enabled in this case. The menu has 3 extra
// elements (label, back and end), but there's an OSD element that we intentionally
// don't show here (OSD_DEBUG).

189
src/main/common/calibration.c Normal file
View file

@ -0,0 +1,189 @@
/*
* 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 <string.h>
#include <math.h>
#include "build/debug.h"
#include "drivers/time.h"
#include "common/calibration.h"
void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure)
{
// Reset parameters and state
s->params.state = ZERO_CALIBRATION_IN_PROGRESS;
s->params.startTimeMs = millis();
s->params.windowSizeMs = window;
s->params.stdDevThreshold = threshold;
s->params.allowFailure = allowFailure;
s->params.sampleCount = 0;
s->val.accumulatedValue = 0;
devClear(&s->val.stdDev);
}
bool zeroCalibrationIsCompleteS(zeroCalibrationScalar_t * s)
{
return !(s->params.state == ZERO_CALIBRATION_IN_PROGRESS);
}
bool zeroCalibrationIsSuccessfulS(zeroCalibrationScalar_t * s)
{
return (s->params.state == ZERO_CALIBRATION_DONE);
}
void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v)
{
if (s->params.state != ZERO_CALIBRATION_IN_PROGRESS) {
return;
}
// Add value
s->val.accumulatedValue += v;
s->params.sampleCount++;
devPush(&s->val.stdDev, v);
// Check if calibration is complete
if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
const float stddev = devStandardDeviation(&s->val.stdDev);
if (stddev > s->params.stdDevThreshold) {
if (!s->params.allowFailure) {
// If deviation is too big - restart calibration
s->params.startTimeMs = millis();
s->params.sampleCount = 0;
s->val.accumulatedValue = 0;
devClear(&s->val.stdDev);
}
else {
// We are allowed to fail
s->params.state = ZERO_CALIBRATION_FAIL;
}
}
else {
// All seems ok - calculate average value
s->val.accumulatedValue = s->val.accumulatedValue / s->params.sampleCount;
s->params.state = ZERO_CALIBRATION_DONE;
}
}
}
void zeroCalibrationGetZeroS(zeroCalibrationScalar_t * s, float * v)
{
if (s->params.state != ZERO_CALIBRATION_DONE) {
*v = 0.0f;
}
else {
*v = s->val.accumulatedValue;
}
}
void zeroCalibrationStartV(zeroCalibrationVector_t * s, timeMs_t window, float threshold, bool allowFailure)
{
// Reset parameters and state
s->params.state = ZERO_CALIBRATION_IN_PROGRESS;
s->params.startTimeMs = millis();
s->params.windowSizeMs = window;
s->params.stdDevThreshold = threshold;
s->params.allowFailure = allowFailure;
s->params.sampleCount = 0;
for (int i = 0; i < 3; i++) {
s->val[i].accumulatedValue = 0;
devClear(&s->val[i].stdDev);
}
}
bool zeroCalibrationIsCompleteV(zeroCalibrationVector_t * s)
{
return !(s->params.state == ZERO_CALIBRATION_IN_PROGRESS);
}
bool zeroCalibrationIsSuccessfulV(zeroCalibrationVector_t * s)
{
return (s->params.state == ZERO_CALIBRATION_DONE);
}
void zeroCalibrationAddValueV(zeroCalibrationVector_t * s, const fpVector3_t * v)
{
if (s->params.state != ZERO_CALIBRATION_IN_PROGRESS) {
return;
}
// Add value
for (int i = 0; i < 3; i++) {
s->val[i].accumulatedValue += v->v[i];
devPush(&s->val[i].stdDev, v->v[i]);
}
s->params.sampleCount++;
// Check if calibration is complete
if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
bool needRecalibration = false;
for (int i = 0; i < 3 && !needRecalibration; i++) {
const float stddev = devStandardDeviation(&s->val[i].stdDev);
//debug[i] = stddev;
if (stddev > s->params.stdDevThreshold) {
needRecalibration = true;
}
}
if (needRecalibration) {
if (!s->params.allowFailure) {
// If deviation is too big - restart calibration
s->params.startTimeMs = millis();
s->params.sampleCount = 0;
for (int i = 0; i < 3; i++) {
s->val[i].accumulatedValue = 0;
devClear(&s->val[i].stdDev);
}
}
else {
// We are allowed to fail
s->params.state = ZERO_CALIBRATION_FAIL;
}
}
else {
// All seems ok - calculate average value
s->val[0].accumulatedValue = s->val[0].accumulatedValue / s->params.sampleCount;
s->val[1].accumulatedValue = s->val[1].accumulatedValue / s->params.sampleCount;
s->val[2].accumulatedValue = s->val[2].accumulatedValue / s->params.sampleCount;
s->params.state = ZERO_CALIBRATION_DONE;
}
}
}
void zeroCalibrationGetZeroV(zeroCalibrationVector_t * s, fpVector3_t * v)
{
if (s->params.state != ZERO_CALIBRATION_DONE) {
vectorZero(v);
}
else {
v->v[0] = s->val[0].accumulatedValue;
v->v[1] = s->val[1].accumulatedValue;
v->v[2] = s->val[2].accumulatedValue;
}
}

76
src/main/common/calibration.h Normal file
View file

@ -0,0 +1,76 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "common/maths.h"
#include "common/time.h"
#include "common/vector.h"
typedef enum {
ZERO_CALIBRATION_NONE,
ZERO_CALIBRATION_IN_PROGRESS,
ZERO_CALIBRATION_DONE,
ZERO_CALIBRATION_FAIL,
} zeroCalibrationState_e;
typedef struct {
zeroCalibrationState_e state;
timeMs_t startTimeMs;
timeMs_t windowSizeMs;
bool allowFailure;
unsigned sampleCount;
float stdDevThreshold;
} zeroCalibrationParams_t;
typedef struct {
float accumulatedValue;
stdev_t stdDev;
} zeroCalibrationValue_t;
typedef struct {
zeroCalibrationParams_t params;
zeroCalibrationValue_t val;
} zeroCalibrationScalar_t;
typedef struct {
zeroCalibrationParams_t params;
zeroCalibrationValue_t val[3];
} zeroCalibrationVector_t;
void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure);
bool zeroCalibrationIsCompleteS(zeroCalibrationScalar_t * s);
bool zeroCalibrationIsSuccessfulS(zeroCalibrationScalar_t * s);
void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v);
void zeroCalibrationGetZeroS(zeroCalibrationScalar_t * s, float * v);
void zeroCalibrationStartV(zeroCalibrationVector_t * s, timeMs_t window, float threshold, bool allowFailure);
bool zeroCalibrationIsCompleteV(zeroCalibrationVector_t * s);
bool zeroCalibrationIsSuccessfulV(zeroCalibrationVector_t * s);
void zeroCalibrationAddValueV(zeroCalibrationVector_t * s, const fpVector3_t * v);
void zeroCalibrationGetZeroV(zeroCalibrationVector_t * s, fpVector3_t * v);

3
src/main/config/parameter_group_ids.h
View file

@ -75,7 +75,8 @@
//#define PG_IBUS_TELEMETRY_CONFIG 53
#define PG_VTX_CONFIG 54
#define PG_ELERES_CONFIG 55
#define PG_CF_END 56
#define PG_TEMP_SENSOR_CONFIG 56
#define PG_CF_END 57
// Driver configuration
//#define PG_DRIVER_PWM_RX_CONFIG 100

31
src/main/drivers/1-wire.c Normal file
View file

@ -0,0 +1,31 @@
#include <string.h>
#include "drivers/1-wire.h"
#include "drivers/1-wire/ds2482.h"
#include "drivers/logging.h"
#ifdef USE_1WIRE
#ifdef USE_1WIRE_DS2482
static bool ds2482Detected = false;
static owDev_t ds2482Dev;
#endif
void owInit(void)
{
memset(&ds2482Dev, 0, sizeof(ds2482Dev));
addBootlogEvent2(BOOT_EVENT_TEMP_SENSOR_DETECTION, BOOT_EVENT_FLAGS_NONE);
#ifdef USE_1WIRE_DS2482
if (ds2482Detect(&ds2482Dev)) ds2482Detected = true;
#endif
}
owDev_t *getOwDev(void)
{
return ds2482Detected ? &ds2482Dev : NULL;
}
#endif /* USE_1WIRE */

75
src/main/drivers/1-wire.h Normal file
View file

@ -0,0 +1,75 @@
#pragma once
#include "drivers/bus.h"
#ifdef USE_1WIRE
#define OW_STATUS_1WB_POS 0 // 1-Wire busy
#define OW_STATUS_PPD_POS 1 // Presense-pulse detect
#define OW_STATUS_SD_POS 2 // Short detected
#define OW_STATUS_LL_POS 3 // Logic level
#define OW_STATUS_RST_POS 4 // Device reset
#define OW_STATUS_SBR_POS 5 // Single bit result
#define OW_STATUS_TSB_POS 6 // Triplet second bit
#define OW_STATUS_DIR_POS 7 // Branch direction taken
#define OW_BUS_BUSY(status) (status & (1 << OW_STATUS_1WB_POS))
#define OW_DEVICE_PRESENT(status) (status & (1 << OW_STATUS_PPD_POS)) // True if a device have been detected on the bus after a bus reset
#define OW_RESET(status) (status & (1 << OW_STATUS_RST_POS))
#define OW_LOGIC_LEVEL(status) (status & (1 << OW_STATUS_LL_POS))
#define OW_SHORT_DETECTED(status) (status & (1 << OW_STATUS_SD_POS))
#define OW_SBR_VALUE(status) ((status >> OW_STATUS_SBR_POS) & 1) // Extract single bit read value or triplet first bit from status register value
#define OW_TSB_VALUE(status) ((status >> OW_STATUS_TSB_POS) & 1) // Extract triplet second bit value from status register value
#define OW_DIR_VALUE(status) ((status >> OW_STATUS_DIR_POS) & 1) // Extract triplet chosen direction bit value from status register value
#define OW_TRIPLET_FIRST_BIT(tripletResult) (tripletResult & (1 << 0))
#define OW_TRIPLET_SECOND_BIT(tripletResult) (tripletResult & (1 << 1))
#define OW_TRIPLET_DIRECTION_BIT(tripletResult) (tripletResult & (1 << 2))
#define OW_SINGLE_BIT_WRITE0 0
#define OW_SINGLE_BIT_WRITE1_READ (1<<7)
typedef struct owDev_s {
busDevice_t *busDev;
uint8_t status;
bool (*reset)(struct owDev_s *owDev);
bool (*owResetCommand)(struct owDev_s *owDev);
bool (*owReset)(struct owDev_s *owDev);
bool (*waitForBus)(struct owDev_s *owDev);
bool (*readConfig)(struct owDev_s *owDev, uint8_t *config);
bool (*writeConfig)(struct owDev_s *owDev, uint8_t config);
bool (*readStatus)(struct owDev_s *owDev, uint8_t *status);
uint8_t (*getStatus)(struct owDev_s *owDev);
bool (*poll)(struct owDev_s *owDev, bool waitForBus, uint8_t *status);
bool (*owBusReady)(struct owDev_s *owDev);
// 1-Wire ROM
bool (*owSearchRom)(struct owDev_s *owDev, uint8_t familyCode, uint64_t *romTable, uint8_t *romTableLen);
bool (*owMatchRomCommand)(struct owDev_s *owDev);
bool (*owMatchRom)(struct owDev_s *owDev, uint64_t rom);
bool (*owSkipRomCommand)(struct owDev_s *owDev);
bool (*owSkipRom)(struct owDev_s *owDev);
// 1-Wire read/write
bool (*owWriteByteCommand)(struct owDev_s *owDev, uint8_t byte);
bool (*owWriteByte)(struct owDev_s *owDev, uint8_t byte);
bool (*owWriteBuf)(struct owDev_s *owDev, const uint8_t *buf, uint8_t len);
bool (*owReadByteCommand)(struct owDev_s *owDev);
bool (*owReadByteResult)(struct owDev_s *owDev, uint8_t *result);
bool (*owReadByte)(struct owDev_s *owDev, uint8_t *result);
bool (*owReadBuf)(struct owDev_s *owDev, uint8_t *buf, uint8_t len);
bool (*owSingleBitCommand)(struct owDev_s *owDev, uint8_t type);
bool (*owSingleBitResult)(struct owDev_s *owDev);
bool (*owSingleBit)(struct owDev_s *owDev, uint8_t type, bool *result);
bool (*owTripletCommand)(struct owDev_s *owDev, uint8_t direction);
uint8_t (*owTripletResult)(struct owDev_s *owDev);
bool (*owTriplet)(struct owDev_s *owDev, uint8_t direction, uint8_t *result);
} owDev_t;
void owInit(void);
owDev_t *getOwDev(void);
#endif /* defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) */

433
src/main/drivers/1-wire/ds2482.c Normal file
View file

@ -0,0 +1,433 @@
/*
* 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"
#include "build/debug.h"
#include "drivers/1-wire.h"
#include "drivers/1-wire/ds_crc.h"
#include "drivers/1-wire/ds2482.h"
#include "drivers/time.h"
#define DS2482_STATUS_REG_ADDR 0xF0
#define DS2482_READ_DATA_REG_ADDR 0xE1
#define DS2482_CONFIG_REG_ADDR 0xC3
#define DS2482_CONFIG_REG_APU (1<<0) // Active pull-up
#define DS2482_CONFIG_REG_SPU (1<<2) // Strong pull-up
#define DS2482_CONFIG_REG_WS (1<<3) // 1-Wire speed
#define DS2482_STATUS_REG_1WB_POS 0 // 1-Wire busy
#define DS2482_STATUS_REG_PPD_POS 1 // Presense-pulse detect
#define DS2482_STATUS_REG_SD_POS 2 // Short detected
#define DS2482_STATUS_REG_LL_POS 3 // Logic level
#define DS2482_STATUS_REG_RST_POS 4 // Device reset
#define DS2482_STATUS_REG_SBR_POS 5 // Single bit result
#define DS2482_STATUS_REG_TSB_POS 6 // Triplet second bit
#define DS2482_STATUS_REG_DIR_POS 7 // Branch direction taken
#define DS2482_1WIRE_BUSY(status) (status & (1 << DS2482_STATUS_REG_1WB_POS))
#define DS2482_DEVICE_PRESENT(status) (status & (1 << DS2482_STATUS_REG_PPD_POS)) // True if a device have been detected on the bus after a bus reset
#define DS2482_RESET(status) (status & (1 << DS2482_STATUS_REG_RST_POS))
#define DS2482_LOGIC_LEVEL(status) (status & (1 << DS2482_STATUS_REG_LL_POS))
#define DS2482_SHORT_DETECTED(status) (status & (1 << DS2482_STATUS_REG_SD_POS))
#define DS2482_SBR_VALUE(status) ((status >> DS2482_STATUS_REG_SBR_POS) & 1) // Extract single bit read value or triplet first bit from status register value
#define DS2482_TSB_VALUE(status) ((status >> DS2482_STATUS_REG_TSB_POS) & 1) // Extract triplet second bit value from status register value
#define DS2482_DIR_VALUE(status) ((status >> DS2482_STATUS_REG_DIR_POS) & 1) // Extract triplet chosen direction bit value from status register value
#define DS2482_1WIRE_SINGLE_BIT_WRITE0 0
#define DS2482_1WIRE_SINGLE_BIT_WRITE1_READ (1<<7)
#define DS2482_CONFIG_WRITE_BYTE(config) (config | ((~config & 0xF) << 4)) // Config's upper nibble should be the one's complement of lower nibble when writing
#define DS2482_RESET_CMD 0xF0
#define DS2482_SET_READ_PTR_CMD 0xE1
#define DS2482_WRITE_CONFIG_CMD 0xD2
#define DS2482_1WIRE_RESET_CMD 0xB4
#define DS2482_1WIRE_SINGLE_BIT_CMD 0x87
#define DS2482_1WIRE_WRITE_BYTE_CMD 0xA5
#define DS2482_1WIRE_READ_BYTE_CMD 0x96
#define DS2482_1WIRE_TRIPLET_CMD 0x78
#define _1WIRE_SEARCH_ROM_CMD 0xF0
#define _1WIRE_READ_ROM_CMD 0x33
#define _1WIRE_MATCH_ROM_CMD 0x55
#define _1WIRE_SKIP_ROM_CMD 0xCC
#if defined(USE_1WIRE) && defined(USE_1WIRE_DS2482)
static bool ds2482Reset(owDev_t *owDev)
{
return busWrite(owDev->busDev, 0xFF, DS2482_RESET_CMD);
}
static bool ds2482SetReadPtr(owDev_t *owDev, uint8_t reg)
{
return busWrite(owDev->busDev, DS2482_SET_READ_PTR_CMD, reg);
}
static bool ds2482ReadReg(owDev_t *owDev, uint8_t reg, uint8_t *byte)
{
bool ack = ds2482SetReadPtr(owDev, reg);
if (!ack) return false;
return busRead(owDev->busDev, 0xFF, byte);
}
static bool ds2482ReadByte(owDev_t *owDev, uint8_t *byte)
{
return ds2482ReadReg(owDev, DS2482_READ_DATA_REG_ADDR, byte);
}
static bool ds2482ReadConfig(owDev_t *owDev, uint8_t *config)
{
return ds2482ReadReg(owDev, DS2482_CONFIG_REG_ADDR, config);
}
static bool ds2482WriteConfig(owDev_t *owDev, uint8_t config)
{
return busWrite(owDev->busDev, DS2482_WRITE_CONFIG_CMD, DS2482_CONFIG_WRITE_BYTE(config));
}
static bool ds2482ReadStatus(owDev_t *owDev, uint8_t *status)
{
bool ack = ds2482ReadReg(owDev, DS2482_STATUS_REG_ADDR, &owDev->status);
if (!ack) return false;
*status = owDev->status;
return true;
}
static uint8_t ds2482GetStatus(owDev_t *owDev)
{
return owDev->status;
}
static bool ds2482Poll(owDev_t *owDev, bool waitForBus, uint8_t *status)
{
do {
bool ack = busRead(owDev->busDev, 0xFF, &owDev->status);
if (!ack) return false;
} while (waitForBus && DS2482_1WIRE_BUSY(owDev->status));
if (status) *status = owDev->status;
return true;
}
static bool ds2482WaitForBus(owDev_t *owDev)
{
return ds2482Poll(owDev, true, NULL);
}
static bool ds2482OwBusReady(owDev_t *owDev)
{
bool ack = busRead(owDev->busDev, 0xFF, &owDev->status);
if (!ack) return false;
return !DS2482_1WIRE_BUSY(owDev->status);
}
static bool ds2482OwResetCommand(owDev_t *owDev)
{
return busWrite(owDev->busDev, 0xFF, DS2482_1WIRE_RESET_CMD);
}
static bool ds2482OwReset(owDev_t *owDev)
{
bool ack = ds2482OwResetCommand(owDev);
if (!ack) return false;
return ds2482WaitForBus(owDev);
}
static bool ds2482OwWriteByteCommand(owDev_t *owDev, uint8_t byte)
{
return busWrite(owDev->busDev, DS2482_1WIRE_WRITE_BYTE_CMD, byte);
}
static bool ds2482OwWriteByte(owDev_t *owDev, uint8_t byte)
{
bool ack = ds2482OwWriteByteCommand(owDev, byte);
if (!ack) return false;
return ds2482WaitForBus(owDev);
}
static bool ds2482OwWriteBuf(owDev_t *owDev, const uint8_t *buf, uint8_t len)
{
for (uint8_t index = 0; index < len; ++index) {
bool ack = ds2482OwWriteByte(owDev, buf[index]);
if (!ack) return false;
}
return true;
}
static bool ds2482OwReadByteCommand(owDev_t *owDev)
{
return busWrite(owDev->busDev, 0xFF, DS2482_1WIRE_READ_BYTE_CMD);
}
static bool ds2482OwReadByte(owDev_t *owDev, uint8_t *result)
{
bool ack = ds2482OwReadByteCommand(owDev);
if (!ack) return false;
ack = ds2482WaitForBus(owDev);
if (!ack) return false;
return ds2482ReadByte(owDev, result);
}
static bool ds2482OwReadBuf(owDev_t *owDev, uint8_t *buf, uint8_t len)
{
for (uint8_t index = 0; index < len; ++index) {
bool ack = ds2482OwReadByte(owDev, buf + index);
if (!ack) return false;
}
return true;
}
static bool ds2482OwSingleBitCommand(owDev_t *owDev, uint8_t type)
{
return busWrite(owDev->busDev, DS2482_1WIRE_SINGLE_BIT_CMD, type);
}
static bool ds2482OwSingleBitResult(owDev_t *owDev)
{
return DS2482_SBR_VALUE(owDev->status);
}
static bool ds2482OwSingleBit(owDev_t *owDev, uint8_t type, bool *result)
{
bool ack = ds2482OwSingleBitCommand(owDev, type);
ack = ds2482WaitForBus(owDev);
if (!ack) return false;
if (result) *result = ds2482OwSingleBitResult(owDev);
return true;
}
static bool ds2482OwTripletCommand(owDev_t *owDev, uint8_t direction)
{
return busWrite(owDev->busDev, DS2482_1WIRE_TRIPLET_CMD, direction << 7);
}
static uint8_t ds2482OwTripletResult(owDev_t *owDev)
{
return owDev->status >> 5;
}
static bool ds2482OwTriplet(owDev_t *owDev, uint8_t direction, uint8_t *result)
{
bool ack = ds2482OwTripletCommand(owDev, direction << 7);
if (!ack) return false;
ack = ds2482Poll(owDev, true, NULL);
if (!ack) return false;
*result = ds2482OwTripletResult(owDev);
return true;
}
static bool ds2482OwSkipRomCommand(owDev_t *owDev)
{
return ds2482OwWriteByte(owDev, _1WIRE_SKIP_ROM_CMD);
}
static bool ds2482OwSkipRom(owDev_t *owDev)
{
bool ack = ds2482OwReset(owDev);
if (!ack) return false;
ack = ds2482OwSkipRomCommand(owDev);
if (!ack) return false;
return ds2482WaitForBus(owDev);
}
static bool ds2482OwMatchRomCommand(owDev_t *owDev)
{
return ds2482OwWriteByteCommand(owDev, _1WIRE_MATCH_ROM_CMD);
}
static bool ds2482OwMatchRom(owDev_t *owDev, uint64_t rom)
{
bool ack = ds2482OwReset(owDev);
if (!ack) return false;
ack = ds2482OwMatchRomCommand(owDev);
if (!ack) return false;
ack = ds2482WaitForBus(owDev);
if (!ack) return false;
for (uint8_t romByteIndex = 0; romByteIndex < 8; ++romByteIndex) {
ack = ds2482OwWriteByte(owDev, ((uint8_t *)&rom)[romByteIndex]);
if (!ack) return false;
}
return true;
}
static bool ds2482OwSearchRom(owDev_t *owDev, uint8_t family_code, uint64_t *rom_table, uint8_t *rom_table_len)
{
bool ack;
uint8_t last_collision_index = 0, rom_index = 0;
do {
uint8_t rom_byte_index = 0, rom_bit_index = 1, rom_byte_mask = 1;
uint8_t dir_zero_last_index = 0; // Bit index where the 0 direction has been chosen after collision
uint8_t *rom = (uint8_t *)&rom_table[rom_index];
ack = ds2482OwReset(owDev);
if (!ack) goto ds2482SearchRomReturn;
ack = ds2482Poll(owDev, true, NULL);
if (!ack) goto ds2482SearchRomReturn;
if (!DS2482_DEVICE_PRESENT(owDev->status))
goto ds2482SearchRomReturn;
ack = ds2482OwWriteByte(owDev, _1WIRE_SEARCH_ROM_CMD);
if (!ack) goto ds2482SearchRomReturn;
do {
uint8_t direction;
if (family_code && (rom_bit_index < 9)) {
direction = (family_code >> (rom_bit_index - 1)) & 1;
} else if ((rom_index > 0) && (rom_bit_index < last_collision_index)) {
const uint8_t *previous_rom = (uint8_t *)&rom_table[rom_index-1];
direction = (previous_rom[rom_byte_index] & rom_byte_mask) > 0;
} else {
direction = rom_bit_index == last_collision_index;
}
ack = ds2482OwTripletCommand(owDev, direction);
if (!ack) goto ds2482SearchRomReturn;
ack = ds2482Poll(owDev, true, NULL);
if (!ack) goto ds2482SearchRomReturn;
uint8_t triplet_sbr = DS2482_SBR_VALUE(owDev->status);
uint8_t triplet_tsb = DS2482_TSB_VALUE(owDev->status);
uint8_t triplet_dir = DS2482_DIR_VALUE(owDev->status);
if (triplet_sbr && triplet_tsb) break; // Error, the device have been disconnected during the search, restart
if (family_code && (rom_bit_index < 9) && (triplet_dir != direction))
goto ds2482SearchRomReturn;
if (triplet_dir)
rom[rom_byte_index] |= rom_byte_mask;
else {
if (!(triplet_sbr || triplet_tsb)) dir_zero_last_index = rom_bit_index; // Collision
rom[rom_byte_index] &= ~rom_byte_mask;
}
rom_bit_index += 1;
if (!(rom_byte_mask <<= 1)) {
rom_byte_index += 1;
rom_byte_mask = 1;
}
} while (rom_byte_index < 8);
if ((rom_bit_index > 64) && (rom[7] == ds_crc8(rom, 7))) {
rom_index += 1;
last_collision_index = dir_zero_last_index;
if (!last_collision_index) break; // All the devices have been found
}
} while (rom_index < *rom_table_len);
ds2482SearchRomReturn:
*rom_table_len = rom_index;
return ack;
}
static bool ds2482Init(owDev_t *owDev)
{
return ds2482WriteConfig(owDev, DS2482_CONFIG_REG_APU);
}
#define DETECTION_MAX_RETRY_COUNT 5
static bool deviceDetect(owDev_t *owDev)
{
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
delay(10);
if (ds2482Reset(owDev)) return true;
}
return false;
}
bool ds2482Detect(owDev_t *owDev)
{
owDev->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_DS2482, 0, OWNER_1WIRE);
if (owDev->busDev == NULL) {
return false;
}
if (!deviceDetect(owDev)) {
busDeviceDeInit(owDev->busDev);
return false;
}
ds2482Init(owDev);
owDev->reset = ds2482Reset;
owDev->owResetCommand = ds2482OwResetCommand;
owDev->owReset = ds2482OwReset;
owDev->waitForBus = ds2482WaitForBus;
owDev->readConfig = ds2482ReadConfig;
owDev->writeConfig = ds2482WriteConfig;
owDev->readStatus = ds2482ReadStatus;
owDev->getStatus = ds2482GetStatus;
owDev->poll = ds2482Poll;
owDev->owBusReady = ds2482OwBusReady;
owDev->owSearchRom = ds2482OwSearchRom;
owDev->owMatchRomCommand = ds2482OwMatchRomCommand;
owDev->owMatchRom = ds2482OwMatchRom;
owDev->owSkipRomCommand = ds2482OwSkipRomCommand;
owDev->owSkipRom = ds2482OwSkipRom;
owDev->owWriteByteCommand = ds2482OwWriteByteCommand;
owDev->owWriteByte = ds2482OwWriteByte;
owDev->owWriteBuf = ds2482OwWriteBuf;
owDev->owReadByteCommand = ds2482OwReadByteCommand;
owDev->owReadByteResult = ds2482ReadByte;
owDev->owReadByte = ds2482OwReadByte;
owDev->owReadBuf = ds2482OwReadBuf;
owDev->owSingleBitCommand = ds2482OwSingleBitCommand;
owDev->owSingleBitResult = ds2482OwSingleBitResult;
owDev->owSingleBit = ds2482OwSingleBit;
owDev->owTripletCommand = ds2482OwTripletCommand;
owDev->owTripletResult = ds2482OwTripletResult;
owDev->owTriplet = ds2482OwTriplet;
return true;
}
#endif /* defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) */

28
src/main/drivers/1-wire/ds2482.h Normal file
View file

@ -0,0 +1,28 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
//#include "drivers/io_types.h"
#include <stdbool.h>
#include "drivers/1-wire.h"
#if defined(USE_1WIRE) && defined(USE_1WIRE_DS2482)
bool ds2482Detect(owDev_t *owDev);
#endif /* defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) */

28
src/main/drivers/1-wire/ds_crc.c Normal file
View file

@ -0,0 +1,28 @@
#include <stdint.h>
uint8_t ds_crc8_bit_update(uint8_t crc8, uint8_t bit)
{
uint8_t crc8_base = (crc8 >> 1) & 0x73;
uint8_t xor1 = (crc8 & 1) ^ bit;
return crc8_base | ((((crc8 >> 3) & 1) ^ xor1) << 2) | ((((crc8 >> 4) & 1) ^ xor1) << 3) | (xor1 << 7);
}
uint8_t ds_crc8_byte_update(uint8_t crc8, uint8_t byte)
{
for (uint8_t i = 0; i < 8; ++i) {
uint8_t mix = (crc8 ^ byte) & 0x01;
crc8 >>= 1;
if (mix) crc8 ^= 0x8C;
byte >>= 1;
}
return crc8;
}
uint8_t ds_crc8(const uint8_t *addr, uint8_t len)
{
uint8_t crc8 = 0;
for (uint8_t i=0; i < len; ++i) crc8 = ds_crc8_byte_update(crc8, addr[i]);
return crc8;
}

8
src/main/drivers/1-wire/ds_crc.h Normal file
View file

@ -0,0 +1,8 @@
#pragma once
#include <stdint.h>
uint8_t ds_crc8_bit_update(uint8_t crc, uint8_t bit);
uint8_t ds_crc8_byte_update(uint8_t crc, uint8_t byte);
uint8_t ds_crc8(const uint8_t *addr, uint8_t len);

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

@ -114,7 +114,17 @@ typedef enum {
DEVHW_LIS3MDL,
/* Temp sensor chips */
DEVHW_LM75,
DEVHW_LM75_0,
DEVHW_LM75_1,
DEVHW_LM75_2,
DEVHW_LM75_3,
DEVHW_LM75_4,
DEVHW_LM75_5,
DEVHW_LM75_6,
DEVHW_LM75_7,
/* 1-wire interface chips */
DEVHW_DS2482,
/* OSD chips */
DEVHW_MAX7456,

1
src/main/drivers/logging.c
View file

@ -57,6 +57,7 @@ static const char * eventDescription[BOOT_EVENT_CODE_COUNT] = {
[BOOT_EVENT_TIMER_CH_MAPPED] = "TIMER_CHANNEL_MAPPED",
[BOOT_EVENT_PITOT_DETECTION] = "PITOT_DETECTION",
[BOOT_EVENT_TEMP_SENSOR_DETECTION] = "TEMP_SENSOR_DETECTION",
[BOOT_EVENT_1WIRE_DETECTION] = "1WIRE_DETECTION",
[BOOT_EVENT_HARDWARE_IO_CONFLICT] = "HARDWARE_CONFLICT",
[BOOT_EVENT_OPFLOW_DETECTION] = "OPFLOW_DETECTION",
};

5
src/main/drivers/logging_codes.h
View file

@ -49,8 +49,9 @@ typedef enum {
BOOT_EVENT_TIMER_CH_MAPPED = 19, // 0 - PPM, 1 - PWM, 2 - MOTOR, 3 - SERVO
BOOT_EVENT_PITOT_DETECTION = 20,
BOOT_EVENT_TEMP_SENSOR_DETECTION = 21,
BOOT_EVENT_HARDWARE_IO_CONFLICT = 22, // Hardware IO resource conflict, parameters: #1 - current owner, #2 - requested owner
BOOT_EVENT_OPFLOW_DETECTION = 23,
BOOT_EVENT_1WIRE_DETECTION = 22,
BOOT_EVENT_HARDWARE_IO_CONFLICT = 23, // Hardware IO resource conflict, parameters: #1 - current owner, #2 - requested owner
BOOT_EVENT_OPFLOW_DETECTION = 24,
BOOT_EVENT_CODE_COUNT
} bootLogEventCode_e;

2
src/main/drivers/max7456.c
View file

@ -290,7 +290,7 @@ uint16_t max7456GetScreenSize(void)
// deal with a zero returned from here.
// TODO: Inspect all callers, make sure they can handle zero and
// change this function to return zero before initialization.
if (state.isInitialized && (state.registers.vm0 & VIDEO_LINES_NTSC)) {
if (state.isInitialized && ((state.registers.vm0 & VIDEO_MODE_PAL) == 0)) {
return VIDEO_BUFFER_CHARS_NTSC;
}
return VIDEO_BUFFER_CHARS_PAL;

2
src/main/drivers/max7456_symbols.h
View file

@ -102,7 +102,7 @@
#define SYM_DIRECTION 0x72 // 114 to 121, directional little arrows
// 0x7A // 122 -
#define SYM_HOME_NEAR 0x7A // 122 Home, near
// 0x7B // 123 to 125 ASCII

3
src/main/drivers/rangefinder/rangefinder_vl53l0x.c
View file

@ -805,6 +805,9 @@ static void vl53l0x_Init(rangefinderDev_t * rangefinder)
uint8_t byte;
isInitialized = false;
// During init set timeout to a higher value
setTimeout(500);
// VL53L0X_DataInit() begin
// Switch sensor to 2.8V mode
byte = readReg(rangefinder->busDev, VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV);

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

@ -52,6 +52,7 @@ typedef enum {
OWNER_SPI_PREINIT,
OWNER_COMPASS,
OWNER_TEMPERATURE,
OWNER_1WIRE,
OWNER_AIRSPEED,
OWNER_OLED_DISPLAY,
OWNER_PINIO,

148
src/main/drivers/temperature/ds18b20.c Normal file
View file

@ -0,0 +1,148 @@
/*
* 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"
#include "build/debug.h"
#include "drivers/1-wire.h"
#include "drivers/1-wire/ds2482.h"
#include "drivers/1-wire/ds_crc.h"
#include "drivers/temperature/ds18b20.h"
#define DS18B20_FAMILY_CODE 0x28
#define DS18B20_ALARM_SEARCH_CMD 0xEC
#define DS18B20_START_CONVERSION_CMD 0x44
#define DS18B20_WRITE_SCRATCHPAD_CMD 0x4E
#define DS18B20_READ_SCRATCHPAD_CMD 0xBE
#define DS18B20_COPY_SCRATCHPAD_CMD 0x48
#define DS18B20_RECALL_EEPROM_CMD 0xB8
#define DS18B20_READ_POWER_SUPPLY_CMD 0xB4
#if defined(USE_1WIRE) && defined(USE_TEMPERATURE_DS18B20)
bool ds18b20Enumerate(owDev_t *owDev, uint64_t *rom_table, uint8_t *rom_table_len)
{
return owDev->owSearchRom(owDev, DS18B20_FAMILY_CODE, rom_table, rom_table_len);
}
bool ds18b20Configure(owDev_t *owDev, uint64_t rom, uint8_t config)
{
bool ack = owDev->owMatchRom(owDev, rom);
if (!ack) return false;
uint8_t buf[4] = { DS18B20_WRITE_SCRATCHPAD_CMD, 0, 0, config };
return owDev->owWriteBuf(owDev, buf, sizeof(buf));
}
static bool readPowerSupply(owDev_t *owDev, bool *result)
{
bool ack = owDev->owWriteByte(owDev, DS18B20_READ_POWER_SUPPLY_CMD);
if (!ack) return false;
bool sbr;
ack = owDev->owSingleBit(owDev, OW_SINGLE_BIT_WRITE1_READ, &sbr);
if (!ack) return false;
*result = !sbr;
return true;
}
bool ds18b20ParasiticPoweredPresent(owDev_t *owDev, bool *result)
{
bool ack = owDev->owSkipRom(owDev);
if (!ack) return false;
return readPowerSupply(owDev, result);
}
bool ds18b20ReadPowerSupply(owDev_t *owDev, uint64_t rom, bool *parasiticPowered)
{
bool ack = owDev->owMatchRom(owDev, rom);
if (!ack) return false;
return readPowerSupply(owDev, parasiticPowered);
}
bool ds18b20ReadScratchpadCommand(owDev_t *owDev)
{
return owDev->owWriteByteCommand(owDev, DS18B20_READ_SCRATCHPAD_CMD);
}
static bool ds18b20ReadScratchpadBuf(owDev_t *owDev, uint8_t *buf, uint8_t len)
{
bool ack = ds18b20ReadScratchpadCommand(owDev);
if (!ack) return false;
ack = owDev->waitForBus(owDev);
if (!ack) return false;
return owDev->owReadBuf(owDev, buf, len);
}
bool ds18b20StartConversionCommand(owDev_t *owDev)
{
return owDev->owWriteByteCommand(owDev, DS18B20_START_CONVERSION_CMD);
}
// start conversion on all devices present on the bus
// note: parasitic power only supports one device converting at a time
bool ds18b20StartConversion(owDev_t *owDev)
{
bool ack = owDev->owSkipRom(owDev);
if (!ack) return false;
return ds18b20StartConversionCommand(owDev);
}
bool ds18b20WaitForConversion(owDev_t *owDev)
{
bool ack = owDev->waitForBus(owDev);
if (!ack) return false;
bool read_bit;
do {
ack = owDev->owSingleBit(owDev, OW_SINGLE_BIT_WRITE1_READ, &read_bit);
if (!ack) return false;
} while (!read_bit);
return true;
}
bool ds18b20ReadTemperatureFromScratchPadBuf(const uint8_t *buf, int16_t *temperature)
{
if (buf[8] != ds_crc8(buf, 8)) return false;
*temperature = (int16_t)(((buf[0] | (buf[1] << 8)) >> 3) | ((buf[1] & 0x80) ? 0xE000 : 0)) * 5;
return true;
}
bool ds18b20ReadTemperature(owDev_t *owDev, uint64_t rom, int16_t *temperature)
{
bool ack = owDev->owMatchRom(owDev, rom);
if (!ack) return false;
uint8_t buf[9];
ack = ds18b20ReadScratchpadBuf(owDev, buf, 9);
if (!ack) return false;
return ds18b20ReadTemperatureFromScratchPadBuf(buf, temperature);
}
#endif /* defined(USE_1WIRE) && defined(USE_TEMPERATURE_DS18B20) */

36
src/main/drivers/temperature/ds18b20.h Normal file
View file

@ -0,0 +1,36 @@
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include "drivers/1-wire.h"
#if defined(USE_1WIRE) && defined(USE_TEMPERATURE_DS18B20)
#define USE_TEMPERATURE_SENSOR
#define DS18B20_DRIVER_AVAILABLE
#define DS18B20_CONFIG_9BIT 0x1F
#define DS18B20_CONFIG_10BIT 0x3F
#define DS18B20_CONFIG_11BIT 0x5F
#define DS18B20_CONFIG_12BIT 0x7F
// milliseconds
#define DS18B20_9BIT_CONVERSION_TIME 94
#define DS18B20_10BIT_CONVERSION_TIME 188
#define DS18B20_11BIT_CONVERSION_TIME 375
#define DS18B20_12BIT_CONVERSION_TIME 750
bool ds18b20Enumerate(owDev_t *owDev, uint64_t *rom_table, uint8_t *rom_table_len);
bool ds18b20Configure(owDev_t *owDev, uint64_t rom, uint8_t config);
bool ds18b20ParasiticPoweredPresent(owDev_t *owDev, bool *result);
bool ds18b20ReadPowerSupply(owDev_t *owDev, uint64_t rom, bool *parasiticPowered);
bool ds18b20StartConversionCommand(owDev_t *owDev);
bool ds18b20StartConversion(owDev_t *owDev);
bool ds18b20WaitForConversion(owDev_t *owDev);
bool ds18b20ReadScratchpadCommand(owDev_t *owDev);
bool ds18b20ReadTemperatureFromScratchPadBuf(const uint8_t *buf, int16_t *temperature);
bool ds18b20ReadTemperature(owDev_t *owDev, uint64_t rom, int16_t *temperature);
#endif /* defined(USE_1WIRE) && defined(USE_TEMPERATURE_DS18B20) */

8
src/main/drivers/temperature/lm75.c
View file

@ -48,7 +48,7 @@ static bool lm75Read(temperatureDev_t *tempDev, int16_t *temperature)
return false;
}
#define DETECTION_MAX_RETRY_COUNT 20
#define DETECTION_MAX_RETRY_COUNT 5
static bool deviceDetect(temperatureDev_t *tempDev)
{
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
@ -59,9 +59,11 @@ static bool deviceDetect(temperatureDev_t *tempDev)
return false;
}
bool lm75Detect(temperatureDev_t *tempDev)
bool lm75Detect(temperatureDev_t *tempDev, uint8_t partialAddress)
{
tempDev->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_LM75, 0, OWNER_TEMPERATURE);
if (partialAddress > 7) return false; // invalid address
tempDev->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_LM75_0 + partialAddress, 0, OWNER_TEMPERATURE);
if (tempDev->busDev == NULL) {
return false;
}

8
src/main/drivers/temperature/lm75.h
View file

@ -20,4 +20,10 @@
#include "drivers/io_types.h"
#include "drivers/temperature/temperature.h"
bool lm75Detect(temperatureDev_t * mag);
#ifdef USE_TEMPERATURE_LM75
#define USE_TEMPERATURE_SENSOR
bool lm75Detect(temperatureDev_t *tempDev, uint8_t partial_address);
#endif /* USE_TEMPERATURE_LM75 */

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

@ -108,6 +108,7 @@ extern uint8_t __config_end;
#include "sensors/rangefinder.h"
#include "sensors/opflow.h"
#include "sensors/sensors.h"
#include "sensors/temperature.h"
#include "telemetry/frsky_d.h"
#include "telemetry/telemetry.h"
@ -1126,6 +1127,107 @@ static void cliRxRange(char *cmdline)
}
}
#ifdef USE_TEMPERATURE_SENSOR
static void printTempSensor(uint8_t dumpMask, const tempSensorConfig_t *tempSensorConfigs, const tempSensorConfig_t *defaultTempSensorConfigs)
{
const char *format = "temp_sensor %u %u %s %d %d %s";
for (uint8_t i = 0; i < MAX_TEMP_SENSORS; i++) {
bool equalsDefault = false;
char label[5], hex_address[17];
strncpy(label, tempSensorConfigs[i].label, TEMPERATURE_LABEL_LEN);
label[4] = '\0';
tempSensorAddressToString(tempSensorConfigs[i].address, hex_address);
if (defaultTempSensorConfigs) {
equalsDefault = tempSensorConfigs[i].type == defaultTempSensorConfigs[i].type
&& tempSensorConfigs[i].address == defaultTempSensorConfigs[i].address
&& !memcmp(tempSensorConfigs[i].label, defaultTempSensorConfigs[i].label, TEMPERATURE_LABEL_LEN)
&& tempSensorConfigs[i].alarm_min == defaultTempSensorConfigs[i].alarm_min
&& tempSensorConfigs[i].alarm_max == defaultTempSensorConfigs[i].alarm_max;
cliDefaultPrintLinef(dumpMask, equalsDefault, format,
i,
defaultTempSensorConfigs[i].type,
"0",
defaultTempSensorConfigs[i].alarm_min,
defaultTempSensorConfigs[i].alarm_max,
""
);
}
cliDumpPrintLinef(dumpMask, equalsDefault, format,
i,
tempSensorConfigs[i].type,
hex_address,
tempSensorConfigs[i].alarm_min,
tempSensorConfigs[i].alarm_max,
label
);
}
}
static void cliTempSensor(char *cmdline)
{
if (isEmpty(cmdline)) {
printTempSensor(DUMP_MASTER, tempSensorConfig(0), NULL);
} else if (sl_strcasecmp(cmdline, "reset") == 0) {
resetTempSensorConfig();
} else {
int16_t i;
const char *ptr = cmdline, *label;
int16_t type, alarm_min, alarm_max;
bool addressValid = false;
uint64_t address;
uint8_t validArgumentCount = 0;
i = fastA2I(ptr);
if (i >= 0 && i < MAX_TEMP_SENSORS) {
ptr = nextArg(ptr);
if (ptr) {
type = fastA2I(ptr);
validArgumentCount++;
}
ptr = nextArg(ptr);
if (ptr) {
addressValid = tempSensorStringToAddress(ptr, &address);
validArgumentCount++;
}
ptr = nextArg(ptr);
if (ptr) {
alarm_min = fastA2I(ptr);
validArgumentCount++;
}
ptr = nextArg(ptr);
if (ptr) {
alarm_max = fastA2I(ptr);
validArgumentCount++;
}
label = nextArg(ptr);
if (label)
++validArgumentCount;
else
label = "";
if (validArgumentCount < 4) {
cliShowParseError();
} else if (type < 0 || type > TEMP_SENSOR_DS18B20 || alarm_min < -550 || alarm_min > 1250 || alarm_max < -550 || alarm_max > 1250 || strlen(label) > 4 || !addressValid) {
cliShowParseError();
} else {
tempSensorConfig_t *sensorConfig = tempSensorConfigMutable(i);
sensorConfig->type = type;
sensorConfig->address = address;
sensorConfig->alarm_min = alarm_min;
sensorConfig->alarm_max = alarm_max;
strncpy(sensorConfig->label, label, TEMPERATURE_LABEL_LEN);
}
} else {
cliShowArgumentRangeError("sensor index", 0, MAX_TEMP_SENSORS - 1);
}
}
}
#endif
#ifdef USE_LED_STRIP
static void printLed(uint8_t dumpMask, const ledConfig_t *ledConfigs, const ledConfig_t *defaultLedConfigs)
{
@ -1428,7 +1530,7 @@ static void cliServoMix(char *cmdline)
if (i >= 0 && i < MAX_SERVO_RULES &&
args[TARGET] >= 0 && args[TARGET] < MAX_SUPPORTED_SERVOS &&
args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT &&
args[RATE] >= -125 && args[RATE] <= 125 &&
args[RATE] >= -1000 && args[RATE] <= 1000 &&
args[SPEED] >= 0 && args[SPEED] <= MAX_SERVO_SPEED) {
customServoMixersMutable(i)->targetChannel = args[TARGET];
customServoMixersMutable(i)->inputSource = args[INPUT];
@ -2326,7 +2428,7 @@ static void cliStatus(char *cmdline)
rtcGetDateTime(&dt);
dateTimeFormatLocal(buf, &dt);
cliPrintLinef("Current Time: %s", buf);
cliPrintLinef("Voltage: %d.%dV (%dS battery - %s)", getBatteryVoltage() / 100, getBatteryVoltage() % 100, getBatteryCellCount(), getBatteryStateString());
cliPrintLinef("Voltage: %d.%02dV (%dS battery - %s)", getBatteryVoltage() / 100, getBatteryVoltage() % 100, getBatteryCellCount(), getBatteryStateString());
cliPrintf("CPU Clock=%dMHz", (SystemCoreClock / 1000000));
const uint32_t detectedSensorsMask = sensorsMask();
@ -2638,6 +2740,11 @@ static void printConfig(const char *cmdline, bool doDiff)
cliPrintHashLine("rxrange");
printRxRange(dumpMask, rxChannelRangeConfigs_CopyArray, rxChannelRangeConfigs(0));
#ifdef USE_TEMPERATURE_SENSOR
cliPrintHashLine("temp_sensor");
printTempSensor(dumpMask, tempSensorConfig_CopyArray, tempSensorConfig(0));
#endif
#ifdef USE_OSD
cliPrintHashLine("osd_layout");
printOsdLayout(dumpMask, &osdConfig_Copy, osdConfig(), -1, -1);
@ -2797,6 +2904,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("status", "show status", NULL, cliStatus),
#ifndef SKIP_TASK_STATISTICS
CLI_COMMAND_DEF("tasks", "show task stats", NULL, cliTasks),
#endif
#ifdef USE_TEMPERATURE_SENSOR
CLI_COMMAND_DEF("temp_sensor", "change temp sensor settings", NULL, cliTempSensor),
#endif
CLI_COMMAND_DEF("version", "show version", NULL, cliVersion),
#ifdef USE_OSD

9
src/main/fc/fc_init.c
View file

@ -41,6 +41,7 @@
#include "cms/cms.h"
#include "drivers/1-wire.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/adc.h"
#include "drivers/compass/compass.h"
@ -531,6 +532,11 @@ void init(void)
}
#endif
// 1-Wire IF chip
#ifdef USE_1WIRE
owInit();
#endif
if (!sensorsAutodetect()) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
@ -629,7 +635,8 @@ void init(void)
blackboxInit();
#endif
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
gyroStartCalibration();
#ifdef USE_BARO
baroStartCalibration();
#endif

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

@ -104,6 +104,7 @@
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/opflow.h"
#include "sensors/temperature.h"
#include "telemetry/telemetry.h"
@ -454,7 +455,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
for (int i = 0; i < MAX_SERVO_RULES; i++) {
sbufWriteU8(dst, customServoMixers(i)->targetChannel);
sbufWriteU8(dst, customServoMixers(i)->inputSource);
sbufWriteU8(dst, customServoMixers(i)->rate);
sbufWriteU16(dst, customServoMixers(i)->rate);
sbufWriteU8(dst, customServoMixers(i)->speed);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 100);
@ -956,16 +957,23 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break;
case MSP_BLACKBOX_CONFIG:
sbufWriteU8(dst, 0); // API no longer supported
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
break;
case MSP2_BLACKBOX_CONFIG:
#ifdef USE_BLACKBOX
sbufWriteU8(dst, 1); //Blackbox supported
sbufWriteU8(dst, blackboxConfig()->device);
sbufWriteU8(dst, blackboxConfig()->rate_num);
sbufWriteU8(dst, blackboxConfig()->rate_denom);
sbufWriteU16(dst, blackboxConfig()->rate_num);
sbufWriteU16(dst, blackboxConfig()->rate_denom);
#else
sbufWriteU8(dst, 0); // Blackbox not supported
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
#endif
break;
@ -1107,7 +1115,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, HEADING_HOLD_ERROR_LPF_FREQ);
sbufWriteU16(dst, mixerConfig()->yaw_jump_prevention_limit);
sbufWriteU8(dst, gyroConfig()->gyro_lpf);
sbufWriteU8(dst, pidProfile()->acc_soft_lpf_hz);
sbufWriteU8(dst, accelerometerConfig()->acc_lpf_hz);
sbufWriteU8(dst, 0); //reserved
sbufWriteU8(dst, 0); //reserved
sbufWriteU8(dst, 0); //reserved
@ -1347,6 +1355,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, osdConfig()->alt_alarm);
sbufWriteU16(dst, osdConfig()->dist_alarm);
sbufWriteU16(dst, osdConfig()->neg_alt_alarm);
sbufWriteU16(dst, osdConfig()->imu_temp_alarm_min);
sbufWriteU16(dst, osdConfig()->imu_temp_alarm_max);
#ifdef USE_BARO
sbufWriteU16(dst, osdConfig()->baro_temp_alarm_min);
sbufWriteU16(dst, osdConfig()->baro_temp_alarm_max);
#else
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
#endif
break;
case MSP2_INAV_OSD_PREFERENCES:
@ -1382,6 +1399,31 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#endif
break;
#ifdef USE_TEMPERATURE_SENSOR
case MSP2_INAV_TEMP_SENSOR_CONFIG:
for (uint8_t index = 0; index < MAX_TEMP_SENSORS; ++index) {
const tempSensorConfig_t *sensorConfig = tempSensorConfig(index);
sbufWriteU8(dst, sensorConfig->type);
for (uint8_t addrIndex; addrIndex < 8; ++addrIndex)
sbufWriteU8(dst, ((uint8_t *)&sensorConfig->address)[addrIndex]);
for (uint8_t labelIndex; labelIndex < 4; ++labelIndex)
sbufWriteU8(dst, sensorConfig->label[labelIndex]);
sbufWriteU16(dst, sensorConfig->alarm_min);
sbufWriteU16(dst, sensorConfig->alarm_max);
}
break;
#endif
#ifdef USE_TEMPERATURE_SENSOR
case MSP2_INAV_TEMPERATURES:
for (uint8_t index = 0; index < MAX_TEMP_SENSORS; ++index) {
int16_t temperature;
bool valid = getSensorTemperature(index, &temperature);
sbufWriteU16(dst, valid ? temperature : -1000);
}
break;
#endif
default:
return false;
}
@ -1632,9 +1674,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
#endif
sbufReadU8(src); // multiwiiCurrentMeterOutput
tmp_u8 = sbufReadU8(src);
if (tmp_u8 <= MAX_SUPPORTED_RC_CHANNEL_COUNT)
if (tmp_u8 <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
rxConfigMutable()->rssi_channel = tmp_u8;
rxUpdateRSSISource(); // Changing rssi_channel might change the RSSI source
}
sbufReadU8(src);
#ifdef USE_MAG
@ -1768,10 +1811,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_SERVO_MIX_RULE:
sbufReadU8Safe(&tmp_u8, src);
if ((dataSize >= 8) && (tmp_u8 < MAX_SERVO_RULES)) {
if ((dataSize >= 9) && (tmp_u8 < MAX_SERVO_RULES)) {
customServoMixersMutable(tmp_u8)->targetChannel = sbufReadU8(src);
customServoMixersMutable(tmp_u8)->inputSource = sbufReadU8(src);
customServoMixersMutable(tmp_u8)->rate = sbufReadU8(src);
customServoMixersMutable(tmp_u8)->rate = sbufReadU16(src);
customServoMixersMutable(tmp_u8)->speed = sbufReadU8(src);
sbufReadU16(src); //Read 2bytes for min/max and ignore it
sbufReadU8(src); //Read 1 byte for `box` and ignore it
@ -1923,7 +1966,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
sbufReadU8(src); //HEADING_HOLD_ERROR_LPF_FREQ
mixerConfigMutable()->yaw_jump_prevention_limit = sbufReadU16(src);
gyroConfigMutable()->gyro_lpf = sbufReadU8(src);
pidProfileMutable()->acc_soft_lpf_hz = sbufReadU8(src);
accelerometerConfigMutable()->acc_lpf_hz = sbufReadU8(src);
sbufReadU8(src); //reserved
sbufReadU8(src); //reserved
sbufReadU8(src); //reserved
@ -2068,7 +2111,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_ACC_CALIBRATION:
if (!ARMING_FLAG(ARMED))
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
accStartCalibration();
else
return MSP_RESULT_ERROR;
break;
@ -2089,12 +2132,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
break;
#ifdef USE_BLACKBOX
case MSP_SET_BLACKBOX_CONFIG:
case MSP2_SET_BLACKBOX_CONFIG:
// Don't allow config to be updated while Blackbox is logging
if ((dataSize >= 3) && blackboxMayEditConfig()) {
if ((dataSize >= 5) && blackboxMayEditConfig()) {
blackboxConfigMutable()->device = sbufReadU8(src);
blackboxConfigMutable()->rate_num = sbufReadU8(src);
blackboxConfigMutable()->rate_denom = sbufReadU8(src);
blackboxConfigMutable()->rate_num = sbufReadU16(src);
blackboxConfigMutable()->rate_denom = sbufReadU16(src);
} else
return MSP_RESULT_ERROR;
break;
@ -2337,10 +2380,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_RSSI_CONFIG:
sbufReadU8Safe(&tmp_u8, src);
if ((dataSize >= 1) && (tmp_u8 <= MAX_SUPPORTED_RC_CHANNEL_COUNT))
if ((dataSize >= 1) && (tmp_u8 <= MAX_SUPPORTED_RC_CHANNEL_COUNT)) {
rxConfigMutable()->rssi_channel = tmp_u8;
else
rxUpdateRSSISource();
} else {
return MSP_RESULT_ERROR;
}
break;
case MSP_SET_RX_MAP:
@ -2561,29 +2606,42 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
}
break;
case MSP2_INAV_OSD_SET_ALARMS:
{
sbufReadU8Safe(&osdConfigMutable()->rssi_alarm, src);
sbufReadU16Safe(&osdConfigMutable()->time_alarm, src);
sbufReadU16Safe(&osdConfigMutable()->alt_alarm, src);
sbufReadU16Safe(&osdConfigMutable()->dist_alarm, src);
sbufReadU16Safe(&osdConfigMutable()->neg_alt_alarm, src);
osdStartFullRedraw();
if (dataSize >= 17) {
osdConfigMutable()->rssi_alarm = sbufReadU8(src);
osdConfigMutable()->time_alarm = sbufReadU16(src);
osdConfigMutable()->alt_alarm = sbufReadU16(src);
osdConfigMutable()->dist_alarm = sbufReadU16(src);
osdConfigMutable()->neg_alt_alarm = sbufReadU16(src);
osdConfigMutable()->imu_temp_alarm_min = sbufReadU16(src);
osdConfigMutable()->imu_temp_alarm_max = sbufReadU16(src);
#ifdef USE_BARO
osdConfigMutable()->baro_temp_alarm_min = sbufReadU16(src);
osdConfigMutable()->baro_temp_alarm_max = sbufReadU16(src);
#endif
} else
return MSP_RESULT_ERROR;
}
break;
case MSP2_INAV_OSD_SET_PREFERENCES:
{
sbufReadU8Safe(&osdConfigMutable()->video_system, src);
sbufReadU8Safe(&osdConfigMutable()->main_voltage_decimals, src);
sbufReadU8Safe(&osdConfigMutable()->ahi_reverse_roll, src);
sbufReadU8Safe(&osdConfigMutable()->crosshairs_style, src);
sbufReadU8Safe(&osdConfigMutable()->left_sidebar_scroll, src);
sbufReadU8Safe(&osdConfigMutable()->right_sidebar_scroll, src);
sbufReadU8Safe(&osdConfigMutable()->sidebar_scroll_arrows, src);
sbufReadU8Safe(&osdConfigMutable()->units, src);
sbufReadU8Safe(&osdConfigMutable()->stats_energy_unit, src);
if (dataSize == 9) {
osdConfigMutable()->video_system = sbufReadU8(src);
osdConfigMutable()->main_voltage_decimals = sbufReadU8(src);
osdConfigMutable()->ahi_reverse_roll = sbufReadU8(src);
osdConfigMutable()->crosshairs_style = sbufReadU8(src);
osdConfigMutable()->left_sidebar_scroll = sbufReadU8(src);
osdConfigMutable()->right_sidebar_scroll = sbufReadU8(src);
osdConfigMutable()->sidebar_scroll_arrows = sbufReadU8(src);
osdConfigMutable()->units = sbufReadU8(src);
osdConfigMutable()->stats_energy_unit = sbufReadU8(src);
osdStartFullRedraw();
} else
return MSP_RESULT_ERROR;
}
break;
@ -2591,6 +2649,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP2_INAV_SET_MC_BRAKING:
#ifdef USE_MR_BRAKING_MODE
if (dataSize == 14) {
navConfigMutable()->mc.braking_speed_threshold = sbufReadU16(src);
navConfigMutable()->mc.braking_disengage_speed = sbufReadU16(src);
navConfigMutable()->mc.braking_timeout = sbufReadU16(src);
@ -2599,7 +2658,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
navConfigMutable()->mc.braking_boost_speed_threshold = sbufReadU16(src);
navConfigMutable()->mc.braking_boost_disengage_speed = sbufReadU16(src);
navConfigMutable()->mc.braking_bank_angle = sbufReadU8(src);
} else
#endif
return MSP_RESULT_ERROR;
break;
case MSP2_INAV_SELECT_BATTERY_PROFILE:
@ -2609,6 +2670,24 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
}
break;
#ifdef USE_TEMPERATURE_SENSOR
case MSP2_INAV_SET_TEMP_SENSOR_CONFIG:
if (dataSize == sizeof(tempSensorConfig_t) * MAX_TEMP_SENSORS) {
for (uint8_t index = 0; index < MAX_TEMP_SENSORS; ++index) {
tempSensorConfig_t *sensorConfig = tempSensorConfigMutable(index);
sensorConfig->type = sbufReadU8(src);
for (uint8_t addrIndex; addrIndex < 8; ++addrIndex)
((uint8_t *)&sensorConfig->address)[addrIndex] = sbufReadU8(src);
for (uint8_t labelIndex; labelIndex < 4; ++labelIndex)
sensorConfig->label[labelIndex] = sbufReadU8(src);
sensorConfig->alarm_min = sbufReadU16(src);
sensorConfig->alarm_max = sbufReadU16(src);
}
} else
return MSP_RESULT_ERROR;
break;
#endif
default:
return MSP_RESULT_ERROR;
}

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

@ -367,7 +367,8 @@ uint16_t packSensorStatus(void)
IS_ENABLED(sensors(SENSOR_GPS)) << 3 |
IS_ENABLED(sensors(SENSOR_RANGEFINDER)) << 4 |
IS_ENABLED(sensors(SENSOR_OPFLOW)) << 5 |
IS_ENABLED(sensors(SENSOR_PITOT)) << 6;
IS_ENABLED(sensors(SENSOR_PITOT)) << 6 |
IS_ENABLED(sensors(SENSOR_TEMP)) << 7;
// Hardware failure indication bit
if (!isHardwareHealthy()) {

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

@ -387,7 +387,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
[TASK_TEMPERATURE] = {
.taskName = "TEMPERATURE",
.taskFunc = taskUpdateTemperature,
.desiredPeriod = TASK_PERIOD_HZ(1), // 1 Hz
.desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz
.staticPriority = TASK_PRIORITY_LOW,
},

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

@ -235,7 +235,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
// GYRO calibration
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
gyroStartCalibration();
return;
}
@ -317,7 +317,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
// Calibrating Acc
if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
accStartCalibration();
return;
}

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

@ -396,11 +396,11 @@ groups:
- name: blackbox_rate_num
field: rate_num
min: 1
max: 255
max: 65535
- name: blackbox_rate_denom
field: rate_denom
min: 1
max: 255
max: 65535
- name: blackbox_device
field: device
table: blackbox_device
@ -1592,6 +1592,24 @@ groups:
field: neg_alt_alarm
min: 0
max: 10000
- name: osd_imu_temp_alarm_min
field: imu_temp_alarm_min
min: -550
max: 1250
- name: osd_imu_temp_alarm_max
field: imu_temp_alarm_max
min: -550
max: 1250
- name: osd_baro_temp_alarm_min
field: baro_temp_alarm_min
condition: USE_BARO
min: -550
max: 1250
- name: osd_baro_temp_alarm_max
field: baro_temp_alarm_max
condition: USE_BARO
min: -550
max: 1250
- name: osd_artificial_horizon_reverse_roll
field: ahi_reverse_roll
@ -1768,4 +1786,3 @@ groups:
min: 0
max: 255
type: uint8_t

3
src/main/flight/pid.c
View file

@ -109,7 +109,7 @@ int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_
STATIC_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 5);
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 6);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = {
@ -170,7 +170,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
}
},
.acc_soft_lpf_hz = 15,
.dterm_soft_notch_hz = 0,
.dterm_soft_notch_cutoff = 1,
.dterm_lpf_hz = 40,

1
src/main/flight/pid.h
View file

@ -84,7 +84,6 @@ typedef struct pidProfile_s {
uint8_t dterm_lpf_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
uint8_t yaw_pterm_lpf_hz; // Used for filering Pterm noise on noisy frames
uint8_t acc_soft_lpf_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
uint8_t yaw_lpf_hz;
uint16_t yaw_p_limit;

4
src/main/flight/servos.c
View file

@ -231,8 +231,8 @@ void servoMixer(float dT)
input[INPUT_FEATURE_FLAPS] = FLIGHT_MODE(FLAPERON) ? servoConfig()->flaperon_throw_offset : 0;
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -1000, +1000);
input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -1000, +1000);
input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500);
input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500);
} else {
input[INPUT_GIMBAL_PITCH] = 0;
input[INPUT_GIMBAL_ROLL] = 0;

2
src/main/flight/servos.h
View file

@ -91,7 +91,7 @@ typedef enum {
typedef struct servoMixer_s {
uint8_t targetChannel; // servo that receives the output of the rule
uint8_t inputSource; // input channel for this rule
int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction
int16_t rate; // range [-1000;+1000] ; can be used to adjust a rate 0-1000% and a direction
uint8_t speed; // reduces the speed of the rule, 0=unlimited speed
} servoMixer_t;

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

@ -357,7 +357,7 @@ static void showStatusPage(void)
if (feature(FEATURE_VBAT)) {
i2c_OLED_set_line(rowIndex++);
tfp_sprintf(lineBuffer, "V: %d.%1d ", getBatteryVoltage() / 100, getBatteryVoltage() % 100);
tfp_sprintf(lineBuffer, "V: %d.%02d ", getBatteryVoltage() / 100, getBatteryVoltage() % 100);
padLineBufferToChar(12);
i2c_OLED_send_string(lineBuffer);
@ -387,7 +387,7 @@ static void showStatusPage(void)
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "HDOP: %d.%1d", gpsSol.hdop / 100, gpsSol.hdop % 100);
tfp_sprintf(lineBuffer, "HDOP: %d.%02d", gpsSol.hdop / 100, gpsSol.hdop % 100);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);

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

@ -181,7 +181,7 @@ static displayPort_t *osdDisplayPort;
#define AH_SIDEBAR_WIDTH_POS 7
#define AH_SIDEBAR_HEIGHT_POS 3
PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 5);
PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 6);
static int digitCount(int32_t value)
{
@ -500,27 +500,37 @@ static uint16_t osdConvertRSSI(void)
}
/**
* Converts temperature into a string based on the current unit system
* postfixed with a symbol to indicate the unit used.
* @param temperature Raw temperature (i.e. as taken from getCurrentTemperature() in degC)
* Displays a temperature postfixed with a symbol depending on the current unit system
* @param label to display
* @param valid true if measurement is valid
* @param temperature in deciDegrees Celcius
*/
static void osdFormatTemperatureSymbol(char *buff, float temperature)
static void osdDisplayTemperature(uint8_t elemPosX, uint8_t elemPosY, const char *label, bool valid, int16_t temperature, int16_t alarm_min, int16_t alarm_max)
{
int units_symbol;
switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_IMPERIAL:
units_symbol = SYM_TEMP_F;
temperature = (temperature * (9.0f/5)) + 32;
break;
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_METRIC:
units_symbol = SYM_TEMP_C;
break;
char buff[6];
textAttributes_t elemAttr = valid ? TEXT_ATTRIBUTES_NONE : _TEXT_ATTRIBUTES_BLINK_BIT;
if (label[0] != '\0') {
uint8_t label_len = strnlen(label, TEMPERATURE_LABEL_LEN);
memcpy(buff, label, label_len);
memset(buff + label_len, ' ', TEMPERATURE_LABEL_LEN + 1 - label_len);
buff[5] = '\0';
displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
}
osdFormatCentiNumber(buff, (int32_t) (temperature * 100), 0, 0, 0, 3);
buff[3] = units_symbol;
if (valid) {
if ((temperature <= alarm_min) || (temperature >= alarm_max)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
if (osdConfig()->units == OSD_UNIT_IMPERIAL) temperature = temperature * 9 / 5.0f + 320;
tfp_sprintf(buff, "%3d", temperature / 10);
} else
strcpy(buff, "---");
buff[3] = osdConfig()->units == OSD_UNIT_IMPERIAL ? SYM_TEMP_F : SYM_TEMP_C;
buff[4] = '\0';
displayWriteWithAttr(osdDisplayPort, elemPosX + 5, elemPosY, buff, elemAttr);
}
static void osdFormatCoordinate(char *buff, char sym, int32_t val)
@ -1293,6 +1303,11 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_HOME_DIR:
{
if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) {
buff[0] = SYM_HOME_NEAR;
}
else
{
// There are 16 orientations for the home direction arrow.
// so we use 22.5deg per image, where the 1st image is used
// for [349, 11], the 2nd for [12, 33], etc...
@ -1301,6 +1316,7 @@ static bool osdDrawSingleElement(uint8_t item)
int homeArrowDir = osdGetHeadingAngle(homeDirection + 11);
unsigned arrowOffset = homeArrowDir * 2 / 45;
buff[0] = SYM_ARROW_UP + arrowOffset;
}
} else {
// No home or no fix or unknown heading, blink.
// If we're unarmed, show the arrow pointing up so users can see the arrow
@ -2284,11 +2300,108 @@ static bool osdDrawSingleElement(uint8_t item)
break;
}
case OSD_TEMPERATURE:
case OSD_IMU_TEMPERATURE:
{
int16_t temperature = getCurrentTemperature();
osdFormatTemperatureSymbol(buff, temperature);
break;
int16_t temperature;
const bool valid = getIMUTemperature(&temperature);
osdDisplayTemperature(elemPosX, elemPosY, "IMU", valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max);
return true;
}
case OSD_BARO_TEMPERATURE:
{
int16_t temperature;
const bool valid = getBaroTemperature(&temperature);
osdDisplayTemperature(elemPosX, elemPosY, "BARO", valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max);
return true;
}
case OSD_TEMP_SENSOR_0_TEMPERATURE:
{
#ifdef USE_TEMPERATURE_SENSOR
int16_t temperature;
const bool valid = getSensorTemperature(0, &temperature);
const tempSensorConfig_t *sensorConfig = tempSensorConfig(0);
osdDisplayTemperature(elemPosX, elemPosY, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max);
#endif
return true;
}
case OSD_TEMP_SENSOR_1_TEMPERATURE:
{
#ifdef USE_TEMPERATURE_SENSOR
int16_t temperature;
const bool valid = getSensorTemperature(1, &temperature);
const tempSensorConfig_t *sensorConfig = tempSensorConfig(1);
osdDisplayTemperature(elemPosX, elemPosY, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max);
#endif
return true;
}
case OSD_TEMP_SENSOR_2_TEMPERATURE:
{
#ifdef USE_TEMPERATURE_SENSOR
int16_t temperature;
const bool valid = getSensorTemperature(2, &temperature);
const tempSensorConfig_t *sensorConfig = tempSensorConfig(2);
osdDisplayTemperature(elemPosX, elemPosY, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max);
#endif
return true;
}
case OSD_TEMP_SENSOR_3_TEMPERATURE:
{
#ifdef USE_TEMPERATURE_SENSOR
int16_t temperature;
const bool valid = getSensorTemperature(3, &temperature);
const tempSensorConfig_t *sensorConfig = tempSensorConfig(3);
osdDisplayTemperature(elemPosX, elemPosY, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max);
#endif
return true;
}
case OSD_TEMP_SENSOR_4_TEMPERATURE:
{
#ifdef USE_TEMPERATURE_SENSOR
int16_t temperature;
const bool valid = getSensorTemperature(4, &temperature);
const tempSensorConfig_t *sensorConfig = tempSensorConfig(4);
osdDisplayTemperature(elemPosX, elemPosY, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max);
#endif
return true;
}
case OSD_TEMP_SENSOR_5_TEMPERATURE:
{
#ifdef USE_TEMPERATURE_SENSOR
int16_t temperature;
const bool valid = getSensorTemperature(5, &temperature);
const tempSensorConfig_t *sensorConfig = tempSensorConfig(5);
osdDisplayTemperature(elemPosX, elemPosY, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max);
#endif
return true;
}
case OSD_TEMP_SENSOR_6_TEMPERATURE:
{
#ifdef USE_TEMPERATURE_SENSOR
int16_t temperature;
const bool valid = getSensorTemperature(6, &temperature);
const tempSensorConfig_t *sensorConfig = tempSensorConfig(6);
osdDisplayTemperature(elemPosX, elemPosY, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max);
#endif
return true;
}
case OSD_TEMP_SENSOR_7_TEMPERATURE:
{
#ifdef USE_TEMPERATURE_SENSOR
int16_t temperature;
const bool valid = getSensorTemperature(7, &temperature);
const tempSensorConfig_t *sensorConfig = tempSensorConfig(7);
osdDisplayTemperature(elemPosX, elemPosY, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max);
#endif
return true;
}
case OSD_WIND_SPEED_HORIZONTAL:
@ -2532,7 +2645,17 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
osdConfig->item_pos[0][OSD_MC_POS_XYZ_P_OUTPUTS] = OSD_POS(2, 12);
osdConfig->item_pos[0][OSD_POWER] = OSD_POS(15, 1);
osdConfig->item_pos[0][OSD_TEMPERATURE] = OSD_POS(23, 2);
osdConfig->item_pos[0][OSD_IMU_TEMPERATURE] = OSD_POS(19, 2);
osdConfig->item_pos[0][OSD_BARO_TEMPERATURE] = OSD_POS(19, 3);
osdConfig->item_pos[0][OSD_TEMP_SENSOR_0_TEMPERATURE] = OSD_POS(19, 4);
osdConfig->item_pos[0][OSD_TEMP_SENSOR_1_TEMPERATURE] = OSD_POS(19, 5);
osdConfig->item_pos[0][OSD_TEMP_SENSOR_2_TEMPERATURE] = OSD_POS(19, 6);
osdConfig->item_pos[0][OSD_TEMP_SENSOR_3_TEMPERATURE] = OSD_POS(19, 7);
osdConfig->item_pos[0][OSD_TEMP_SENSOR_4_TEMPERATURE] = OSD_POS(19, 8);
osdConfig->item_pos[0][OSD_TEMP_SENSOR_5_TEMPERATURE] = OSD_POS(19, 9);
osdConfig->item_pos[0][OSD_TEMP_SENSOR_6_TEMPERATURE] = OSD_POS(19, 10);
osdConfig->item_pos[0][OSD_TEMP_SENSOR_7_TEMPERATURE] = OSD_POS(19, 11);
osdConfig->item_pos[0][OSD_AIR_SPEED] = OSD_POS(3, 5);
osdConfig->item_pos[0][OSD_WIND_SPEED_HORIZONTAL] = OSD_POS(3, 6);
@ -2552,6 +2675,12 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
osdConfig->alt_alarm = 100;
osdConfig->dist_alarm = 1000;
osdConfig->neg_alt_alarm = 5;
osdConfig->imu_temp_alarm_min = -200;
osdConfig->imu_temp_alarm_max = 600;
#ifdef USE_BARO
osdConfig->baro_temp_alarm_min = -200;
osdConfig->baro_temp_alarm_max = 600;
#endif
osdConfig->video_system = VIDEO_SYSTEM_AUTO;

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

@ -121,10 +121,19 @@ typedef enum {
OSD_MC_VEL_Y_PID_OUTPUTS,
OSD_MC_VEL_Z_PID_OUTPUTS,
OSD_MC_POS_XYZ_P_OUTPUTS,
OSD_3D_SPEED, // 85
OSD_TEMPERATURE, // 86
OSD_ALTITUDE_MSL, // 87
OSD_PLUS_CODE, // 88
OSD_3D_SPEED,
OSD_IMU_TEMPERATURE,
OSD_BARO_TEMPERATURE,
OSD_TEMP_SENSOR_0_TEMPERATURE,
OSD_TEMP_SENSOR_1_TEMPERATURE,
OSD_TEMP_SENSOR_2_TEMPERATURE,
OSD_TEMP_SENSOR_3_TEMPERATURE,
OSD_TEMP_SENSOR_4_TEMPERATURE,
OSD_TEMP_SENSOR_5_TEMPERATURE,
OSD_TEMP_SENSOR_6_TEMPERATURE,
OSD_TEMP_SENSOR_7_TEMPERATURE,
OSD_ALTITUDE_MSL,
OSD_PLUS_CODE,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;
@ -161,6 +170,12 @@ typedef struct osdConfig_s {
uint16_t alt_alarm; // positive altitude in m
uint16_t dist_alarm; // home distance in m
uint16_t neg_alt_alarm; // abs(negative altitude) in m
int16_t imu_temp_alarm_min;
int16_t imu_temp_alarm_max;
#ifdef USE_BARO
int16_t baro_temp_alarm_min;
int16_t baro_temp_alarm_max;
#endif
videoSystem_e video_system;
uint8_t row_shiftdown;

2
src/main/io/serial.c
View file

@ -115,7 +115,7 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig)
serialConfig->portConfigs[i].peripheral_baudrateIndex = BAUD_115200;
}
serialConfig->portConfigs[0].functionMask = FUNCTION_MSP;
serialConfig->portConfigs[0].functionMask = FUNCTION_MSP | FUNCTION_DEBUG_TRACE;
#ifdef SERIALRX_UART
serialPortConfig_t *serialRxUartConfig = serialFindPortConfiguration(SERIALRX_UART);

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

@ -60,7 +60,7 @@
#define MSP_PROTOCOL_VERSION 0 // Same version over MSPv1 & MSPv2 - message format didn't change and it backward compatible
#define API_VERSION_MAJOR 2 // increment when major changes are made
#define API_VERSION_MINOR 2 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_MINOR 3 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_LENGTH 2

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

@ -45,3 +45,10 @@
#define MSP2_INAV_SELECT_BATTERY_PROFILE 0x2018
#define MSP2_INAV_DEBUG 0x2019
#define MSP2_BLACKBOX_CONFIG 0x201A
#define MSP2_SET_BLACKBOX_CONFIG 0x201B
#define MSP2_INAV_TEMP_SENSOR_CONFIG 0x201C
#define MSP2_INAV_SET_TEMP_SENSOR_CONFIG 0x201D
#define MSP2_INAV_TEMPERATURES 0x201E

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

@ -342,18 +342,24 @@ void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs)
* Update IMU topic
* Function is called at main loop rate
*/
static void restartGravityCalibration(void)
{
zeroCalibrationStartS(&posEstimator.imu.gravityCalibration, CALIBRATING_GRAVITY_TIME_MS, positionEstimationConfig()->gravity_calibration_tolerance, false);
}
static bool gravityCalibrationComplete(void)
{
return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
}
static void updateIMUTopic(void)
{
static float calibratedGravityCMSS = GRAVITY_CMSS;
static timeMs_t gravityCalibrationTimeout = 0;
if (!isImuReady()) {
posEstimator.imu.accelNEU.x = 0;
posEstimator.imu.accelNEU.y = 0;
posEstimator.imu.accelNEU.z = 0;
gravityCalibrationTimeout = millis();
posEstimator.imu.gravityCalibrationComplete = false;
restartGravityCalibration();
}
else {
fpVector3_t accelBF;
@ -377,24 +383,18 @@ static void updateIMUTopic(void)
posEstimator.imu.accelNEU.z = accelBF.z;
/* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
if (!ARMING_FLAG(ARMED) && !posEstimator.imu.gravityCalibrationComplete) {
// Slowly converge on calibrated gravity while level
const float gravityOffsetError = posEstimator.imu.accelNEU.z - calibratedGravityCMSS;
calibratedGravityCMSS += gravityOffsetError * 0.0025f;
if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) {
zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z);
if (fabsf(gravityOffsetError) < positionEstimationConfig()->gravity_calibration_tolerance) { // Error should be within 0.5% of calibrated gravity
if ((millis() - gravityCalibrationTimeout) > 250) {
posEstimator.imu.gravityCalibrationComplete = true;
}
}
else {
gravityCalibrationTimeout = millis();
if (gravityCalibrationComplete()) {
zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS);
DEBUG_TRACE_SYNC("Gravity calibration complete (%d)", lrintf(posEstimator.imu.calibratedGravityCMSS));
}
}
/* If calibration is incomplete - report zero acceleration */
if (posEstimator.imu.gravityCalibrationComplete) {
posEstimator.imu.accelNEU.z -= calibratedGravityCMSS;
if (gravityCalibrationComplete()) {
posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
}
else {
posEstimator.imu.accelNEU.x = 0;
@ -768,7 +768,7 @@ void initializePositionEstimator(void)
posEstimator.est.flowCoordinates[X] = 0;
posEstimator.est.flowCoordinates[Y] = 0;
posEstimator.imu.gravityCalibrationComplete = false;
restartGravityCalibration();
for (axis = 0; axis < 3; axis++) {
posEstimator.imu.accelBias.v[axis] = 0;
@ -807,7 +807,7 @@ void updatePositionEstimator(void)
bool navIsCalibrationComplete(void)
{
return posEstimator.imu.gravityCalibrationComplete;
return gravityCalibrationComplete();
}
#endif

6
src/main/navigation/navigation_pos_estimator_private.h
View file

@ -23,6 +23,7 @@
#include "common/axis.h"
#include "common/maths.h"
#include "common/filter.h"
#include "common/calibration.h"
#include "sensors/sensors.h"
@ -44,6 +45,8 @@
#define INAV_SURFACE_TIMEOUT_MS 400 // Surface timeout (missed 3 readings in a row)
#define INAV_FLOW_TIMEOUT_MS 200
#define CALIBRATING_GRAVITY_TIME_MS 2000
// Time constants for calculating Baro/Sonar averages. Should be the same value to impose same amount of group delay
#define INAV_BARO_AVERAGE_HZ 1.0f
#define INAV_SURFACE_AVERAGE_HZ 1.0f
@ -125,7 +128,8 @@ typedef struct {
typedef struct {
fpVector3_t accelNEU;
fpVector3_t accelBias;
bool gravityCalibrationComplete;
float calibratedGravityCMSS;
zeroCalibrationScalar_t gravityCalibration;
} navPosisitonEstimatorIMU_t;
typedef enum {

55
src/main/rx/crsf.c
View file

@ -40,11 +40,13 @@
#include "rx/rx.h"
#include "rx/crsf.h"
#define CRSF_TIME_NEEDED_PER_FRAME_US 1000
#define CRSF_TIME_BETWEEN_FRAMES_US 4000 // a frame is sent by the transmitter every 4 milliseconds
#include "telemetry/crsf.h"
#define CRSF_TIME_NEEDED_PER_FRAME_US 1100 // 700 ms + 400 ms for potential ad-hoc request
#define CRSF_TIME_BETWEEN_FRAMES_US 6667 // At fastest, frames are sent by the transmitter every 6.667 milliseconds, 150 Hz
#define CRSF_DIGITAL_CHANNEL_MIN 172
#define CRSF_DIGITAL_CHANNEL_MAX 1811
#define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type)
STATIC_UNIT_TESTED bool crsfFrameDone = false;
STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
@ -69,10 +71,10 @@ static uint8_t telemetryBufLen = 0;
* 1 Stop bit
* Big endian
* 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte
* Assume a max payload of 32 bytes (needs confirming with TBS), so max frame size of 36 bytes
* A 36 byte frame can be transmitted in 771 microseconds.
* Max frame size is 64 bytes
* A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds.
*
* CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1000 microseconds
* CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds
*
* Every frame has the structure:
* <Device address> <Frame length> < Type> <Payload> < CRC>
@ -121,6 +123,16 @@ struct crsfPayloadLinkStatistics_s {
typedef struct crsfPayloadLinkStatistics_s crsfPayloadLinkStatistics_t;
STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
{
// CRC includes type and payload
uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) {
crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
}
return crc;
}
// Receive ISR callback, called back from serial port
STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData)
{
@ -149,17 +161,30 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData)
if (crsfFramePosition < fullFrameLength) {
crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true;
if (crsfFrameDone) {
crsfFramePosition = 0;
if (crsfFrame.frame.type != CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
const uint8_t crc = crsfFrameCRC();
if (crc == crsfFrame.bytes[fullFrameLength - 1]) {
switch (crsfFrame.frame.type)
{
#if defined(USE_MSP_OVER_TELEMETRY)
case CRSF_FRAMETYPE_MSP_REQ:
case CRSF_FRAMETYPE_MSP_WRITE: {
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
crsfScheduleMspResponse();
}
break;
}
#endif
default:
break;
}
}
}
}
}
STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
{
// CRC includes type and payload
uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) {
crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
}
return crc;
}
STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
@ -261,7 +286,7 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
}
rxRuntimeConfig->channelCount = CRSF_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 11000; //!!TODO this needs checking
rxRuntimeConfig->rxRefreshRate = CRSF_TIME_BETWEEN_FRAMES_US; //!!TODO this needs checking
rxRuntimeConfig->rcReadRawFn = crsfReadRawRC;
rxRuntimeConfig->rcFrameStatusFn = crsfFrameStatus;

68
src/main/rx/crsf.h
View file

@ -23,14 +23,23 @@
#define CRSF_MAX_CHANNEL 17
typedef enum {
CRSF_FRAMETYPE_GPS = 0x02,
CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
CRSF_FRAMETYPE_ATTITUDE = 0x1E,
CRSF_FRAMETYPE_FLIGHT_MODE = 0x21
} crsfFrameTypes_e;
enum { CRSF_SYNC_BYTE = 0xC8 };
enum { CRSF_FRAME_SIZE_MAX = 64 }; // 62 bytes frame plus 2 bytes frame header(<length><type>)
enum { CRSF_PAYLOAD_SIZE_MAX = CRSF_FRAME_SIZE_MAX - 6 };
enum {
CRSF_DISPLAYPORT_SUBCMD_UPDATE = 0x01, // transmit displayport buffer to remote
CRSF_DISPLAYPORT_SUBCMD_CLEAR = 0X02, // clear client screen
CRSF_DISPLAYPORT_SUBCMD_OPEN = 0x03, // client request to open cms menu
CRSF_DISPLAYPORT_SUBCMD_CLOSE = 0x04, // client request to close cms menu
CRSF_DISPLAYPORT_SUBCMD_POLL = 0x05, // client request to poll/refresh cms menu
};
enum {
CRSF_DISPLAYPORT_OPEN_ROWS_OFFSET = 1,
CRSF_DISPLAYPORT_OPEN_COLS_OFFSET = 2,
};
enum {
CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
@ -42,25 +51,56 @@ enum {
CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field
CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field
CRSF_FRAME_LENGTH_CRC = 1, // length of CRC field
CRSF_FRAME_LENGTH_TYPE_CRC = 2 // length of TYPE and CRC fields combined
CRSF_FRAME_LENGTH_TYPE_CRC = 2, // length of TYPE and CRC fields combined
CRSF_FRAME_LENGTH_EXT_TYPE_CRC = 4, // length of Extended Dest/Origin, TYPE and CRC fields combined
CRSF_FRAME_LENGTH_NON_PAYLOAD = 4, // combined length of all fields except payload
};
enum {
CRSF_FRAME_TX_MSP_FRAME_SIZE = 58,
CRSF_FRAME_RX_MSP_FRAME_SIZE = 8,
CRSF_FRAME_ORIGIN_DEST_SIZE = 2,
};
// Clashes with CRSF_ADDRESS_FLIGHT_CONTROLLER
#define CRSF_TELEMETRY_SYNC_BYTE 0XC8
typedef enum {
CRSF_ADDRESS_BROADCAST = 0x00,
CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x8,
CRSF_ADDRESS_USB = 0x10,
CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x80,
CRSF_ADDRESS_RESERVED1 = 0x8A,
CRSF_ADDRESS_CURRENT_SENSOR = 0xC0,
CRSF_ADDRESS_GPS = 0xC2,
CRSF_ADDRESS_TBS_BLACKBOX = 0xC4,
CRSF_ADDRESS_COLIBRI_RACE_FC = 0xC8,
CRSF_ADDRESS_FLIGHT_CONTROLLER = 0xC8,
CRSF_ADDRESS_RESERVED2 = 0xCA,
CRSF_ADDRESS_RACE_TAG = 0xCC,
CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA,
CRSF_ADDRESS_CRSF_RECEIVER = 0xEC,
CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE
};
} crsfAddress_e;
#define CRSF_PAYLOAD_SIZE_MAX 62
#define CRSF_FRAME_SIZE_MAX (CRSF_PAYLOAD_SIZE_MAX + 4)
typedef enum {
CRSF_FRAMETYPE_GPS = 0x02,
CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
CRSF_FRAMETYPE_ATTITUDE = 0x1E,
CRSF_FRAMETYPE_FLIGHT_MODE = 0x21,
// Extended Header Frames, range: 0x28 to 0x96
CRSF_FRAMETYPE_DEVICE_PING = 0x28,
CRSF_FRAMETYPE_DEVICE_INFO = 0x29,
CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY = 0x2B,
CRSF_FRAMETYPE_PARAMETER_READ = 0x2C,
CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D,
CRSF_FRAMETYPE_COMMAND = 0x32,
// MSP commands
CRSF_FRAMETYPE_MSP_REQ = 0x7A, // response request using msp sequence as command
CRSF_FRAMETYPE_MSP_RESP = 0x7B, // reply with 58 byte chunked binary
CRSF_FRAMETYPE_MSP_WRITE = 0x7C, // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit)
CRSF_FRAMETYPE_DISPLAYPORT_CMD = 0x7D, // displayport control command
} crsfFrameType_e;
typedef struct crsfFrameDef_s {
uint8_t deviceAddress;

6
src/main/rx/eleres.c
View file

@ -272,8 +272,12 @@ static void telemetryRX(void)
presfil -= presfil/4;
presfil += baro.baroPressure;
int16_t temperature;
const bool temp_valid = sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature);
if (!temp_valid) temperature = TEMPERATURE_INVALID_VALUE; // If temperature not valid report value outside of range
thempfil -= thempfil/8;
thempfil += DEGREES_TO_DECIDEGREES(getCurrentTemperature());
thempfil += temperature;
switch (telem_state++) {
case 0:

2
src/main/scheduler/protothreads.h
View file

@ -39,6 +39,8 @@
#pragma once
#include "common/time.h"
#include "common/utils.h"
#include "drivers/time.h"
/*
Protothreads are a extremely lightweight, stackless threads that provides a blocking context, without the overhead of per-thread stacks.

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

@ -27,6 +27,7 @@
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
#include "common/calibration.h"
#include "config/config_reset.h"
#include "config/parameter_group.h"
@ -70,7 +71,7 @@
FASTRAM acc_t acc; // acc access functions
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
STATIC_FASTRAM zeroCalibrationVector_t zeroCalibration;
STATIC_FASTRAM int32_t accADC[XYZ_AXIS_COUNT];
@ -341,26 +342,6 @@ bool accInit(uint32_t targetLooptime)
return true;
}
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
calibratingA = calibrationCyclesRequired;
}
bool accIsCalibrationComplete(void)
{
return calibratingA == 0;
}
static bool isOnFinalAccelerationCalibrationCycle(void)
{
return calibratingA == 1;
}
static bool isOnFirstAccelerationCalibrationCycle(void)
{
return calibratingA == CALIBRATING_ACC_CYCLES;
}
static bool calibratedPosition[6];
static int32_t accSamples[6][3];
static int calibratedAxisCount = 0;
@ -406,17 +387,21 @@ static int getPrimaryAxisIndex(int32_t accADCData[3])
return -1;
}
static void performAcclerationCalibration(void)
bool accIsCalibrationComplete(void)
{
return zeroCalibrationIsCompleteV(&zeroCalibration);
}
void accStartCalibration(void)
{
int positionIndex = getPrimaryAxisIndex(accADC);
// Check if sample is usable
if (positionIndex < 0) {
return;
}
// Top-up and first calibration cycle, reset everything
if (positionIndex == 0 && isOnFirstAccelerationCalibrationCycle()) {
// Top+up and first calibration cycle, reset everything
if (positionIndex == 0) {
for (int axis = 0; axis < 6; axis++) {
calibratedPosition[axis] = false;
accSamples[axis][X] = 0;
@ -428,19 +413,40 @@ static void performAcclerationCalibration(void)
DISABLE_STATE(ACCELEROMETER_CALIBRATED);
}
zeroCalibrationStartV(&zeroCalibration, CALIBRATING_ACC_TIME_MS, acc.dev.acc_1G * 0.01f, true);
}
static void performAcclerationCalibration(void)
{
fpVector3_t v;
int positionIndex = getPrimaryAxisIndex(accADC);
// Check if sample is usable
if (positionIndex < 0) {
return;
}
if (!calibratedPosition[positionIndex]) {
accSamples[positionIndex][X] += accADC[X];
accSamples[positionIndex][Y] += accADC[Y];
accSamples[positionIndex][Z] += accADC[Z];
v.v[0] = accADC[0];
v.v[1] = accADC[1];
v.v[2] = accADC[2];
zeroCalibrationAddValueV(&zeroCalibration, &v);
if (zeroCalibrationIsCompleteV(&zeroCalibration)) {
if (zeroCalibrationIsSuccessfulV(&zeroCalibration)) {
zeroCalibrationGetZeroV(&zeroCalibration, &v);
accSamples[positionIndex][X] = v.v[X];
accSamples[positionIndex][Y] = v.v[Y];
accSamples[positionIndex][Z] = v.v[Z];
if (isOnFinalAccelerationCalibrationCycle()) {
calibratedPosition[positionIndex] = true;
accSamples[positionIndex][X] = accSamples[positionIndex][X] / CALIBRATING_ACC_CYCLES;
accSamples[positionIndex][Y] = accSamples[positionIndex][Y] / CALIBRATING_ACC_CYCLES;
accSamples[positionIndex][Z] = accSamples[positionIndex][Z] / CALIBRATING_ACC_CYCLES;
calibratedAxisCount++;
}
else {
calibratedPosition[positionIndex] = false;
}
beeperConfirmationBeeps(2);
}
@ -459,9 +465,9 @@ static void performAcclerationCalibration(void)
sensorCalibrationSolveForOffset(&calState, accTmp);
for (int axis = 0; axis < 3; axis++) {
accelerometerConfigMutable()->accZero.raw[axis] = lrintf(accTmp[axis]);
}
accelerometerConfigMutable()->accZero.raw[X] = lrintf(accTmp[X]);
accelerometerConfigMutable()->accZero.raw[Y] = lrintf(accTmp[Y]);
accelerometerConfigMutable()->accZero.raw[Z] = lrintf(accTmp[Z]);
/* Not we can offset our accumulated averages samples and calculate scale factors and calculate gains */
sensorCalibrationResetState(&calState);
@ -484,8 +490,6 @@ static void performAcclerationCalibration(void)
saveConfigAndNotify();
}
calibratingA--;
}
static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain)

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

@ -73,7 +73,7 @@ PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
bool accInit(uint32_t accTargetLooptime);
bool accIsCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void accStartCalibration(void);
void accGetMeasuredAcceleration(fpVector3_t *measuredAcc);
void accGetVibrationLevels(fpVector3_t *accVibeLevels);
float accGetVibrationLevel(void);

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

@ -26,6 +26,7 @@
#include "common/maths.h"
#include "common/time.h"
#include "common/utils.h"
#include "common/calibration.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
@ -66,8 +67,7 @@ PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig,
#ifdef USE_BARO
static timeMs_t baroCalibrationTimeout = 0;
static bool baroCalibrationFinished = false;
static zeroCalibrationScalar_t zeroCalibration;
static float baroGroundAltitude = 0;
static float baroGroundPressure = 101325.0f; // 101325 pascal, 1 standard atmosphere
@ -183,17 +183,6 @@ bool baroInit(void)
return true;
}
bool baroIsCalibrationComplete(void)
{
return baroCalibrationFinished;
}
void baroStartCalibration(void)
{
baroCalibrationTimeout = millis();
baroCalibrationFinished = false;
}
#define PRESSURE_SAMPLES_MEDIAN 3
/*
@ -275,27 +264,33 @@ static float pressureToAltitude(const float pressure)
return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f;
}
static void performBaroCalibrationCycle(void)
static float altitudeToPressure(const float altCm)
{
const float baroGroundPressureError = baro.baroPressure - baroGroundPressure;
baroGroundPressure += baroGroundPressureError * 0.15f;
return powf(1.0f - (altCm / 4433000.0f), 5.254999) * 101325.0f;
}
if (fabsf(baroGroundPressureError) < (baroGroundPressure * 0.00005f)) { // 0.005% calibration error (should give c. 10cm calibration error)
if ((millis() - baroCalibrationTimeout) > 250) {
baroGroundAltitude = pressureToAltitude(baroGroundPressure);
baroCalibrationFinished = true;
DEBUG_TRACE_SYNC("Barometer calibration complete (%d)", lrintf(baroGroundAltitude));
}
}
else {
baroCalibrationTimeout = millis();
}
bool baroIsCalibrationComplete(void)
{
return zeroCalibrationIsCompleteS(&zeroCalibration) && zeroCalibrationIsSuccessfulS(&zeroCalibration);
}
void baroStartCalibration(void)
{
const float acceptedPressureVariance = (101325.0f - altitudeToPressure(30.0f)); // max 30cm deviation during calibration (at sea level)
zeroCalibrationStartS(&zeroCalibration, CALIBRATING_BARO_TIME_MS, acceptedPressureVariance, false);
}
int32_t baroCalculateAltitude(void)
{
if (!baroIsCalibrationComplete()) {
performBaroCalibrationCycle();
zeroCalibrationAddValueS(&zeroCalibration, baro.baroPressure);
if (zeroCalibrationIsCompleteS(&zeroCalibration)) {
zeroCalibrationGetZeroS(&zeroCalibration, &baroGroundPressure);
baroGroundAltitude = pressureToAltitude(baroGroundPressure);
DEBUG_TRACE_SYNC("Barometer calibration complete (%d)", lrintf(baroGroundAltitude));
}
baro.BaroAlt = 0;
}
else {

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

@ -27,6 +27,7 @@
#include "common/axis.h"
#include "common/maths.h"
#include "common/calibration.h"
#include "common/filter.h"
#include "common/utils.h"
@ -74,13 +75,7 @@ FASTRAM gyro_t gyro; // gyro sensor object
STATIC_UNIT_TESTED gyroDev_t gyroDev0; // Not in FASTRAM since it may hold DMA buffers
STATIC_FASTRAM int16_t gyroTemperature0;
typedef struct gyroCalibration_s {
int32_t g[XYZ_AXIS_COUNT];
stdev_t var[XYZ_AXIS_COUNT];
uint16_t calibratingG;
} gyroCalibration_t;
STATIC_FASTRAM_UNIT_TESTED gyroCalibration_t gyroCalibration;
STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration;
STATIC_FASTRAM int32_t gyroADC[XYZ_AXIS_COUNT];
STATIC_FASTRAM filterApplyFnPtr softLpfFilterApplyFn;
@ -344,64 +339,42 @@ void gyroInitFilters(void)
#endif
}
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
void gyroStartCalibration(void)
{
gyroCalibration.calibratingG = calibrationCyclesRequired;
zeroCalibrationStartV(&gyroCalibration, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
}
STATIC_UNIT_TESTED bool FAST_CODE NOINLINE isCalibrationComplete(gyroCalibration_t *gyroCalibration)
bool FAST_CODE NOINLINE gyroIsCalibrationComplete(void)
{
return gyroCalibration->calibratingG == 0;
return zeroCalibrationIsCompleteV(&gyroCalibration) && zeroCalibrationIsSuccessfulV(&gyroCalibration);
}
bool gyroIsCalibrationComplete(void)
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration)
{
return gyroCalibration.calibratingG == 0;
}
fpVector3_t v;
static bool isOnFinalGyroCalibrationCycle(gyroCalibration_t *gyroCalibration)
{
return gyroCalibration->calibratingG == 1;
}
// Consume gyro reading
v.v[0] = dev->gyroADCRaw[0];
v.v[1] = dev->gyroADCRaw[1];
v.v[2] = dev->gyroADCRaw[2];
static bool isOnFirstGyroCalibrationCycle(gyroCalibration_t *gyroCalibration)
{
return gyroCalibration->calibratingG == CALIBRATING_GYRO_CYCLES;
}
zeroCalibrationAddValueV(gyroCalibration, &v);
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold)
{
for (int axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle(gyroCalibration)) {
gyroCalibration->g[axis] = 0;
devClear(&gyroCalibration->var[axis]);
}
// Check if calibration is complete after this cycle
if (zeroCalibrationIsCompleteV(gyroCalibration)) {
zeroCalibrationGetZeroV(gyroCalibration, &v);
dev->gyroZero[0] = v.v[0];
dev->gyroZero[1] = v.v[1];
dev->gyroZero[2] = v.v[2];
// Sum up CALIBRATING_GYRO_CYCLES readings
gyroCalibration->g[axis] += dev->gyroADCRaw[axis];
devPush(&gyroCalibration->var[axis], dev->gyroADCRaw[axis]);
// gyroZero is set to zero until calibration complete
dev->gyroZero[axis] = 0;
if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
const float stddev = devStandardDeviation(&gyroCalibration->var[axis]);
// check deviation and start over if the model was moved
if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
return;
}
// calibration complete, so set the gyro zero values
dev->gyroZero[axis] = (gyroCalibration->g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
}
}
if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
DEBUG_TRACE_SYNC("Gyro calibration complete (%d, %d, %d)", dev->gyroZero[0], dev->gyroZero[1], dev->gyroZero[2]);
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
}
gyroCalibration->calibratingG--;
else {
dev->gyroZero[0] = 0;
dev->gyroZero[1] = 0;
dev->gyroZero[2] = 0;
}
}
/*
@ -418,7 +391,7 @@ void FAST_CODE NOINLINE gyroUpdate()
{
// range: +/- 8192; +/- 2000 deg/sec
if (gyroDev0.readFn(&gyroDev0)) {
if (isCalibrationComplete(&gyroCalibration)) {
if (zeroCalibrationIsCompleteV(&gyroCalibration)) {
// Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment
gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X];
gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y];
@ -426,7 +399,7 @@ void FAST_CODE NOINLINE gyroUpdate()
applySensorAlignment(gyroADC, gyroADC, gyroDev0.gyroAlign);
applyBoardAlignment(gyroADC);
} else {
performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold);
performGyroCalibration(&gyroDev0, &gyroCalibration);
// Reset gyro values to zero to prevent other code from using uncalibrated data
gyro.gyroADCf[X] = 0.0f;
gyro.gyroADCf[Y] = 0.0f;

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

@ -68,7 +68,7 @@ bool gyroInit(void);
void gyroInitFilters(void);
void gyroGetMeasuredRotationRate(fpVector3_t *imuMeasuredRotationBF);
void gyroUpdate();
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroStartCalibration(void);
bool gyroIsCalibrationComplete(void);
bool gyroReadTemperature(void);
int16_t gyroGetTemperature(void);

18
src/main/sensors/pitotmeter.c
View file

@ -153,27 +153,21 @@ bool pitotInit(void)
bool pitotIsCalibrationComplete(void)
{
return pitot.calibrationFinished;
return zeroCalibrationIsCompleteS(&pitot.zeroCalibration) && zeroCalibrationIsSuccessfulS(&pitot.zeroCalibration);
}
void pitotStartCalibration(void)
{
pitot.calibrationTimeoutMs = millis();
pitot.calibrationFinished = false;
zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, P0 * 0.00001f, false);
}
static void performPitotCalibrationCycle(void)
{
const float pitotPressureZeroError = pitot.pressure - pitot.pressureZero;
pitot.pressureZero += pitotPressureZeroError * 0.25f;
zeroCalibrationAddValueS(&pitot.zeroCalibration, pitot.pressure);
if (fabsf(pitotPressureZeroError) < (P0 * 0.000005f)) {
if ((millis() - pitot.calibrationTimeoutMs) > 500) {
pitot.calibrationFinished = true;
}
}
else {
pitot.calibrationTimeoutMs = millis();
if (zeroCalibrationIsCompleteS(&pitot.zeroCalibration)) {
zeroCalibrationGetZeroS(&pitot.zeroCalibration, &pitot.pressureZero);
DEBUG_TRACE_SYNC("Pitot calibration complete (%d)", lrintf(pitot.pressureZero));
}
}

4
src/main/sensors/pitotmeter.h
View file

@ -19,6 +19,7 @@
#include "config/parameter_group.h"
#include "common/filter.h"
#include "common/calibration.h"
#include "drivers/pitotmeter.h"
@ -46,11 +47,10 @@ typedef struct pito_s {
pitotDev_t dev;
float airSpeed;
zeroCalibrationScalar_t zeroCalibration;
pt1Filter_t lpfState;
timeUs_t lastMeasurementUs;
timeMs_t lastSeenHealthyMs;
timeMs_t calibrationTimeoutMs;
bool calibrationFinished;
float pressureZero;
float pressure;

7
src/main/sensors/sensors.h
View file

@ -39,8 +39,10 @@ typedef union flightDynamicsTrims_u {
flightDynamicsTrims_def_t values;
} flightDynamicsTrims_t;
#define CALIBRATING_GYRO_CYCLES 1000
#define CALIBRATING_ACC_CYCLES 400
#define CALIBRATING_BARO_TIME_MS 2000
#define CALIBRATING_PITOT_TIME_MS 2000
#define CALIBRATING_GYRO_TIME_MS 2000
#define CALIBRATING_ACC_TIME_MS 500
// These bits have to be aligned with sensorIndex_e
typedef enum {
@ -53,6 +55,7 @@ typedef enum {
SENSOR_OPFLOW = 1 << 6,
SENSOR_GPS = 1 << 7,
SENSOR_GPSMAG = 1 << 8,
SENSOR_TEMP = 1 << 9
} sensors_e;
extern uint8_t requestedSensors[SENSOR_INDEX_COUNT];

298
src/main/sensors/temperature.c
View file

@ -17,19 +17,23 @@
#include "stdbool.h"
#include "stdint.h"
#include <string.h>
#include "platform.h"
#include "build/debug.h"
#include "common/maths.h"
#include "common/printf.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/1-wire.h"
#include "drivers/logging.h"
#include "drivers/temperature/temperature.h"
#include "drivers/temperature/lm75.h"
#include "drivers/temperature/ds18b20.h"
#include "fc/runtime_config.h"
@ -38,54 +42,284 @@
#include "sensors/gyro.h"
#include "sensors/barometer.h"
static int16_t tempSensorValue[TEMP_COUNT];
static tempSensor_e tempSensorValid;
#include "scheduler/protothreads.h"
PG_REGISTER_ARRAY(tempSensorConfig_t, MAX_TEMP_SENSORS, tempSensorConfig, PG_TEMP_SENSOR_CONFIG, 0);
#define MPU_TEMP_VALID_BIT 0
#define BARO_TEMP_VALID_BIT 1
#define MPU_TEMP_VALID (mpuBaroTempValid & (1 << MPU_TEMP_VALID_BIT))
#define BARO_TEMP_VALID (mpuBaroTempValid & (1 << BARO_TEMP_VALID_BIT))
static uint16_t mpuTemperature, baroTemperature;
static uint8_t mpuBaroTempValid = 0;
#ifdef USE_TEMPERATURE_SENSOR
static int16_t tempSensorValue[MAX_TEMP_SENSORS];
static bool lm75Detected = false;
temperatureDev_t temperatureDev;
// Each bit corresponds to a sensor LSB = first sensor. 1 = valid value
static uint8_t sensorStatus[MAX_TEMP_SENSORS / 8 + (MAX_TEMP_SENSORS % 8 ? 1 : 0)];
#ifdef USE_TEMPERATURE_LM75
static temperatureDev_t lm75Dev[8];
#endif
#ifdef DS18B20_DRIVER_AVAILABLE
static owDev_t *owDev;
#endif
static void newSensorCheckAndEnter(uint8_t type, uint64_t addr)
{
int8_t foundConfigIndex = -1, firstFreeConfigSlot = -1;
// try to find sensor or free config slot
for (uint8_t configIndex = 0; configIndex < MAX_TEMP_SENSORS; ++configIndex) {
const tempSensorConfig_t *configSlot = tempSensorConfig(configIndex);
if ((configSlot->type == type) && (configSlot->address == addr)) {
foundConfigIndex = configIndex;
break;
}
if ((firstFreeConfigSlot == -1) && !configSlot->type) firstFreeConfigSlot = configIndex;
}
// if new sensor and have free config slot enter new sensor
if ((foundConfigIndex == -1) && (firstFreeConfigSlot != -1)) {
tempSensorConfig_t *configSlot = tempSensorConfigMutable(firstFreeConfigSlot);
configSlot->type = type;
configSlot->address = addr;
configSlot->label[0] = '\0';
configSlot->alarm_min = -200;
configSlot->alarm_max = 600;
}
}
void temperatureInit(void)
{
memset(sensorStatus, 0, sizeof(sensorStatus) * sizeof(*sensorStatus));
addBootlogEvent2(BOOT_EVENT_TEMP_SENSOR_DETECTION, BOOT_EVENT_FLAGS_NONE);
lm75Detected = lm75Detect(&temperatureDev);
}
sensorsSet(SENSOR_TEMP);
#ifdef USE_TEMPERATURE_LM75
memset(lm75Dev, 0, sizeof(lm75Dev));
for (uint8_t lm75Addr = 0; lm75Addr < 8; ++lm75Addr) {
if (lm75Detect(lm75Dev + lm75Addr, lm75Addr))
newSensorCheckAndEnter(TEMP_SENSOR_LM75, lm75Addr);
}
#endif
int16_t getTemperature(tempSensor_e sensor)
#ifdef DS18B20_DRIVER_AVAILABLE
owDev = getOwDev();
if (owDev) {
uint64_t ds18b20_rom_table[MAX_TEMP_SENSORS];
uint8_t ds18b20_rom_count = MAX_TEMP_SENSORS;
ds18b20Enumerate(owDev, ds18b20_rom_table, &ds18b20_rom_count);
for (uint8_t rom_index = 0; rom_index < ds18b20_rom_count; ++rom_index)
newSensorCheckAndEnter(TEMP_SENSOR_DS18B20, ds18b20_rom_table[rom_index]);
}
#endif
// configure sensors
for (uint8_t configIndex = 0; configIndex < MAX_TEMP_SENSORS; ++configIndex) {
const tempSensorConfig_t *configSlot = tempSensorConfig(configIndex);
switch (configSlot->type) {
#ifdef DS18B20_DRIVER_AVAILABLE
case TEMP_SENSOR_DS18B20:
if (owDev) {
tempSensorValue[configIndex] = TEMPERATURE_INVALID_VALUE;
ds18b20Configure(owDev, configSlot->address, DS18B20_CONFIG_9BIT);
}
break;
#endif
default:;
}
}
}
static bool temperatureSensorValueIsValid(uint8_t temperatureUpdateSensorIndex)
{
return tempSensorValue[sensor];
uint8_t mask = 1 << (temperatureUpdateSensorIndex % 8);
uint8_t byteIndex = temperatureUpdateSensorIndex / 8;
return sensorStatus[byteIndex] & mask;
}
float getCurrentTemperature(void)
{ //returns current temperature in degrees celsius
return tempSensorValue[tempSensorValid]/10.0f;
}
tempSensor_e getCurrentTemperatureSensorUsed(void) {
return tempSensorValid;
}
void temperatureUpdate(void)
// returns decidegrees centigrade
bool getSensorTemperature(uint8_t temperatureUpdateSensorIndex, int16_t *temperature)
{
// TEMP_GYRO: Update gyro temperature in decidegrees
*temperature = tempSensorValue[temperatureUpdateSensorIndex];
return temperatureSensorValueIsValid(temperatureUpdateSensorIndex);
}
// Converts 64bit integer address to hex format
// hex_address must be at least 17 bytes long (16 chars + NULL)
void tempSensorAddressToString(uint64_t address, char *hex_address)
{
if (address < 8)
tfp_sprintf(hex_address, "%d", address);
else {
uint32_t *address32 = (uint32_t *)&address;
tfp_sprintf(hex_address, "%08lx%08lx", address32[1], address32[0]);
}
}
// Converts address string in hex format to unsigned integer
// the hex_address parameter must be NULL or space terminated
bool tempSensorStringToAddress(const char *hex_address, uint64_t *address)
{
uint16_t char_count = 0;
*address = 0;
while (*hex_address && (*hex_address != ' ')) {
if (++char_count > 16) return false;
char byte = *hex_address++;
if (byte >= '0' && byte <= '9') byte = byte - '0';
else if (byte >= 'a' && byte <='f') byte = byte - 'a' + 10;
else if (byte >= 'A' && byte <='F') byte = byte - 'A' + 10;
else return false;
*address = (*address << 4) | (byte & 0xF);
}
return true;
}
#endif /* USE_TEMPERATURE_SENSOR */
#ifdef USE_TEMPERATURE_SENSOR
static uint8_t temperatureUpdateSensorIndex;
static bool temperatureUpdateValueValid;
#ifdef DS18B20_DRIVER_AVAILABLE
static uint8_t temperatureUpdateIndex;
static uint8_t temperatureUpdateBuf[9];
#endif
#endif /* defined(USE_TEMPERATURE_SENSOR) */
PROTOTHREAD(temperatureUpdate)
{
ptBegin(temperatureUpdate);
while (1) {
if (gyroReadTemperature()) {
tempSensorValue[TEMP_GYRO] = gyroGetTemperature();
tempSensorValid = TEMP_GYRO;
}
mpuTemperature = gyroGetTemperature();
mpuBaroTempValid |= (1 << MPU_TEMP_VALID_BIT);
} else
mpuBaroTempValid &= ~(1 << MPU_TEMP_VALID_BIT);
#if defined(USE_BARO)
// TEMP_BARO: Update baro temperature in decidegrees
if(sensors(SENSOR_BARO)){
tempSensorValue[TEMP_BARO] = baroGetTemperature();
tempSensorValid = TEMP_BARO;
}
#ifdef USE_BARO
if (sensors(SENSOR_BARO)) {
baroTemperature = baroGetTemperature();
mpuBaroTempValid |= (1 << BARO_TEMP_VALID_BIT);
} else
mpuBaroTempValid &= ~(1 << BARO_TEMP_VALID_BIT);
#endif
#ifdef USE_TEMPERATURE_SENSOR
if (lm75Detected && temperatureDev.read(&temperatureDev, &tempSensorValue[TEMP_LM75])) {
tempSensorValid = TEMP_LM75;
#ifdef USE_TEMPERATURE_SENSOR
temperatureUpdateSensorIndex = 0;
do {
const tempSensorConfig_t *configSlot = tempSensorConfig(temperatureUpdateSensorIndex);
temperatureUpdateValueValid = false;
#ifdef USE_TEMPERATURE_LM75
if (configSlot->type == TEMP_SENSOR_LM75) {
if (configSlot->address < 8) {
temperatureDev_t *dev = lm75Dev + configSlot->address;
if (dev->read && dev->read(dev, &tempSensorValue[temperatureUpdateSensorIndex])) temperatureUpdateValueValid = true;
}
#endif
}
#endif
#ifdef DS18B20_DRIVER_AVAILABLE
if ((configSlot->type == TEMP_SENSOR_DS18B20) && owDev) {
bool ack = owDev->owResetCommand(owDev);
if (!ack) goto temperatureUpdateError;
ptWait(owDev->owBusReady(owDev));
ack = owDev->owMatchRomCommand(owDev);
if (!ack) goto temperatureUpdateError;
ptWait(owDev->owBusReady(owDev));
temperatureUpdateIndex = 0;
do {
ack = owDev->owWriteByteCommand(owDev, ((uint8_t *)&tempSensorConfig(temperatureUpdateSensorIndex)->address)[temperatureUpdateIndex]);
if (!ack) goto temperatureUpdateError;
ptWait(owDev->owBusReady(owDev));
} while (++temperatureUpdateIndex < 8);
ack = ds18b20ReadScratchpadCommand(owDev);
if (!ack) goto temperatureUpdateError;
ptWait(owDev->owBusReady(owDev));
temperatureUpdateIndex = 0;
do {
ack = owDev->owReadByteCommand(owDev);
if (!ack) goto temperatureUpdateError;
ptWait(owDev->owBusReady(owDev));
ack = owDev->owReadByteResult(owDev, temperatureUpdateBuf + temperatureUpdateIndex);
if (!ack) goto temperatureUpdateError;
} while (++temperatureUpdateIndex < 9);
int16_t temperature;
if (ds18b20ReadTemperatureFromScratchPadBuf(temperatureUpdateBuf, &temperature)) {
if (temperatureSensorValueIsValid(temperatureUpdateSensorIndex) || (tempSensorValue[temperatureUpdateSensorIndex] == -1240)) {
tempSensorValue[temperatureUpdateSensorIndex] = temperature;
temperatureUpdateValueValid = true;
} else
tempSensorValue[temperatureUpdateSensorIndex] = -1240;
} else
tempSensorValue[temperatureUpdateSensorIndex] = TEMPERATURE_INVALID_VALUE;
}
temperatureUpdateError:;
#endif
uint8_t statusMask = 1 << (temperatureUpdateSensorIndex % 8);
uint8_t byteIndex = temperatureUpdateSensorIndex / 8;
if (temperatureUpdateValueValid)
sensorStatus[byteIndex] |= statusMask;
else
sensorStatus[byteIndex] &= ~statusMask;
ptYield();
} while (++temperatureUpdateSensorIndex < MAX_TEMP_SENSORS);
#ifdef DS18B20_DRIVER_AVAILABLE
if (owDev) ds18b20StartConversion(owDev);
#endif
#endif /* defined(USE_TEMPERATURE_SENSOR) */
ptDelayMs(100); // DS18B20 sensors take 94ms for a temperature conversion with 9bit resolution
}
ptEnd(0);
}
// returns decidegrees centigrade
bool getIMUTemperature(int16_t *temperature)
{
*temperature = mpuTemperature;
return MPU_TEMP_VALID;
}
// returns decidegrees centigrade
bool getBaroTemperature(int16_t *temperature)
{
*temperature = baroTemperature;
return BARO_TEMP_VALID;
}
void resetTempSensorConfig(void)
{
memset(tempSensorConfigMutable(0), 0, sizeof(tempSensorConfig_t) * MAX_TEMP_SENSORS);
}

38
src/main/sensors/temperature.h
View file

@ -19,21 +19,39 @@
#include "config/parameter_group.h"
#define TEMPERATURE_LABEL_LEN 4
#define MAX_TEMP_SENSORS 8
#define TEMPERATURE_INVALID_VALUE -1250
typedef enum {
TEMP_GYRO = 0,
TEMP_BARO = 1,
#ifdef USE_TEMPERATURE_SENSOR
TEMP_LM75 = 2,
#endif
TEMP_COUNT
} tempSensor_e;
TEMP_SENSOR_NONE = 0,
TEMP_SENSOR_LM75,
TEMP_SENSOR_DS18B20
} tempSensorType_e;
typedef struct {
tempSensorType_e type;
uint64_t address;
char label[TEMPERATURE_LABEL_LEN];
int16_t alarm_min;
int16_t alarm_max;
} tempSensorConfig_t;
PG_DECLARE_ARRAY(tempSensorConfig_t, MAX_TEMP_SENSORS, tempSensorConfig);
// Temperature is returned in degC*10
int16_t getTemperature(tempSensor_e sensor);
float getCurrentTemperature(void);
tempSensor_e getCurrentTemperatureSensorUsed(void);
bool getIMUTemperature(int16_t *temperature);
bool getBaroTemperature(int16_t *temperature);
void temperatureUpdate(void);
#ifdef USE_TEMPERATURE_SENSOR
void temperatureInit(void);
bool getSensorTemperature(uint8_t sensorIndex, int16_t *temperature);
void tempSensorAddressToString(uint64_t address, char *hex_address);
bool tempSensorStringToAddress(const char *hex_address, uint64_t *address);
#endif
void resetTempSensorConfig(void);

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

@ -74,6 +74,10 @@
#define USE_MAG_MAG3110
#define USE_MAG_LIS3MDL
#define USE_RANGEFINDER
#define USE_RANGEFINDER_HCSR04_I2C
#define RANGEFINDER_I2C_BUS BUS_I2C2
#define PITOT_I2C_BUS BUS_I2C2
#define TEMPERATURE_I2C_BUS BUS_I2C2

3
src/main/target/MATEKF411/target.h
View file

@ -75,6 +75,9 @@
#define UART2_RX_PIN PA3
#if defined(MATEKF411_SFTSRL2)
#define USE_SOFTSERIAL1
#define SOFTSERIAL_1_TX_PIN PA0 // ST1 pad
#define SOFTSERIAL_1_RX_PIN PA0
#define USE_SOFTSERIAL2
#define SOFTSERIAL_2_TX_PIN PA8 // LED pad
#define SOFTSERIAL_2_RX_PIN PA8

39
src/main/target/NOX/target.c Executable file
View file

@ -0,0 +1,39 @@
/*
* 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 <platform.h>
#include "drivers/io.h"
#include "drivers/dma.h"
#include "drivers/timer.h"
const timerHardware_t timerHardware[] = {
DEF_TIM(TIM2, CH3, PB10, TIM_USE_PPM, 0, 0), //PPM
DEF_TIM(TIM2, CH1, PA0, TIM_USE_LED, 0, 0), //2812LED
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT
DEF_TIM(TIM1, CH1, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT
DEF_TIM(TIM9, CH1, PA2, TIM_USE_ANY, 0, 0), //UART2 TX
DEF_TIM(TIM9, CH2, PA3, TIM_USE_ANY, 0, 0), //UART2 RX
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

147
src/main/target/NOX/target.h Executable file
View file

@ -0,0 +1,147 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "NOX1"
#define USBD_PRODUCT_STRING "NoxF4V1"
#define LED0 PA4
#define BEEPER PC13
#define BEEPER_INVERTED
#define I2C2_SCL NONE
#define I2C2_SDA NONE
// *************** SPI **********************
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PB3
#define SPI1_MISO_PIN PB4
#define SPI1_MOSI_PIN PB5
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
// *************** SPI Gyro & ACC **********************
#define MPU6000_CS_PIN PB12
#define MPU6000_SPI_BUS BUS_SPI2
#define USE_EXTI
#define GYRO_INT_EXTI PA8
#define USE_MPU_DATA_READY_SIGNAL
#define USE_GYRO
#define USE_GYRO_MPU6000
#define USE_ACC
#define USE_ACC_MPU6000
// *************** SPI BARO *****************************
#define USE_BARO
#define USE_BARO_BMP280
#define USE_BARO_SPI_BMP280
#define BMP280_SPI_BUS BUS_SPI2
#define BMP280_CS_PIN PA9
// *************** SPI OSD *****************************
#define USE_OSD
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI2
#define MAX7456_CS_PIN PA10
//#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2)
//#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST)
// *************** SPI FLASH **************************
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_CS_PIN PA15
#define M25P16_SPI_BUS BUS_SPI1
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
// *************** UART *****************************
#define USE_UART_INVERTER
#define USE_VCP
#define USE_UART1
#define UART1_TX_PIN PB6
#define UART1_RX_PIN PB7
#define USE_UART2
#define UART2_TX_PIN PA2
#define UART2_RX_PIN PA3
// #define INVERTER_PIN_UART2 PC14
#define INVERTER_PIN_UART2_RX PC14 // PC14 used as inverter select GPIO
#define USE_SOFTSERIAL1
#define SOFTSERIAL_1_TX_PIN PA2 // Workaround for softserial not initializing with only RX
#define SOFTSERIAL_1_RX_PIN PA2 // Backdoor timer on UART2_TX, used for ESC telemetry
#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART2, SOFTSERIAL1
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART2
// *************** ADC *****************************
#define USE_ADC
#define ADC_INSTANCE ADC1
#define ADC_CHANNEL_1_PIN NONE
#define ADC_CHANNEL_2_PIN PA5
#define ADC_CHANNEL_3_PIN NONE
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1
#define VBAT_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
/*
#define ADC_INSTANCE ADC1
#define ADC1_DMA_STREAM DMA2_Stream0
#define ADC_CHANNEL_1_PIN PA5
#define ADC_CHANNEL_2_PIN NONE
//#define ADC_CHANNEL_3_PIN PA0
#define VBAT_ADC_CHANNEL ADC_CHN_1
//#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
//#define RSSI_ADC_CHANNEL ADC_CHN_3
*/
// *************** LED2812 ************************
#define USE_LED_STRIP
#define WS2811_PIN PA0
#define WS2811_DMA_HANDLER_IDENTIFER DMA2_ST1_HANDLER
#define WS2811_DMA_STREAM DMA2_Stream1
#define WS2811_DMA_CHANNEL DMA_Channel_6
// *************** OTHERS *************************
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_SOFTSERIAL | FEATURE_VBAT)
// #define USE_SPEKTRUM_BIND
// #define BIND_PIN PA10 // RX1
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13)))
#define TARGET_IO_PORTB (0xffff & ~(BIT(2)|BIT(11)))
#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15))
#define MAX_PWM_OUTPUT_PORTS 4

10
src/main/target/NOX/target.mk Executable file
View file

@ -0,0 +1,10 @@
F411_TARGETS += $(TARGET)
FEATURES += VCP ONBOARDFLASH
TARGET_SRC = \
drivers/accgyro/accgyro_mpu6000.c \
drivers/barometer/barometer_bmp280.c \
drivers/light_ws2811strip.c \
drivers/flash_m25p16.c \
drivers/max7456.c

3
src/main/target/SPRACINGF3NEO/target.h
View file

@ -116,6 +116,9 @@
#undef USE_VTX_TRAMP // Disabled due to flash size
#undef USE_PWM_SERVO_DRIVER // Disabled due to RAM size
#undef USE_PITOT // Disabled due to RAM size
#undef USE_PITOT_ADC // Disabled due to RAM size
#define RTC6705_CS_PIN PF4
#define RTC6705_SPI_INSTANCE SPI3
#define RTC6705_POWER_PIN PC3

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

@ -83,8 +83,12 @@
#define USE_PITOT
#define USE_PITOT_MS4525
#define USE_1WIRE
#define USE_1WIRE_DS2482
#define USE_TEMPERATURE_SENSOR
#define USE_TEMPERATURE_LM75
#define USE_TEMPERATURE_DS18B20
#define USE_MSP_DISPLAYPORT
#define USE_DASHBOARD
@ -92,14 +96,15 @@
#define USE_OLED_UG2864
#define USE_PWM_DRIVER_PCA9685
#define USE_BOOTLOG
#define BOOTLOG_DESCRIPTIONS
#endif
#if (FLASH_SIZE > 128)
#define NAV_FIXED_WING_LANDING
#define USE_AUTOTUNE_FIXED_WING
#define USE_DEBUG_TRACE
#define USE_BOOTLOG
#define BOOTLOG_DESCRIPTIONS
#define USE_STATS
#define USE_GYRO_NOTCH_1
#define USE_GYRO_NOTCH_2

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

@ -186,6 +186,22 @@
#endif
#endif
/** 1-Wire IF **/
#ifdef USE_1WIRE
#if defined(TEMPERATURE_I2C_BUS) && !defined(DS2482_I2C_BUS)
#define DS2482_I2C_BUS TEMPERATURE_I2C_BUS
#endif
#if defined(USE_1WIRE_DS2482) && defined(DS2482_I2C_BUS)
BUSDEV_REGISTER_I2C(busdev_ds2482, DEVHW_DS2482, DS2482_I2C_BUS, 0x18, NONE, DEVFLAGS_USE_RAW_REGISTERS);
#endif
#endif
/** TEMP SENSORS **/
#if defined(TEMPERATURE_I2C_BUS) && !defined(LM75_I2C_BUS)
@ -193,7 +209,14 @@
#endif
#if defined(USE_TEMPERATURE_LM75) && defined(LM75_I2C_BUS)
BUSDEV_REGISTER_I2C(busdev_lm75, DEVHW_LM75, LM75_I2C_BUS, 0x48, NONE, DEVFLAGS_NONE);
BUSDEV_REGISTER_I2C(busdev_lm75_0, DEVHW_LM75_0, LM75_I2C_BUS, 0x48, NONE, DEVFLAGS_NONE);
BUSDEV_REGISTER_I2C(busdev_lm75_1, DEVHW_LM75_1, LM75_I2C_BUS, 0x49, NONE, DEVFLAGS_NONE);
BUSDEV_REGISTER_I2C(busdev_lm75_2, DEVHW_LM75_2, LM75_I2C_BUS, 0x4A, NONE, DEVFLAGS_NONE);
BUSDEV_REGISTER_I2C(busdev_lm75_3, DEVHW_LM75_3, LM75_I2C_BUS, 0x4B, NONE, DEVFLAGS_NONE);
BUSDEV_REGISTER_I2C(busdev_lm75_4, DEVHW_LM75_4, LM75_I2C_BUS, 0x4C, NONE, DEVFLAGS_NONE);
BUSDEV_REGISTER_I2C(busdev_lm75_5, DEVHW_LM75_5, LM75_I2C_BUS, 0x4D, NONE, DEVFLAGS_NONE);
BUSDEV_REGISTER_I2C(busdev_lm75_6, DEVHW_LM75_6, LM75_I2C_BUS, 0x4E, NONE, DEVFLAGS_NONE);
BUSDEV_REGISTER_I2C(busdev_lm75_7, DEVHW_LM75_7, LM75_I2C_BUS, 0x4F, NONE, DEVFLAGS_NONE);
#endif

198
src/main/telemetry/crsf.c
View file

@ -23,6 +23,7 @@
#if defined(USE_TELEMETRY) && defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
#include "build/atomic.h"
#include "build/build_config.h"
#include "build/version.h"
@ -31,11 +32,13 @@
#include "common/streambuf.h"
#include "common/time.h"
#include "common/utils.h"
#include "common/printf.h"
#include "config/feature.h"
#include "drivers/serial.h"
#include "drivers/time.h"
#include "drivers/nvic.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
@ -56,20 +59,75 @@
#include "telemetry/crsf.h"
#include "telemetry/telemetry.h"
#include "telemetry/msp_shared.h"
#define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz
#define CRSF_DEVICEINFO_VERSION 0x01
// According to TBS: "CRSF over serial should always use a sync byte at the beginning of each frame.
// To get better performance it's recommended to use the sync byte 0xC8 to get better performance"
//
// Digitalentity: Using frame address byte as a sync field looks somewhat hacky to me, but seems it's needed to get CRSF working properly
#define CRSF_TELEMETRY_SYNC_BYTE 0xC8
#define CRSF_DEVICEINFO_PARAMETER_COUNT 0
#define CRSF_MSP_BUFFER_SIZE 96
#define CRSF_MSP_LENGTH_OFFSET 1
static bool crsfTelemetryEnabled;
static uint8_t crsfCrc;
static bool crsfTelemetryEnabled;
static bool deviceInfoReplyPending;
static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
#if defined(USE_MSP_OVER_TELEMETRY)
typedef struct mspBuffer_s {
uint8_t bytes[CRSF_MSP_BUFFER_SIZE];
int len;
} mspBuffer_t;
static mspBuffer_t mspRxBuffer;
void initCrsfMspBuffer(void)
{
mspRxBuffer.len = 0;
}
bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength)
{
if (mspRxBuffer.len + CRSF_MSP_LENGTH_OFFSET + frameLength > CRSF_MSP_BUFFER_SIZE) {
return false;
} else {
uint8_t *p = mspRxBuffer.bytes + mspRxBuffer.len;
*p++ = frameLength;
memcpy(p, frameStart, frameLength);
mspRxBuffer.len += CRSF_MSP_LENGTH_OFFSET + frameLength;
return true;
}
}
bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
{
bool requestHandled = false;
if (!mspRxBuffer.len) {
return false;
}
int pos = 0;
while (true) {
const int mspFrameLength = mspRxBuffer.bytes[pos];
if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength)) {
requestHandled |= sendMspReply(payloadSize, responseFn);
}
pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
if (pos >= mspRxBuffer.len) {
mspRxBuffer.len = 0;
return requestHandled;
}
}
}
return requestHandled;
}
#endif
static void crsfInitializeFrame(sbuf_t *dst)
{
crsfCrc = 0;
@ -133,7 +191,7 @@ CRSF frame has the structure:
Device address: (uint8_t)
Frame length: length in bytes including Type (uint8_t)
Type: (uint8_t)
CRC: (uint8_t)
CRC: (uint8_t), crc of <Type> and <Payload>
*/
/*
@ -143,7 +201,7 @@ int32_t Latitude ( degree / 10`000`000 )
int32_t Longitude (degree / 10`000`000 )
uint16_t Groundspeed ( km/h / 10 )
uint16_t GPS heading ( degree / 100 )
uint16 Altitude ( meter ­ 1000m offset )
uint16 Altitude ( meter ­1000m offset )
uint8_t Satellites in use ( counter )
*/
void crsfFrameGps(sbuf_t *dst)
@ -245,6 +303,8 @@ void crsfFrameFlightMode(sbuf_t *dst)
}
if (FLIGHT_MODE(FAILSAFE_MODE)) {
flightMode = "!FS!";
} else if (ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) {
flightMode = "HRST";
} else if (FLIGHT_MODE(MANUAL_MODE)) {
flightMode = "MANU";
} else if (FLIGHT_MODE(NAV_RTH_MODE)) {
@ -255,7 +315,7 @@ void crsfFrameFlightMode(sbuf_t *dst)
flightMode = "3CRS";
} else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
flightMode = "CRS";
} else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
} else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
flightMode = "AH";
} else if (FLIGHT_MODE(NAV_WP_MODE)) {
flightMode = "WP";
@ -265,7 +325,7 @@ void crsfFrameFlightMode(sbuf_t *dst)
flightMode = "HOR";
}
#ifdef USE_GPS
} else if (feature(FEATURE_GPS) && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {
} else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {
flightMode = "WAIT"; // Waiting for GPS lock
#endif
} else if (isArmingDisabled()) {
@ -278,13 +338,77 @@ void crsfFrameFlightMode(sbuf_t *dst)
*lengthPtr = sbufPtr(dst) - lengthPtr;
}
/*
0x29 Device Info
Payload:
uint8_t Destination
uint8_t Origin
char[] Device Name ( Null terminated string )
uint32_t Null Bytes
uint32_t Null Bytes
uint32_t Null Bytes
uint8_t 255 (Max MSP Parameter)
uint8_t 0x01 (Parameter version 1)
*/
void crsfFrameDeviceInfo(sbuf_t *dst) {
char buff[30];
tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, TARGET_BOARD_IDENTIFIER);
uint8_t *lengthPtr = sbufPtr(dst);
sbufWriteU8(dst, 0);
crsfSerialize8(dst, CRSF_FRAMETYPE_DEVICE_INFO);
crsfSerialize8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
crsfSerializeData(dst, (const uint8_t*)buff, strlen(buff));
crsfSerialize8(dst, 0); // zero terminator for string
for (unsigned int ii=0; ii<12; ii++) {
crsfSerialize8(dst, 0x00);
}
crsfSerialize8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT);
crsfSerialize8(dst, CRSF_DEVICEINFO_VERSION);
*lengthPtr = sbufPtr(dst) - lengthPtr;
}
#define BV(x) (1 << (x)) // bit value
// schedule array to decide how often each type of frame is sent
#define CRSF_SCHEDULE_COUNT_MAX 5
typedef enum {
CRSF_FRAME_START_INDEX = 0,
CRSF_FRAME_ATTITUDE_INDEX = CRSF_FRAME_START_INDEX,
CRSF_FRAME_BATTERY_SENSOR_INDEX,
CRSF_FRAME_FLIGHT_MODE_INDEX,
CRSF_FRAME_GPS_INDEX,
CRSF_SCHEDULE_COUNT_MAX
} crsfFrameTypeIndex_e;
static uint8_t crsfScheduleCount;
static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
#if defined(USE_MSP_OVER_TELEMETRY)
static bool mspReplyPending;
void crsfScheduleMspResponse(void)
{
mspReplyPending = true;
}
void crsfSendMspResponse(uint8_t *payload)
{
sbuf_t crsfPayloadBuf;
sbuf_t *dst = &crsfPayloadBuf;
crsfInitializeFrame(dst);
sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
crsfSerialize8(dst, CRSF_FRAMETYPE_MSP_RESP);
crsfSerialize8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
crsfSerializeData(dst, (const uint8_t*)payload, CRSF_FRAME_TX_MSP_FRAME_SIZE);
crsfFinalize(dst);
}
#endif
static void processCrsf(void)
{
static uint8_t crsfScheduleIndex = 0;
@ -293,23 +417,23 @@ static void processCrsf(void)
sbuf_t crsfPayloadBuf;
sbuf_t *dst = &crsfPayloadBuf;
if (currentSchedule & BV(CRSF_FRAME_ATTITUDE)) {
if (currentSchedule & BV(CRSF_FRAME_ATTITUDE_INDEX)) {
crsfInitializeFrame(dst);
crsfFrameAttitude(dst);
crsfFinalize(dst);
}
if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR)) {
if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR_INDEX)) {
crsfInitializeFrame(dst);
crsfFrameBatterySensor(dst);
crsfFinalize(dst);
}
if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) {
if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE_INDEX)) {
crsfInitializeFrame(dst);
crsfFrameFlightMode(dst);
crsfFinalize(dst);
}
#ifdef USE_GPS
if (currentSchedule & BV(CRSF_FRAME_GPS)) {
if (currentSchedule & BV(CRSF_FRAME_GPS_INDEX)) {
crsfInitializeFrame(dst);
crsfFrameGps(dst);
crsfFinalize(dst);
@ -318,17 +442,28 @@ static void processCrsf(void)
crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
}
void crsfScheduleDeviceInfoResponse(void)
{
deviceInfoReplyPending = true;
}
void initCrsfTelemetry(void)
{
// check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
// and feature is enabled, if so, set CRSF telemetry enabled
crsfTelemetryEnabled = crsfRxIsActive();
deviceInfoReplyPending = false;
#if defined(USE_MSP_OVER_TELEMETRY)
mspReplyPending = false;
#endif
int index = 0;
crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE);
crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR);
crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE);
crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX);
crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX);
crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX);
if (feature(FEATURE_GPS)) {
crsfSchedule[index++] = BV(CRSF_FRAME_GPS);
crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX);
}
crsfScheduleCount = (uint8_t)index;
@ -354,8 +489,29 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
// in between the RX frames.
crsfRxSendTelemetryData();
// Send ad-hoc response frames as soon as possible
#if defined(USE_MSP_OVER_TELEMETRY)
if (mspReplyPending) {
mspReplyPending = handleCrsfMspFrameBuffer(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse);
crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
return;
}
#endif
if (deviceInfoReplyPending) {
sbuf_t crsfPayloadBuf;
sbuf_t *dst = &crsfPayloadBuf;
crsfInitializeFrame(dst);
crsfFrameDeviceInfo(dst);
crsfFinalize(dst);
deviceInfoReplyPending = false;
crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
return;
}
// Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
if (currentTimeUs >= crsfLastCycleTime + CRSF_CYCLETIME_US) {
// Spread out scheduled frames evenly so each frame is sent at the same frequency.
if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) {
crsfLastCycleTime = currentTimeUs;
processCrsf();
}
@ -369,17 +525,17 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
crsfInitializeFrame(sbuf);
switch (frameType) {
default:
case CRSF_FRAME_ATTITUDE:
case CRSF_FRAMETYPE_ATTITUDE:
crsfFrameAttitude(sbuf);
break;
case CRSF_FRAME_BATTERY_SENSOR:
case CRSF_FRAMETYPE_BATTERY_SENSOR:
crsfFrameBatterySensor(sbuf);
break;
case CRSF_FRAME_FLIGHT_MODE:
case CRSF_FRAMETYPE_FLIGHT_MODE:
crsfFrameFlightMode(sbuf);
break;
#if defined(USE_GPS)
case CRSF_FRAME_GPS:
case CRSF_FRAMETYPE_GPS:
crsfFrameGps(sbuf);
break;
#endif

17
src/main/telemetry/crsf.h
View file

@ -18,17 +18,18 @@
#pragma once
#include "common/time.h"
#include "rx/crsf.h"
typedef enum {
CRSF_FRAME_START = 0,
CRSF_FRAME_ATTITUDE = CRSF_FRAME_START,
CRSF_FRAME_BATTERY_SENSOR,
CRSF_FRAME_FLIGHT_MODE,
CRSF_FRAME_GPS
} crsfFrameType_e;
#define CRSF_MSP_RX_BUF_SIZE 128
#define CRSF_MSP_TX_BUF_SIZE 128
void initCrsfTelemetry(void);
bool checkCrsfTelemetryState(void);
void handleCrsfTelemetry(timeUs_t currentTimeUs);
void crsfScheduleDeviceInfoResponse(void);
void crsfScheduleMspResponse(void);
int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
#if defined(USE_MSP_OVER_TELEMETRY)
void initCrsfMspBuffer(void);
bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength);
#endif

13
src/main/telemetry/ibus_shared.c
View file

@ -141,15 +141,10 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
}
#endif
if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_TEMPERATURE) { //BARO_TEMP\GYRO_TEMP
if (sensors(SENSOR_BARO)) return sendIbusMeasurement2(address, (uint16_t) ((DEGREES_TO_CENTIDEGREES(getCurrentTemperature()) + 50) / 10 + IBUS_TEMPERATURE_OFFSET)); //int32_t
else {
/*
* There is no temperature data
* assuming ((DEGREES_TO_CENTIDEGREES(getCurrentTemperature()) + 50) / 10
* 0 degrees (no sensor) equals 50 / 10 = 5
*/
return sendIbusMeasurement2(address, (uint16_t) (5 + IBUS_TEMPERATURE_OFFSET)); //int16_t
}
int16_t temperature;
const bool temp_valid = sensors(SENSOR_BARO) ? getBaroTemperature(&temperature) : getIMUTemperature(&temperature);
if (!temp_valid || (temperature < -400)) temperature = -400; // Minimum reported temperature is -40°C
return sendIbusMeasurement2(address, (uint16_t)((temperature + 50) / 10 + IBUS_TEMPERATURE_OFFSET));
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_RPM) {
return sendIbusMeasurement2(address, (uint16_t) (rcCommand[THROTTLE]));
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_EXTERNAL_VOLTAGE) { //VBAT

4
src/main/telemetry/msp_shared.h
View file

@ -17,16 +17,12 @@ typedef struct mspPackage_s {
typedef union mspRxBuffer_u {
uint8_t smartPortMspRxBuffer[SMARTPORT_MSP_RX_BUF_SIZE];
#if 0
uint8_t crsfMspRxBuffer[CRSF_MSP_RX_BUF_SIZE];
#endif
} mspRxBuffer_t;
typedef union mspTxBuffer_u {
uint8_t smartPortMspTxBuffer[SMARTPORT_MSP_TX_BUF_SIZE];
#if 0
uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE];
#endif
} mspTxBuffer_t;
void initSharedMsp(void);

10
src/test/Makefile
View file

@ -105,6 +105,15 @@ $(OBJECT_DIR)/common/maths.o : \
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@
$(OBJECT_DIR)/common/calibration.o : \
$(USER_DIR)/common/calibration.c \
$(USER_DIR)/common/calibration.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/calibration.c -o $@
$(OBJECT_DIR)/common/bitarray.o : \
$(USER_DIR)/common/bitarray.c \
$(USER_DIR)/common/bitarray.h \
@ -639,6 +648,7 @@ $(OBJECT_DIR)/sensor_gyro_unittest.o : \
$(OBJECT_DIR)/sensor_gyro_unittest : \
$(OBJECT_DIR)/build/debug.o \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/common/calibration.o \
$(OBJECT_DIR)/common/filter.o \
$(OBJECT_DIR)/drivers/accgyro/accgyro_fake.o \
$(OBJECT_DIR)/sensors/gyro.o \

8
src/test/unit/maths_unittest.cc
View file

@ -128,28 +128,28 @@ void expectVectorsAreEqual(fpVector3_t *a, fpVector3_t *b)
TEST(MathsUnittest, TestRotateVectorWithNoAngle)
{
fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
fpVector3_t vector = { 1.0f, 0.0f, 0.0f};
fp_angles_t euler_angles = {.raw={0.0f, 0.0f, 0.0f}};
fpMat3_t rmat;
rotationMatrixFromAngles(&rmat, &euler_angles);
rotationMatrixRotateVector(&vector, &vector, &rmat);
fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
fpVector3_t expected_result = { 1.0f, 0.0f, 0.0f};
expectVectorsAreEqual(&vector, &expected_result);
}
TEST(MathsUnittest, TestRotateVectorAroundAxis)
{
// Rotate a vector <1, 0, 0> around an each axis x y and z.
fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
fpVector3_t vector = { 1.0f, 0.0f, 0.0f};
fp_angles_t euler_angles = {.raw={90.0f, 0.0f, 0.0f}};
fpMat3_t rmat;
rotationMatrixFromAngles(&rmat, &euler_angles);
rotationMatrixRotateVector(&vector, &vector, &rmat);
fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
fpVector3_t expected_result = { 1.0f, 0.0f, 0.0f};
expectVectorsAreEqual(&vector, &expected_result);
}

34
src/test/unit/sensor_gyro_unittest.cc
View file

@ -28,6 +28,7 @@ extern "C" {
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/calibration.h"
#include "common/utils.h"
#include "drivers/accgyro/accgyro_fake.h"
#include "drivers/logging_codes.h"
@ -37,17 +38,11 @@ extern "C" {
#include "sensors/acceleration.h"
#include "sensors/sensors.h"
typedef struct gyroCalibration_s {
int32_t g[XYZ_AXIS_COUNT];
stdev_t var[XYZ_AXIS_COUNT];
uint16_t calibratingG;
} gyroCalibration_t;
extern gyroCalibration_t gyroCalibration;
extern zeroCalibrationVector_t gyroCalibration;
extern gyroDev_t gyroDev0;
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware);
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold);
STATIC_UNIT_TESTED bool isCalibrationComplete(gyroCalibration_t *gyroCalibration);
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration);
}
#include "unittest_macros.h"
@ -81,7 +76,7 @@ TEST(SensorGyro, Read)
TEST(SensorGyro, Calibrate)
{
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
gyroStartCalibration();
gyroInit();
fakeGyroSet(5, 6, 7);
const bool read = gyroDev0.readFn(&gyroDev0);
@ -89,17 +84,16 @@ TEST(SensorGyro, Calibrate)
EXPECT_EQ(5, gyroDev0.gyroADCRaw[X]);
EXPECT_EQ(6, gyroDev0.gyroADCRaw[Y]);
EXPECT_EQ(7, gyroDev0.gyroADCRaw[Z]);
static const int gyroMovementCalibrationThreshold = 32;
gyroDev0.gyroZero[X] = 8;
gyroDev0.gyroZero[Y] = 9;
gyroDev0.gyroZero[Z] = 10;
performGyroCalibration(&gyroDev0, &gyroCalibration, gyroMovementCalibrationThreshold);
performGyroCalibration(&gyroDev0, &gyroCalibration);
EXPECT_EQ(0, gyroDev0.gyroZero[X]);
EXPECT_EQ(0, gyroDev0.gyroZero[Y]);
EXPECT_EQ(0, gyroDev0.gyroZero[Z]);
EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
while (!isCalibrationComplete(&gyroCalibration)) {
performGyroCalibration(&gyroDev0, &gyroCalibration, gyroMovementCalibrationThreshold);
EXPECT_EQ(false, gyroIsCalibrationComplete());
while (!gyroIsCalibrationComplete()) {
performGyroCalibration(&gyroDev0, &gyroCalibration);
}
EXPECT_EQ(5, gyroDev0.gyroZero[X]);
EXPECT_EQ(6, gyroDev0.gyroZero[Y]);
@ -108,16 +102,16 @@ TEST(SensorGyro, Calibrate)
TEST(SensorGyro, Update)
{
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
gyroStartCalibration();
EXPECT_EQ(false, gyroIsCalibrationComplete());
gyroInit();
fakeGyroSet(5, 6, 7);
gyroUpdate();
EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
while (!isCalibrationComplete(&gyroCalibration)) {
EXPECT_EQ(false, gyroIsCalibrationComplete());
while (!gyroIsCalibrationComplete()) {
gyroUpdate();
}
EXPECT_EQ(true, isCalibrationComplete(&gyroCalibration));
EXPECT_EQ(true, gyroIsCalibrationComplete());
EXPECT_EQ(5, gyroDev0.gyroZero[X]);
EXPECT_EQ(6, gyroDev0.gyroZero[Y]);
EXPECT_EQ(7, gyroDev0.gyroZero[Z]);
@ -140,7 +134,9 @@ TEST(SensorGyro, Update)
// STUBS
extern "C" {
static timeMs_t milliTime = 0;
timeMs_t millis(void) {return milliTime++;}
uint32_t micros(void) {return 0;}
void beeper(beeperMode_e) {}
uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE };