diff --git a/.travis.yml b/.travis.yml index 4229593d89d..979ccd80d23 100755 --- a/.travis.yml +++ b/.travis.yml @@ -5,6 +5,9 @@ env: - GOAL=targets-group-2 - GOAL=targets-group-3 - GOAL=targets-group-4 + - GOAL=targets-group-5 + - GOAL=targets-group-6 + - GOAL=targets-group-7 - GOAL=targets-group-rest # use new docker environment @@ -28,7 +31,7 @@ before_install: install: - ./install-toolchain.sh - - export TOOLCHAINPATH=$PWD/gcc-arm-none-eabi-7-2018-q2-update/bin + - export TOOLCHAINPATH=$PWD/gcc-arm-none-eabi-8-2018-q4-major/bin - export PATH=$TOOLCHAINPATH:$PATH before_script: @@ -41,7 +44,7 @@ script: ./.travis.sh cache: apt: true directories: - - $PWD/gcc-arm-none-eabi-7-2018-q2-update + - $PWD/gcc-arm-none-eabi-8-2018-q4-major notifications: #slack: inavflight:UWRoWFJ4cbbpHXT8HJJlAPXa @@ -51,4 +54,4 @@ notifications: - https://webhooks.gitter.im/e/34e795df229478ac3a3b on_success: always on_failure: always - on_start: never \ No newline at end of file + on_start: never diff --git a/Makefile b/Makefile index d67ec9676a7..99f4f3a79dc 100644 --- a/Makefile +++ b/Makefile @@ -119,10 +119,13 @@ $(error Unknown target MCU specified.) 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_2_TARGETS := SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO CLRACINGF4AIR CLRACINGF4AIRV2 BEEROTORF4 BETAFLIGHTF3 BETAFLIGHTF4 PIKOBLX +GROUP_3_TARGETS := OMNIBUS AIRBOTF4 BLUEJAYF4 OMNIBUSF4 OMNIBUSF4PRO 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 NOX -GROUP_OTHER_TARGETS := $(filter-out $(GROUP_1_TARGETS) $(GROUP_2_TARGETS) $(GROUP_3_TARGETS) $(GROUP_4_TARGETS), $(VALID_TARGETS)) +GROUP_5_TARGETS := ASGARD32F7 CHEBUZZF3 CLRACINGF4AIRV3 DALRCF405 DALRCF722DUAL DYSF4PROV2 F4BY FISHDRONEF4 FOXEERF405 FOXEERF722DUAL FRSKYF3 FRSKYF4 FURYF3 FURYF3_SPIFLASH FURYF4OSD +GROUP_6_TARGETS := MAMBAF405 OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS +GROUP_7_TARGETS := KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI KFC32F3_INAV KROOZX MATEKF411_RSSI MATEKF411_SFTSRL2 MATEKF722MINI MATEKF722SE MATEKF722_HEXSERVO +GROUP_OTHER_TARGETS := $(filter-out $(GROUP_1_TARGETS) $(GROUP_2_TARGETS) $(GROUP_3_TARGETS) $(GROUP_4_TARGETS) $(GROUP_5_TARGETS) $(GROUP_6_TARGETS) $(GROUP_7_TARGETS), $(VALID_TARGETS)) REVISION = $(shell git rev-parse --short HEAD) @@ -221,6 +224,7 @@ CFLAGS += $(ARCH_FLAGS) \ $(DEBUG_FLAGS) \ -std=gnu99 \ -Wall -Wextra -Wunsafe-loop-optimizations -Wdouble-promotion \ + -Wstrict-prototypes \ -Werror=switch \ -ffunction-sections \ -fdata-sections \ @@ -377,9 +381,18 @@ targets-group-2: $(GROUP_2_TARGETS) ## targets-group-3 : build some targets targets-group-3: $(GROUP_3_TARGETS) -## targets-group-3 : build some targets +## targets-group-4 : build some targets targets-group-4: $(GROUP_4_TARGETS) +## targets-group-5 : build some targets +targets-group-5: $(GROUP_5_TARGETS) + +## targets-group-6 : build some targets +targets-group-6: $(GROUP_6_TARGETS) + +## targets-group-7 : build some targets +targets-group-7: $(GROUP_7_TARGETS) + ## targets-group-rest: build the rest of the targets (not listed in group 1, 2 or 3) targets-group-rest: $(GROUP_OTHER_TARGETS) @@ -471,6 +484,9 @@ targets: $(V0) @echo "targets-group-2: $(GROUP_2_TARGETS)" $(V0) @echo "targets-group-3: $(GROUP_3_TARGETS)" $(V0) @echo "targets-group-4: $(GROUP_4_TARGETS)" + $(V0) @echo "targets-group-5: $(GROUP_5_TARGETS)" + $(V0) @echo "targets-group-6: $(GROUP_6_TARGETS)" + $(V0) @echo "targets-group-7: $(GROUP_7_TARGETS)" $(V0) @echo "targets-group-rest: $(GROUP_OTHER_TARGETS)" $(V0) @echo "Release targets: $(RELEASE_TARGETS)" diff --git a/README.md b/README.md index 53e69df53ee..73d8f3ce6ae 100644 --- a/README.md +++ b/README.md @@ -47,7 +47,10 @@ Users of FrSky Taranis X9 and Q X7 can use INAV Lua Telemetry screen created by See: https://github.com/iNavFlight/inav/blob/master/docs/Installation.md ## Documentation, support and learning resources - +* [Fixed Wing Guide](docs/INAV_Fixed_Wing_Setup_Guide.pdf) +* [Autolaunch Guide](docs/INAV_Autolaunch.pdf) +* [Modes Guide](docs/INAV_Modes.pdf) +* [Wing Tuning Masterclass](docs/INAV_Wing_Tuning_Masterclass.pdf) * [Official documentation](https://github.com/iNavFlight/inav/tree/master/docs) * [Official Wiki](https://github.com/iNavFlight/inav/wiki) * [INAV Official on Telegram](https://t.me/INAVFlight) diff --git a/docs/Board - Matek F411 Wing.md b/docs/Board - Matek F411 Wing.md index de1d4bae74d..13c26c95f15 100644 --- a/docs/Board - Matek F411 Wing.md +++ b/docs/Board - Matek F411 Wing.md @@ -21,8 +21,9 @@ ## 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_FD_SFTSRL` if you need the softserial to be full-duplex (TX = ST1 pad, RX = LED pad), at the cost of losing the LED output. * `MATEKF411_RSSI` if you want to have analog RSSI input on ST1 pad. SS1 TX will be available on the LED pad. +* `MATEKF411_SFTSRL2`if you want to use two softserials (TX only) at the same time. Eg. Smart Audio + S. Port, Smart Audio + LTM ## Where to buy: diff --git a/docs/Board - MatekF405.md b/docs/Board - MatekF405.md index 8f3fb8f9754..061d38359ac 100644 --- a/docs/Board - MatekF405.md +++ b/docs/Board - MatekF405.md @@ -107,4 +107,6 @@ Rcgroups Thread Matek F405: https://www.rcgroups.com/forums/showthread.php?28892 Rcgroups Thread Matek F405-AIO: https://www.rcgroups.com/forums/showthread.php?2912273-Matek-Flight-Controller-F405-AIO +This board doesn't have hardware inverters for UART pins. This page explains getting SmartPort telemetry working on F4 board: https://github.com/iNavFlight/inav/blob/master/docs/Telemetry.md + diff --git a/docs/Board - NOX.md b/docs/Board - NOX.md index a9553eabfa6..8910aa97464 100755 --- a/docs/Board - NOX.md +++ b/docs/Board - NOX.md @@ -43,4 +43,4 @@ ## 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) +* [Banggood](https:inavflight.com/shop/s/bg/1310419) diff --git a/docs/Board - Omnibus F4.md b/docs/Board - Omnibus F4.md index 80278ef9465..e2d0dc684fa 100644 --- a/docs/Board - Omnibus F4.md +++ b/docs/Board - Omnibus F4.md @@ -38,6 +38,11 @@ * PPM and UART6 can be used together when S.BUS jumper is removed (close to PPM/SBUS connector) * Uses target **OMNIBUSF4V3** +More target options: +* OMNIBUSF4V3_S6_SS: Softserial1 on S6 +* OMNIBUSF4V3_S5_S6_2SS: Softserial1 on S5 and Softserial2 on S6 +* OMNIBUSF4V3_S5S6_SS: Softserial1 on S5/RX and S6/TX + ### [Omnibus F4 v4/v5](https://inavflight.com/shop/p/OMNIBUSF4V5) * Switching voltage regulator - solves problem of overheating BEC diff --git a/docs/Buzzer.md b/docs/Buzzer.md index 72a01e51ee6..580c58ade14 100644 --- a/docs/Buzzer.md +++ b/docs/Buzzer.md @@ -43,6 +43,43 @@ Sequences: 14 DISARM_REPEAT 0, 100, 10 Stick held in disarm position (after pause) 15 ARMED 0, 245, 10, 5 Board is armed (after pause ; repeats until board is disarmed or throttle is increased) +## Controlling buzzer usage + +The usage of the buzzer can be controlled by the CLI `beeper` command. + +### List current usage + +``` +beeper +``` +### List all buzzer setting options + +``` +beeper list +``` +giving: + +``` +Available: RUNTIME_CALIBRATION HW_FAILURE RX_LOST RX_LOST_LANDING DISARMING ARMING ARMING_GPS_FIX BAT_CRIT_LOW BAT_LOW GPS_STATUS RX_SET ACTION_SUCCESS ACTION_FAIL READY_BEEP MULTI_BEEPS DISARM_REPEAT ARMED SYSTEM_INIT ON_USB LAUNCH_MODE CAM_CONNECTION_OPEN CAM_CONNECTION_CLOSED ALL PREFERED +``` + +The `beeper` command syntax follows that of the `feature` command; a minus (`-`) in front of a name disables that function. + +So to disable the beeper / buzzer when connected to USB (may enhance domestic harmony) + +``` +beeper -ON_USB +``` + +Now the `beeper` command will show: + +``` +# beeper +Disabled: ON_USB + +``` + +As with other CLI commands, the `save` command is needed to save the new settings. ## Types of buzzer supported @@ -54,7 +91,7 @@ Buzzers that need an analog or PWM signal do not work and will make clicking noi Examples of a known-working buzzers. * [Hcm1205x Miniature Buzzer 5v](http://www.rapidonline.com/Audio-Visual/Hcm1205x-Miniature-Buzzer-5v-35-0055) - * [5V Electromagnetic Active Buzzer Continuous Beep](http://www.banggood.com/10Pcs-5V-Electromagnetic-Active-Buzzer-Continuous-Beep-Continuously-p-943524.html) + * [5V Electromagnetic Active Buzzer Continuous Beep](https://inavflight.com/shop/s/bg/943524) * [Radio Shack Model: 273-074 PC-BOARD 12VDC (3-16v) 70DB PIEZO BUZZER](http://www.radioshack.com/pc-board-12vdc-70db-piezo-buzzer/2730074.html#.VIAtpzHF_Si) * [MultiComp MCKPX-G1205A-3700 TRANSDUCER, THRU-HOLE, 4V, 30MA](http://uk.farnell.com/multicomp/mckpx-g1205a-3700/transducer-thru-hole-4v-30ma/dp/2135914?CMP=i-bf9f-00001000) - * [3-24V Piezo Electronic Tone Buzzer Alarm 95DB](http://www.banggood.com/3-24V-Piezo-Electronic-Tone-Buzzer-Alarm-95DB-Continuous-Sound-p-919348.html) + * [3-24V Piezo Electronic Tone Buzzer Alarm 95DB](https://inavflight.com/shop/s/bg/919348) diff --git a/docs/Cli.md b/docs/Cli.md index 79d9f28205a..8984a33bf6a 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -65,6 +65,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | `1wire ` | passthrough 1wire to the specified esc | | `adjrange` | show/set adjustment ranges settings | | `aux` | show/set aux settings | +| `beeper` | show/set beeper (buzzer) usage (see docs/Buzzer.md) | | `mmix` | design custom motor mixer | | `smix` | design custom servo mixer | | `color` | configure colors | @@ -83,12 +84,44 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | `profile` | index (0 to 2) | | `rxrange` | configure rx channel ranges (end-points) | | `save` | save and reboot | +| `serial` | Configure serial ports | | `serialpassthrough `| where `id` is the zero based port index, `baud` is a standard baud rate, and mode is `rx`, `tx`, or both (`rxtx`) | | `set` | name=value or blank or * for list | | `status` | show system status | | `temp_sensor` | list or configure temperature sensor(s). See docs/Temperature sensors.md | +| `wp` | list or configure waypoints. See more in docs/Navigation.md section NAV WP | | `version` | | +### serial + +The syntax of the `serial` command is `serial `. + +A shorter form is also supported to enable and disable functions using `serial +n` and +`serial -n`, where n is the a serial function identifier. The following values are available: + +| Function | Identifier | +|-----------------------|---------------| +| MSP | 0 | +| GPS | 1 | +| TELEMETRY_FRSKY | 2 | +| TELEMETRY_HOTT | 3 | +| TELEMETRY_LTM | 4 | +| TELEMETRY_SMARTPORT | 5 | +| RX_SERIAL | 6 | +| BLACKBOX | 7 | +| TELEMETRY_MAVLINK | 8 | +| TELEMETRY_IBUS | 9 | +| RCDEVICE | 10 | +| VTX_SMARTAUDIO | 11 | +| VTX_TRAMP | 12 | +| UAV_INTERCONNECT | 13 | +| OPTICAL_FLOW | 14 | +| LOG | 15 | +| RANGEFINDER | 16 | +| VTX_FFPV | 17 | + +`serial` can also be used without any argument to print the current configuration of all the serial ports. + ## CLI Variable Reference | Variable Name | Default Value | Description | @@ -115,7 +148,6 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | 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 | | switch_disarm_delay | 250 | Delay before disarming when requested by switch (ms) [0-1000] | | small_angle | 25 | If the aircraft tilt angle exceed this value the copter will refuse to arm. | | reboot_character | 82 | Special character used to trigger reboot | @@ -129,7 +161,6 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | inav_auto_mag_decl | ON | Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. | | inav_gravity_cal_tolerance | 5 | Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. | | inav_use_gps_velned | ON | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. | -| inav_gps_delay | 200 | GPS position and velocity data usually arrive with a delay. This parameter defines this delay. Default (200) should be reasonable for most GPS receivers. | | inav_reset_altitude | FIRST_ARM | Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming) | | inav_reset_home | EACH_ARM | Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM | | inav_max_surface_altitude | 200 | Max allowed altitude for surface following mode. [cm] | @@ -146,12 +177,12 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | name | Empty string | Craft name | | nav_disarm_on_landing | OFF | If set to ON, iNav disarms the FC after landing | | nav_use_midthr_for_althold | OFF | If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude | -| nav_extra_arming_safety | ON | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured | -| nav_user_control_mode | ATTI | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - right stick controls attitude like in ANGLE mode; CRUISE - right stick controls velocity in forward and right direction. | +| nav_extra_arming_safety | ON | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used | +| nav_user_control_mode | ATTI | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. | | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | | nav_wp_radius | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius | | nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value [cm] | -| nav_auto_speed | 300 | Maximum velocity firmware is allowed in full auto modes (POSHOLD, RTH, WP) [cm/s] [Multirotor only] | +| nav_auto_speed | 300 | Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only] | | nav_auto_climb_rate | 500 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] | | nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] | | nav_manual_climb_rate | 200 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] | @@ -210,7 +241,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | frsky_vfas_precision | 0 | D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method | | frsky_pitch_roll | OFF | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data | | smartport_fuel_unit | MAH | S.Port telemetry only: Unit of the value sent with the `FUEL` ID (FrSky D-Series always sends precent). [PERCENT/MAH/MWH] | -| smartport_uart_unidir | OFF | S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details | +| telemetry_uart_unidir | OFF | S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details | | report_cell_voltage | OFF | S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON | | hott_alarm_sound_interval | 5 | Battery alarm delay in seconds for Hott telemetry | | smartport_fuel_unit | MAH | S.Port and D-Series telemetry: Unit of the value sent with the `FUEL` ID. [PERCENT/MAH/MWH] | @@ -274,6 +305,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | failsafe_lights | ON | Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. | | failsafe_lights_flash_period | 1000 | Time in milliseconds between two flashes when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled [40-65535]. | | failsafe_lights_flash_on_time | 100 | Flash lights ON time in milliseconds when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled. [20-65535]. | +| failsafe_mission | ON | If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode | | rx_min_usec | 885 | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc. | | rx_max_usec | 2115 | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of mid_rc. | | rx_nosignal_throttle | HOLD | Defines behavior of throttle channel after signal loss is detected and until `failsafe_procedure` kicks in. Possible values - `HOLD` and `DROP`. | @@ -298,10 +330,14 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | osd_dist_alarm | 1000 | Value above which to make the OSD distance from home indicator blink (meters) | | osd_alt_alarm | 100 | Value above which to make the OSD relative altitude indicator blink (meters) | | osd_neg_alt_alarm | 5 | Value bellow which (negative altitude) to make the OSD relative altitude indicator blink (meters) | +| osd_gforce_alarm | 5 | Value above which the OSD g force indicator will blink (g) | +| osd_gforce_axis_alarm_min | -5 | Value under which the OSD axis g force indicators will blink (g) | +| osd_gforce_axis_alarm_max | 5 | Value above which the OSD axis g force indicators will blink (g) | | osd_imu_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | | osd_imu_temp_alarm_max | 600 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | | osd_baro_temp_alarm_min | -200 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) | | osd_baro_temp_alarm_max | 600 | Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) | +| osd_current_alarm | 0 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. | | osd_estimations_wind_compensation | ON | Use wind estimation for remaining flight time/distance estimation | | osd_failsafe_switch_layout | OFF | If enabled the OSD automatically switches to the first layout during failsafe | | osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` | @@ -317,9 +353,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | accgain_z | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | | nav_mc_pos_z_p | 50 | P gain of altitude PID controller (Multirotor) | | nav_fw_pos_z_p | 50 | P gain of altitude PID controller (Fixedwing) | -| nav_mc_pos_z_i | 0 | I gain of altitude PID controller (Multirotor) | | nav_fw_pos_z_i | 0 | I gain of altitude PID controller (Fixedwing) | -| nav_mc_pos_z_d | 0 | D gain of altitude PID controller (Multirotor) | | nav_fw_pos_z_d | 0 | D gain of altitude PID controller (Fixedwing) | | nav_mc_vel_z_p | 100 | P gain of velocity PID controller | | nav_mc_vel_z_i | 50 | I gain of velocity PID controller | @@ -377,9 +411,11 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | fw_iterm_limit_stick_position | 0.5 | Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side | | fw_min_throttle_down_pitch | 0 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) | | gyro_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | +| gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | | acc_lpf_hz | 15 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | -| dterm_lpf_hz | 40 | | -| yaw_lpf_hz | 30 | | +| acc_lpf_type | BIQUAD | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | +| dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value | +| yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | | gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz). Currently experimental | | pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch/Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | | yaw_p_limit | 300 | | @@ -436,5 +472,17 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | nav_mc_braking_boost_speed_threshold | 150 | BOOST can be enabled when speed is above this value | | nav_mc_braking_boost_disengage_speed | 100 | BOOST will be disabled when speed goes below this value | | nav_mc_braking_bank_angle | 40 | max angle that MR is allowed to bank in BOOST mode | +| nav_mc_pos_deceleration_time | 120 | Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting | +| nav_mc_pos_expo | 10 | Expo for PosHold control | | osd_artificial_horizon_max_pitch | 20 | Max pitch, in degrees, for OSD artificial horizon | -| baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. | +| baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. | +| mc_airmode_type | STICK_CENTER | Defines the Airmode state handling type for Multirotors. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `mc_airmode_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER** mode. | +| mc_airmode_threshold | 1300 | Defines airmode THROTTLE activation threshold when `mc_airmode_type` **THROTTLE_THRESHOLD** is used | +| use_dterm_fir_filter | ON | Setting to **OFF** disabled extra filter on Dterm. **OFF** offers faster Dterm and better inflight performance with a cost of being more sensitive to gyro noise. Small and relatively clean multirotors (7 inches and below) are suggested to use **OFF** setting. If motors are getting too hot, switch back to **ON** | +| sim_ground_station_number | Empty string | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. | +| sim_transmit_interval | 60 | Text message transmission interval in seconds for SIM module. Minimum value: 10 | +| sim_transmit_flags | F | String specifying text message transmit condition flags for the SIM module. Flags can be given in any order. Empty string means the module only sends response messages. `A`: acceleration events, `T`: continuous transmission, `F`: continuous transmission in failsafe mode, `L`: continuous transmission when altitude is below `sim_low_altitude`, `G`: continuous transmission when GPS signal quality is low | +| acc_event_threshold_high | 0 | Acceleration threshold [cm/s/s] for impact / high g event text messages sent by SIM module. Acceleration values greater than 4 g can occur in fixed wing flight without an impact, so a setting of 4000 or greater is suggested. 0 = detection off. | +| acc_event_threshold_low | 0 | Acceleration threshold [cm/s/s] for low-g / freefall detection text messages sent by SIM module. A setting of less than 100 is suggested. Valid values: [0-900], 0 = detection off. | +| acc_event_threshold_neg_x | 0 | Acceleration threshold [cm/s/s] for backwards acceleration / fixed wing landing detection text messages sent by SIM module. Suggested value for fixed wing: 1100. 0 = detection off. | +| sim_low_altitude | 0 | Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`.| \ No newline at end of file diff --git a/docs/Display.md b/docs/Display.md index 3e8e1bd7a27..f65b6b88c64 100755 --- a/docs/Display.md +++ b/docs/Display.md @@ -36,13 +36,13 @@ before they work. Choose wisely! Links to displays: - * [banggood.com](http://www.banggood.com/0_96-Inch-4Pin-White-IIC-I2C-OLED-Display-Module-12864-LED-For-Arduino-p-958196.html) 0.96 Inch 4Pin White IIC I2C OLED Display Module 12864 LED For Arduino - * [banggood.com](http://www.banggood.com/0_96-Inch-4Pin-IIC-I2C-Blue-OLED-Display-Module-For-Arduino-p-969147.html) 0.96 Inch 4Pin IIC I2C Blue OLED Display Module For Arduino + * [banggood.com](https://inavflight.com/shop/s/bg/958196) 0.96 Inch 4Pin White IIC I2C OLED Display Module 12864 LED For Arduino + * [banggood.com](https://inavflight.com/shop/s/bg/969147) 0.96 Inch 4Pin IIC I2C Blue OLED Display Module For Arduino * [wide.hk](http://www.wide.hk/products.php?product=I2C-0.96%22-OLED-display-module-%28-compatible-Arduino-%29) I2C 0.96" OLED display module * [witespyquad.gostorego.com](http://witespyquad.gostorego.com/accessories/readytofly-1-oled-128x64-pid-tuning-display-i2c.html) ReadyToFlyQuads 1" OLED Display * [multiwiicopter.com](http://www.multiwiicopter.com/products/1-oled) PARIS 1" OLED 128x64 PID tuning screen AIR -The banggood.com display is the cheapest at the time fo writing and will correctly send I2C ACK signals. +The banggood.com display is the cheapest at the time of writing and will correctly send I2C ACK signals. #### Crius CO-16 diff --git a/docs/INAV_Autolaunch.pdf b/docs/INAV_Autolaunch.pdf new file mode 100644 index 00000000000..fc85a8b9bca Binary files /dev/null and b/docs/INAV_Autolaunch.pdf differ diff --git a/docs/INAV_Fixed_Wing_Setup_Guide.pdf b/docs/INAV_Fixed_Wing_Setup_Guide.pdf new file mode 100644 index 00000000000..4f86da95204 Binary files /dev/null and b/docs/INAV_Fixed_Wing_Setup_Guide.pdf differ diff --git a/docs/INAV_Modes.pdf b/docs/INAV_Modes.pdf new file mode 100644 index 00000000000..f176a95514c Binary files /dev/null and b/docs/INAV_Modes.pdf differ diff --git a/docs/INAV_Wing_Tuning_Masterclass.pdf b/docs/INAV_Wing_Tuning_Masterclass.pdf new file mode 100644 index 00000000000..d47924ea300 Binary files /dev/null and b/docs/INAV_Wing_Tuning_Masterclass.pdf differ diff --git a/docs/Inflight Adjustments.md b/docs/Inflight Adjustments.md index 593147ad917..7182d9dc385 100644 --- a/docs/Inflight Adjustments.md +++ b/docs/Inflight Adjustments.md @@ -21,15 +21,20 @@ Up to 4 RX channels can be used to make different adjustments at the same time. The adjustment a channel makes can be controlled by another channel. -The following adjustments can be made, in flight, as well as on the ground. +The following adjustments can be made in flight as well as on the ground. * RC Rate * RC Expo * Throttle Expo -* Roll & Pitch Rate -* Yaw Rate -* Pitch+Roll P I and D -* Yaw P I and D +* Pitch, Roll, Yaw Rates +* Pitch, Roll, Yaw PIDs +* Rate profile +* Manual rates +* FW cruise_throttle, pitch2thr, min_throttle_down_pitch_angle +* Board alignment +* Level PIDs +* PosHold, AltHold PIDs +* PIDs profile Example scenarios: Up to 4 3-position switches or pots can be used to adjust 4 different settings at the same time. @@ -117,7 +122,36 @@ this reason ensure that you define enough ranges to cover the range channel's us | 17 | PITCH_D | | 18 | ROLL_P | | 19 | ROLL_I | -| 20 | ROLL_D | +| 20 | RC_YAW_EXPO | +| 21 | MANUAL_RC_EXPO | +| 22 | MANUAL_RC_YAW_EXPO | +| 23 | MANUAL_PITCH_ROLL_RATE | +| 24 | MANUAL_ROLL_RATE | +| 25 | MANUAL_PITCH_RATE | +| 26 | MANUAL_YAW_RATE | +| 27 | NAV_FW_CRUISE_THROTTLE | +| 28 | NAV_FW_PITCH2THR | +| 29 | ROLL_BOARD_ALIGNMENT | +| 30 | PITCH_BOARD_ALIGNMENT | +| 31 | LEVEL_P | +| 32 | LEVEL_I | +| 33 | LEVEL_D | +| 34 | POS_XY_P | +| 35 | POS_XY_I | +| 36 | POS_XY_D | +| 37 | POS_Z_P | +| 38 | POS_Z_I | +| 39 | POS_Z_D | +| 40 | HEADING_P | +| 41 | VEL_XY_P | +| 42 | VEL_XY_I | +| 43 | VEL_XY_D | +| 44 | VEL_Z_P | +| 45 | VEL_Z_P | +| 46 | VEL_Z_I | +| 47 | VEL_Z_D | +| 48 | FW_MIN_THROTTLE_DOWN_PITCH_ANGLE | +| 49 | PROFILE | Switch between 3 rate profiles using a 3 position switch. | ## Examples diff --git a/docs/Mixer.md b/docs/Mixer.md index 604fafe5749..bd787265399 100644 --- a/docs/Mixer.md +++ b/docs/Mixer.md @@ -76,10 +76,18 @@ Each servo mixing rule has the following parameters: | 20 | RC channel 14 | Raw RC channel 14 | | 21 | RC channel 15 | Raw RC channel 15 | | 22 | RC channel 16 | Raw RC channel 16 | - +| 23 | Stabilized ROLL+ | Clipped between 0 and 1000 | +| 24 | Stabilized ROLL- | Clipped between -1000 and 0 | +| 25 | Stabilized PITCH+ | Clipped between 0 and 1000 | +| 26 | Stabilized PITCH- | Clipped between -1000 and 0 | +| 27 | Stabilized YAW+ | Clipped between 0 and 1000 | +| 28 | Stabilized YAW- | Clipped between -1000 and 0 | +| 29 | One | Constant value of 500 | The `smix reset` command removes all the existing motor mixing rules. The `smix` command is used to list, create or modify rules. To list the currently defined rules run the `smix` command without parameters. -To create or modify rules use the `smix` command with the following syntax: `smix `. `` is representing the index of the servo mixing rule to create or modify (integer). To disable a mixing rule set the weight to 0. +To create or modify rules use the `smix` command with the following syntax: `smix `. `` is representing the index of the servo mixing rule to create or modify (integer). To disable a mixing rule set the weight to 0. + +`logic_condition_id` default value is `-1` for rules that should be always executed. diff --git a/docs/Navigation.md b/docs/Navigation.md index c09d5dd65c6..f56ae804893 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -47,3 +47,29 @@ When deciding what altitude to maintain, RTH has 4 different modes of operation * 2 (NAV_RTH_CONST_ALT) - climb/descend to predefined altitude before heading home (*nav_rth_altitude* defined altitude above launch point (cm)) * 3 (NAV_RTH_MAX_ALT) - track maximum altitude of the whole flight, climb to that altitude prior to the return (*nav_rth_altitude* is ignored) * 4 (NAV_RTH_AT_LEAST_ALT) - same as 2 (NAV_RTH_CONST_ALT), but only climb, do not descend + +## CLI command `wp` to manage waypoints + +`wp` - List all waypoints. + +`wp load` - Load list of waypoints from EEPROM to FC. + +`wp ` - Set parameters of waypoint with index ``. + +Parameters: + + * `` - When 0 then waypoint is not used, when 1 then it is normal waypoint, when 4 then it is RTH. + + * `` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789). + + * `` - Longitude. + + * `` - Altitude in cm. + + * `` - For a "RTH waypoint" p1 > 0 alow landing. For a normal waypoint it means speed to this waypoint, it is taken into account only for multicopters and when > 50 and < nav_auto_speed. + + * `` - Last waypoint must have set `flag` to 165 (0xA5), otherwise 0. + +`wp save` - Checks list of waypoints and save from FC to EEPROM (warning: it also saves all unsaved CLI settings like normal `save`). + +`wp reset` - Resets the list, sets the waypoints number to 0 and mark it as invalid (but doesn't delete the waypoints). diff --git a/docs/Rssi.md b/docs/Rssi.md index 63a50f6330a..36ca9d07e2a 100644 --- a/docs/Rssi.md +++ b/docs/Rssi.md @@ -17,10 +17,11 @@ e.g. if you used channel 9 then you would set: ``` set rssi_channel = 9 ``` -Note: Some systems such as EZUHF invert the RSSI ( 0 = Full signal / 100 = Lost signal). To correct this problem you can invert the channel input so you will get a correct reading by using command: +Note: Some systems such as EZUHF invert the RSSI ( 0 = Full signal / 100 = Lost signal). To correct this problem you can invert the RSSI scale so you will get a correct reading by using these commands: ``` -set rssi_ppm_invert = 1 +set rssi_min = 100 +set rssi_max = 0 ``` Default is set to "0" for normal operation ( 100 = Full signal / 0 = Lost signal). diff --git a/docs/Rx.md b/docs/Rx.md index 3436037b45f..8ffef40982b 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -35,6 +35,8 @@ http://www.frsky-rc.com/product/pro.php?pro_id=21 ## Serial Receivers +*Connect the receivers to UARTs and not to Software Serial ports. Using software serial for RX input can cause unexpected behaviours beacause the port cannot handle reliably the bit rate needed by the most common protocols* + ### Spektrum 8 channels via serial currently supported. diff --git a/docs/Serial.md b/docs/Serial.md index 60d382b497e..4f21b1f68b6 100644 --- a/docs/Serial.md +++ b/docs/Serial.md @@ -24,8 +24,8 @@ When selecting a USB to UART converter choose one that has DTR exposed as well a Examples: - * [FT232RL FTDI USB To TTL Serial Converter Adapter](http://www.banggood.com/FT232RL-FTDI-USB-To-TTL-Serial-Converter-Adapter-Module-For-Arduino-p-917226.html) - * [USB To TTL / COM Converter Module buildin-in CP2102](http://www.banggood.com/Wholesale-USB-To-TTL-Or-COM-Converter-Module-Buildin-in-CP2102-New-p-27989.html) + * [FT232RL FTDI USB To TTL Serial Converter Adapter](https://inavflight.com/shop/s/bg/917226) + * [USB To TTL / COM Converter Module buildin-in CP2102](https://inavflight.com/shop/s/bg/27989) Both SoftSerial and UART ports can be connected to your computer via USB to UART converter boards. @@ -59,11 +59,11 @@ You can use the CLI for configuration but the commands are reserved for develope The `serial` CLI command takes 6 arguments. 1. Identifier -1. Function bitmask (see serialPortFunction_e in the source) -1. MSP baud rate -1. GPS baud rate -1. Telemetry baud rate (auto baud allowed) -1. Blackbox baud rate +2. Function bitmask (see serialPortFunction_e in the source) +3. MSP baud rate +4. GPS baud rate +5. Telemetry baud rate (auto baud allowed) +6. Blackbox baud rate ### Baud Rates diff --git a/docs/Telemetry.md b/docs/Telemetry.md index 768224dd7f2..e09a9bab58c 100644 --- a/docs/Telemetry.md +++ b/docs/Telemetry.md @@ -40,7 +40,7 @@ Only TX serial pin has to be connected to Smartport receiver. Disable `telemetry ``` set telemetry_inverted = OFF -set smartport_uart_unidir = OFF +set telemetry_uart_unidir = OFF ``` ### Receiver univerted hack @@ -49,7 +49,7 @@ Some receivers (X4R, XSR and so on) can be hacked to get _uninverted_ Smartport ``` set telemetry_inverted = ON -set smartport_uart_unidir = OFF +set telemetry_uart_unidir = OFF ``` ### Software Serial @@ -86,7 +86,7 @@ It is possible to use DIY UART inverter to connect SmartPort receivers to F1 and When external inverter is used, following configuration has to be applied: ``` -set smartport_uart_unidir = ON +set telemetry_uart_unidir = ON set telemetry_inverted = ON ``` @@ -176,6 +176,9 @@ Use the latest Graupner firmware for your transmitter and receiver. Older HoTT transmitters required the EAM and GPS modules to be enabled in the telemetry menu of the transmitter. (e.g. on MX-20) +You can use a single connection, connect HoTT RX/TX only to serial TX, leave serial RX open and make sure the setting `telemetry_uart_unidir` is OFF. + +The following information is deprecated, use only for compatibility: Serial ports use two wires but HoTT uses a single wire so some electronics are required so that the signals don't get mixed up. The TX and RX pins of a serial port should be connected using a diode and a single wire to the `T` port on a HoTT receiver. @@ -192,6 +195,8 @@ The diode should be arranged to allow the data signals to flow the right way 1N4148 diodes have been tested and work with the GR-24. +When using the diode enable `telemetry_uart_unidir`, go to CLI and type `set telemetry_uart_unidir = ON`, don't forget a `save` afterwards. + As noticed by Skrebber the GR-12 (and probably GR-16/24, too) are based on a PIC 24FJ64GA-002, which has 5V tolerant digital pins. Note: The SoftSerial ports may not be 5V tolerant on your board. Verify if you require a 5v/3.3v level shifters. @@ -243,6 +248,26 @@ for PX4, PIXHAWK, APM and Parrot AR.Drone platforms. MAVLink implementation in INAV is transmit-only and usable on low baud rates and can be used over soft serial. + +## Cellular telemetry via text messages + +INAV can use a SimCom SIM800 series cellular module to provide telemetry via text messages. Telemetry messages can be requested by calling the module's number or sending it a text message. The module can be set to transmit messages at regular intervals, or when an acceleration event is detected. A text message command can be used to put the flight controller into RTH mode. + +The telemetry message looks like this: +``` +12.34V 2.0A ALT:5 SPD:10/13.6 DIS:78/19833 COG:16 SAT:21 SIG:9 ANG maps.google.com/?q=@50.3812711,20.1440378 +``` +giving battery voltage, current, altitude (m), speed / average speed (m/s), distance to home / total traveled distance (m), course over ground (degrees), number of satellites, cellular signal strength, flight mode and GPS coordinates as a Google Maps link. `SIG` has a range of 0 -- 31, with a value of 10 or higher indicating a usable signal quality. + +Transmission at regular intervals can be set by giving a string of flags in the CLI variable `sim_transmit_flags`: `T` - transmit continuously, `F` - transmit in failsafe mode, `A` - transmit when altitude is lower than `sim_low_altitude`, `G` - transmit when GPS signal quality is low. `A` only transmits in ALT HOLD, WAYPOINT, RTH, and FAILSAFE flight modes. The transmission interval is given by `sim_transmit_interval` and is 60 seconds by default. + +Text messages sent to the module can be used to set the transmission flags during flight, or to issue a RTH command to the flight controller. If a message begins with `RTH` it toggles forced RTH on / off, otherwise it is taken as a value for `sim_transmit_flags`. Note that an empty message turns transmission off, setting all flags to zero. + +Acceleration events are indicated at the beginning of the message as follows: `HIT!` indicates impact / high g event, `HIT` indicates landing / backwards acceleration event, `DROP` indicates freefall / low g event. + +To receive acceleration event messages, set one or more of the acceleration event threshold CLI variables to a nonzero value, and use the `A` flag in `sim_transmit_flags`. `acc_event_threshold_high` is the threshold (in cm/s/s) for impact detection by high magnitude of acceleration. `acc_event_threshold_low` is the threshold for freefall detection by low magnitude of acceleration. `acc_event_threshold_neg_x` is the threshold for landing detection (for fixed wing models) by high magnitude of negative x axis acceleration. + + ## Ibus telemetry Ibus telemetry requires a single connection from the TX pin of a bidirectional serial port to the Ibus sens pin on an FlySky telemetry receiver. (tested with fs-iA6B receiver, iA10 should work) diff --git a/docs/Temperature sensors.md b/docs/Temperature sensors.md index b63a980a9ca..4bd272c9402 100644 --- a/docs/Temperature sensors.md +++ b/docs/Temperature sensors.md @@ -35,7 +35,7 @@ The `temp_sensor` CLI command can be used to display and change the temperature * `address` is the address of the device on the bus. 0 to 7 for a LM75 or the full 64bit ROM in hex format for a 18B20 * `alarm_min` is the temperature under which the corresponding OSD element will start blinking (decidegrees centigrade) * `alarm_max` is the temperature above which the corresponding OSD element will start blinking (decidegrees centigrade) -* `osd_symbol` is the ID of a symbol to display on the OSD to the left of the temperature value. Use 0 to display a label instea (see next parameter). See the table bellow for the available IDs +* `osd_symbol` is the ID of a symbol to display on the OSD to the left of the temperature value. Use 0 to display a label instead (see next parameter). See the table bellow for the available IDs * `label` is a 4 characters maximum label that is displayed on the OSD next to the temperature value | Symbol ID | Description | diff --git a/CODE_OF_CONDUCT.md b/docs/policies/CODE_OF_CONDUCT.md similarity index 100% rename from CODE_OF_CONDUCT.md rename to docs/policies/CODE_OF_CONDUCT.md diff --git a/CONTRIBUTING.md b/docs/policies/CONTRIBUTING.md similarity index 100% rename from CONTRIBUTING.md rename to docs/policies/CONTRIBUTING.md diff --git a/docs/policies/NEW_HARDWARE_POLICY.md b/docs/policies/NEW_HARDWARE_POLICY.md new file mode 100644 index 00000000000..6d38333a60e --- /dev/null +++ b/docs/policies/NEW_HARDWARE_POLICY.md @@ -0,0 +1,49 @@ +# INAV New Hardware policies + +## General + +To prevent explosive growth of different target and feature count and ensure reasonable quality and usability of the firmware the following policy is mandatory for all new hardware additions (including variants of the same target). + +## Definitions + +"Target" - part of the source code responsible for providing a new build artifact (hex-file). + +"Hardware" - physical hardware requiring support from firmware side. + +## New target additions + +New targets are accepted into INAV code if any of the following conditions is satisfied: + +1. Board manufacturer provides specs, schematics and production samples are provided to at least two core developers. In this case the new target becomes part of official INAV release. + +2. Community member or board manufacturer provides board samples to at least one core developer and the target already exists in official Cleanflight or Betaflight firmware. In this case the new target may or may not become part of official release based on the decision made by core developers. + +# New hardware support + +For the hardware which does not require a new target, but still require support from the firmware side the following rules apply: + +1. Hardware manufacturer provides specs and production samples for the unsupported hardware to at least two core developers. + +2. Community member or hardware manufacturer provides hardware samples to at least one core developer and the firmware support already exists in official Cleanflight or Betaflight firmware. + +# Using own hardware exception + +If one of the core developers has the hardware in possession they may opt in and submit support for it anyway. In this case the support is not official and is generally not included in official releases. + +# Providing samples to developers + +1. Hardware provided to the developers would be considered a donation to the INAV project. Under no circumstances developer will be obliged to return the hardware. + +2. Manufacturer or community member providing the hardware bears all the costs of the hardware, delivery and VAT/customs duties. Hardware manufacturer also assumes the responsibility to provide up to date specs, documentation and firmware (if applicable). + +3. Before sending samples the providing party should reach out to developers and agree on specific terms of implementing support for the unsupported hardware. Developers may place additional requirements on a case by case basis and at their sole discretion. + +4. The new target and new hardware policies do not apply in the following cases. Developers may still chose to apply the "own hardware exception" at their own discretion. + + * if the receiving developer has to bear part of the costs (and is not reimbursed by the sender) + * if the hardware provided does not behave according to specs (different communication protocol, undocumented or incorrect CPU pin mappings, damaged or dead on arrival) and the sender doesn't provide a way to resolve the problem (up to date docs, new firmware etc). + * if the hardware was sent without reaching out to developers and agreeing on the terms first. + +5. It's advised to provide more than one sample to avoid issues with damaged or dead on arrival hardware. + + diff --git a/install-toolchain.sh b/install-toolchain.sh index d3bda40c275..58f24a85c13 100755 --- a/install-toolchain.sh +++ b/install-toolchain.sh @@ -1,5 +1,5 @@ #!/bin/bash -if [ ! -d $PWD/gcc-arm-none-eabi-7-2018-q2-update/bin ] ; then - curl --retry 10 --retry-max-time 120 -L "https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/7-2018q2/gcc-arm-none-eabi-7-2018-q2-update-linux.tar.bz2" | tar xfj - +if [ ! -d $PWD/gcc-arm-none-eabi-8-2018-q4-major/bin ] ; then + curl --retry 10 --retry-max-time 120 -L "https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/8-2018q4/gcc-arm-none-eabi-8-2018-q4-major-linux.tar.bz2" | tar xfj - fi diff --git a/make/release.mk b/make/release.mk index 19e6cad3355..248dac14116 100644 --- a/make/release.mk +++ b/make/release.mk @@ -6,15 +6,16 @@ RELEASE_TARGETS += ALIENFLIGHTNGF7 RELEASE_TARGETS += BETAFLIGHTF3 BETAFLIGHTF4 RELEASE_TARGETS += FF_F35_LIGHTNING FF_FORTINIF4 -RELEASE_TARGETS += KAKUTEF4 KAKUTEF4V2 KAKUTEF7 +RELEASE_TARGETS += KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI -RELEASE_TARGETS += SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF3NEO SPRACINGF4EVO +RELEASE_TARGETS += SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO SPRACINGF7DUAL RELEASE_TARGETS += OMNIBUS AIRBOTF4 ASGARD32F4 ASGARD32F7 FIREWORKSV2 -RELEASE_TARGETS += OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4V3 OMNIBUSF4PRO_LEDSTRIPM5 DYSF4PRO DYSF4PROV2 +RELEASE_TARGETS += OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 DYSF4PRO DYSF4PROV2 +RELEASE_TARGETS += OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS RELEASE_TARGETS += OMNIBUSF7 OMNIBUSF7V2 OMNIBUSF7NXT YUPIF7 -RELEASE_TARGETS += MATEKF405 MATEKF405_SERVOS6 MATEKF405OSD MATEKF722 MATEKF722_HEXSERVO MATEKF722SE MATEKF722MINI MATEKF405SE MATEKF411 MATEKF411_SFTSRL2 MATEKF411_RSSI +RELEASE_TARGETS += MATEKF405 MATEKF405_SERVOS6 MATEKF405OSD MATEKF722 MATEKF722_HEXSERVO MATEKF722SE MATEKF722MINI MATEKF405SE MATEKF411 MATEKF411_SFTSRL2 MATEKF411_FD_SFTSRL MATEKF411_RSSI RELEASE_TARGETS += FOXEERF405 FOXEERF722DUAL @@ -23,3 +24,5 @@ RELEASE_TARGETS += SPEEDYBEEF4 FRSKYF3 FRSKYF4 RELEASE_TARGETS += NOX WINGFC RELEASE_TARGETS += OMNIBUSF4V6 + +RELEASE_TARGETS += MAMBAF405 diff --git a/make/source.mk b/make/source.mk index 5756257f2bc..a130f5e27e6 100644 --- a/make/source.mk +++ b/make/source.mk @@ -7,11 +7,15 @@ COMMON_SRC = \ build/debug.c \ build/version.c \ common/bitarray.c \ + common/calibration.c \ + common/colorconversion.c \ common/crc.c \ common/encoding.c \ common/filter.c \ + common/gps_conversion.c \ + common/log.c \ + common/logic_condition.c \ common/maths.c \ - common/calibration.c \ common/memory.c \ common/olc.c \ common/printf.c \ @@ -35,12 +39,12 @@ COMMON_SRC = \ drivers/io.c \ drivers/io_pca9685.c \ drivers/light_led.c \ - drivers/logging.c \ drivers/resource.c \ drivers/rx_nrf24l01.c \ drivers/rx_spi.c \ drivers/rx_xn297.c \ drivers/pitotmeter_adc.c \ + drivers/pitotmeter_virtual.c \ drivers/pwm_esc_detect.c \ drivers/pwm_mapping.c \ drivers/pwm_output.c \ @@ -89,6 +93,7 @@ COMMON_SRC = \ io/beeper.c \ io/lights.c \ io/pwmdriver_i2c.c \ + io/esc_serialshot.c \ io/piniobox.c \ io/serial.c \ io/serial_4way.c \ @@ -98,11 +103,13 @@ COMMON_SRC = \ io/rcdevice.c \ io/rcdevice_cam.c \ msp/msp_serial.c \ + rx/crsf.c \ + rx/eleres.c \ rx/fport.c \ rx/ibus.c \ rx/jetiexbus.c \ rx/msp.c \ - rx/uib_rx.c \ + rx/msp_override.c \ rx/nrf24_cx10.c \ rx/nrf24_inav.c \ rx/nrf24_h8_3d.c \ @@ -111,14 +118,13 @@ COMMON_SRC = \ rx/pwm.c \ rx/rx.c \ rx/rx_spi.c \ - rx/crsf.c \ rx/sbus.c \ rx/sbus_channels.c \ rx/spektrum.c \ rx/sumd.c \ rx/sumh.c \ + rx/uib_rx.c \ rx/xbus.c \ - rx/eleres.c \ scheduler/scheduler.c \ sensors/acceleration.c \ sensors/battery.c \ @@ -140,13 +146,13 @@ COMMON_SRC = \ cms/cms_menu_imu.c \ cms/cms_menu_ledstrip.c \ cms/cms_menu_misc.c \ + cms/cms_menu_mixer_servo.c \ cms/cms_menu_navigation.c \ cms/cms_menu_osd.c \ + cms/cms_menu_saveexit.c \ cms/cms_menu_vtx_smartaudio.c \ cms/cms_menu_vtx_tramp.c \ cms/cms_menu_vtx_ffpv.c \ - common/colorconversion.c \ - common/gps_conversion.c \ drivers/display_ug2864hsweg01.c \ drivers/rangefinder/rangefinder_hcsr04.c \ drivers/rangefinder/rangefinder_hcsr04_i2c.c \ @@ -164,11 +170,13 @@ COMMON_SRC = \ io/displayport_max7456.c \ io/displayport_msp.c \ io/displayport_oled.c \ + io/displayport_hott.c \ io/gps.c \ io/gps_ublox.c \ io/gps_nmea.c \ io/gps_naza.c \ io/ledstrip.c \ + io/osd_hud.c \ io/osd.c \ navigation/navigation.c \ navigation/navigation_fixedwing.c \ @@ -192,6 +200,7 @@ COMMON_SRC = \ telemetry/mavlink.c \ telemetry/msp_shared.c \ telemetry/smartport.c \ + telemetry/sim.c \ telemetry/telemetry.c \ io/vtx.c \ io/vtx_string.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f60f48a64d8..84f3470d4b9 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -65,6 +65,7 @@ #include "navigation/navigation.h" #include "rx/rx.h" +#include "rx/msp_override.h" #include "sensors/diagnostics.h" #include "sensors/acceleration.h" @@ -217,6 +218,9 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"mcVelAxisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, {"mcVelAxisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, {"mcVelAxisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcVelAxisFF", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcVelAxisFF", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcVelAxisFF", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, {"mcVelAxisOut",0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, {"mcVelAxisOut",1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, {"mcVelAxisOut",2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, @@ -285,8 +289,15 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"motor", 6, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_7)}, {"motor", 7, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)}, - /* Tricopter tail servo */ - {"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(TRICOPTER)}, + /* servos */ + {"servo", 0, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)}, + {"servo", 1, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)}, + {"servo", 2, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)}, + {"servo", 3, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)}, + {"servo", 4, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)}, + {"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)}, + {"servo", 6, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)}, + {"servo", 7, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)}, #ifdef NAV_BLACKBOX {"navState", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, @@ -350,9 +361,12 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { {"hwHealthStatus", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"powerSupplyImpedance", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"sagCompensatedVBat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"wind", 0, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"wind", 1, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"wind", 2, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"wind", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"wind", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"wind", 2, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + {"mspOverrideFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, +#endif {"IMUTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, #ifdef USE_BARO {"baroTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, @@ -396,7 +410,7 @@ typedef struct blackboxMainState_s { int32_t axisPID_Setpoint[XYZ_AXIS_COUNT]; int32_t mcPosAxisP[XYZ_AXIS_COUNT]; - int32_t mcVelAxisPID[3][XYZ_AXIS_COUNT]; + int32_t mcVelAxisPID[4][XYZ_AXIS_COUNT]; int32_t mcVelAxisOutput[XYZ_AXIS_COUNT]; int32_t mcSurfacePID[3]; @@ -465,6 +479,9 @@ typedef struct blackboxSlowState_s { uint16_t powerSupplyImpedance; uint16_t sagCompensatedVBat; int16_t wind[XYZ_AXIS_COUNT]; +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + uint16_t mspOverrideFlags; +#endif int16_t imuTemperature; #ifdef USE_BARO int16_t baroTemperature; @@ -517,10 +534,10 @@ static blackboxGpsState_t gpsHistory; static blackboxSlowState_t slowHistory; // Keep a history of length 2, plus a buffer for MW to store the new values into -static blackboxMainState_t blackboxHistoryRing[3]; +static EXTENDED_FASTRAM blackboxMainState_t blackboxHistoryRing[3]; // These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old) -static blackboxMainState_t* blackboxHistory[3]; +static EXTENDED_FASTRAM blackboxMainState_t* blackboxHistory[3]; static bool blackboxModeActivationConditionPresent = false; @@ -553,8 +570,8 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8: return getMotorCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1; - case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER: - return mixerConfig()->platformType == PLATFORM_TRICOPTER; + case FLIGHT_LOG_FIELD_CONDITION_SERVOS: + return isMixerUsingServos(); case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1: @@ -706,7 +723,7 @@ static void writeIntraframe(void) blackboxWriteSignedVBArray(blackboxCurrent->mcPosAxisP, XYZ_AXIS_COUNT); - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 4; i++) { blackboxWriteSignedVBArray(blackboxCurrent->mcVelAxisPID[i], XYZ_AXIS_COUNT); } @@ -788,9 +805,11 @@ static void writeIntraframe(void) blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]); } - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { - //Assume the tail spends most of its time around the center - blackboxWriteSignedVB(blackboxCurrent->servo[5] - 1500); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) { + for (int x = 0; x < MAX_SUPPORTED_SERVOS; x++) { + //Assume that servos spends most of its time around the center + blackboxWriteSignedVB(blackboxCurrent->servo[x] - 1500); + } } #ifdef NAV_BLACKBOX @@ -906,7 +925,7 @@ static void writeInterframe(void) arraySubInt32(deltas, blackboxCurrent->mcPosAxisP, blackboxLast->mcPosAxisP, XYZ_AXIS_COUNT); blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); - for (int i = 0; i < XYZ_AXIS_COUNT; i++) { + for (int i = 0; i < 4; i++) { arraySubInt32(deltas, blackboxCurrent->mcVelAxisPID[i], blackboxLast->mcVelAxisPID[i], XYZ_AXIS_COUNT); blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); } @@ -991,8 +1010,8 @@ static void writeInterframe(void) } blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor), getMotorCount()); - if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) { - blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) { + blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, servo), MAX_SUPPORTED_SERVOS); } #ifdef NAV_BLACKBOX @@ -1059,6 +1078,10 @@ static void writeSlowFrame(void) blackboxWriteSigned16VBArray(slowHistory.wind, XYZ_AXIS_COUNT); +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + blackboxWriteUnsignedVB(slowHistory.mspOverrideFlags); +#endif + blackboxWriteSignedVB(slowHistory.imuTemperature); #ifdef USE_BARO @@ -1101,6 +1124,10 @@ static void loadSlowState(blackboxSlowState_t *slow) #endif } +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + slow->mspOverrideFlags = (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) ? 2 : 0) + (mspOverrideIsInFailsafe() ? 1 : 0); +#endif + bool valid_temp; valid_temp = getIMUTemperature(&slow->imuTemperature); if (!valid_temp) slow->imuTemperature = TEMPERATURE_INVALID_VALUE; @@ -1111,7 +1138,7 @@ static void loadSlowState(blackboxSlowState_t *slow) #endif #ifdef USE_TEMPERATURE_SENSOR - for (uint8_t index; index < MAX_TEMP_SENSORS; ++index) { + for (uint8_t index = 0; index < MAX_TEMP_SENSORS; ++index) { valid_temp = getSensorTemperature(index, slow->tempSensorTemperature + index); if (!valid_temp) slow->tempSensorTemperature[index] = TEMPERATURE_INVALID_VALUE; } @@ -1338,6 +1365,7 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->mcVelAxisPID[0][i] = lrintf(nav_pids->vel[i].proportional); blackboxCurrent->mcVelAxisPID[1][i] = lrintf(nav_pids->vel[i].integral); blackboxCurrent->mcVelAxisPID[2][i] = lrintf(nav_pids->vel[i].derivative); + blackboxCurrent->mcVelAxisPID[3][i] = lrintf(nav_pids->vel[i].feedForward); blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained); } #endif @@ -1367,7 +1395,7 @@ static void loadMainState(timeUs_t currentTimeUs) #endif for (int i = 0; i < 4; i++) { - blackboxCurrent->rcData[i] = rcData[i]; + blackboxCurrent->rcData[i] = rxGetChannelValue(i); blackboxCurrent->rcCommand[i] = rcCommand[i]; } @@ -1402,8 +1430,9 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->rssi = getRSSI(); - //Tail servo for tricopters - blackboxCurrent->servo[5] = servo[5]; + for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + blackboxCurrent->servo[i] = servo[i]; + } #ifdef NAV_BLACKBOX blackboxCurrent->navState = navCurrentState; diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index ea07135956f..f7f3dd71488 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -32,7 +32,7 @@ typedef enum FlightLogFieldCondition { FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6, FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7, FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8, - FLIGHT_LOG_FIELD_CONDITION_TRICOPTER, + FLIGHT_LOG_FIELD_CONDITION_SERVOS, FLIGHT_LOG_FIELD_CONDITION_MAG, FLIGHT_LOG_FIELD_CONDITION_BARO, diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h index e62166f7d56..e0cfb5cdc3e 100644 --- a/src/main/build/atomic.h +++ b/src/main/build/atomic.h @@ -91,7 +91,7 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio) // ideally this would only protect memory passed as parameter (any type should work), but gcc is currently creating almost full barrier // this macro can be used only ONCE PER LINE, but multiple uses per block are fine -#if (__GNUC__ > 8) +#if (__GNUC__ > 9) #warning "Please verify that ATOMIC_BARRIER works as intended" // increment version number is BARRIER works // TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead diff --git a/src/main/build/build_config.h b/src/main/build/build_config.h index 79b4a7a4bde..b6a561fecde 100644 --- a/src/main/build/build_config.h +++ b/src/main/build/build_config.h @@ -44,5 +44,12 @@ #else #define FASTRAM __attribute__ ((section(".fastram_bss"), aligned(4))) #endif + +#if defined (STM32F4) || defined (STM32F7) +#define EXTENDED_FASTRAM FASTRAM +#else +#define EXTENDED_FASTRAM +#endif + #define STATIC_FASTRAM static FASTRAM #define STATIC_FASTRAM_UNIT_TESTED STATIC_UNIT_TESTED FASTRAM diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 7852afd98ca..dc5fdbfea83 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -15,29 +15,7 @@ * along with Cleanflight. If not, see . */ -#include -#include -#include -#include - #include "build/debug.h" -#include "build/version.h" - -#include "drivers/serial.h" -#include "drivers/time.h" - -#include "common/printf.h" -#include "common/utils.h" - -#include "config/feature.h" - -#include "io/serial.h" - -#include "fc/config.h" - -#include "msp/msp.h" -#include "msp/msp_serial.h" -#include "msp/msp_protocol.h" #ifdef DEBUG_SECTION_TIMES timeUs_t sectionTimes[2][4]; @@ -45,146 +23,3 @@ timeUs_t sectionTimes[2][4]; int32_t debug[DEBUG32_VALUE_COUNT]; uint8_t debugMode; - -#if defined(USE_DEBUG_TRACE) - -#define DEBUG_TRACE_PREFIX "[%6d.%03d] " -#define DEBUG_TRACE_PREFIX_FORMATTED_SIZE 13 - -static serialPort_t * tracePort = NULL; -static mspPort_t * mspTracePort = NULL; - -void debugTraceInit(void) -{ - if (!feature(FEATURE_DEBUG_TRACE)) { - return; - } - - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_DEBUG_TRACE); - if (!portConfig) { - return; - } - - bool tracePortIsSharedWithMSP = false; - - if (determinePortSharing(portConfig, FUNCTION_DEBUG_TRACE) == PORTSHARING_SHARED) { - // We support sharing a DEBUG_TRACE port only with MSP - if (portConfig->functionMask != (FUNCTION_DEBUG_TRACE | FUNCTION_MSP)) { - return; - } - tracePortIsSharedWithMSP = true; - } - - // If the port is shared with MSP, reuse the port - if (tracePortIsSharedWithMSP) { - const serialPort_t *traceAndMspPort = findSharedSerialPort(FUNCTION_DEBUG_TRACE, FUNCTION_MSP); - if (!traceAndMspPort) { - return; - } - - mspTracePort = mspSerialPortFind(traceAndMspPort); - if (!mspTracePort) { - return; - } - - } else { - tracePort = openSerialPort(portConfig->identifier, FUNCTION_DEBUG_TRACE, NULL, NULL, baudRates[BAUD_921600], MODE_TX, SERIAL_NOT_INVERTED); - if (!tracePort) { - return; - } - } - // Initialization done - DEBUG_TRACE_SYNC("%s/%s %s %s / %s (%s)", - FC_FIRMWARE_NAME, - targetName, - FC_VERSION_STRING, - buildDate, - buildTime, - shortGitRevision - ); -} - -static void debugTracePutcp(void *p, char ch) -{ - *(*((char **) p))++ = ch; -} - -static void debugTracePrint(bool synchronous, const char *buf, size_t size) -{ - if (tracePort) { - // Send data via trace UART (if configured & connected - a safeguard against zombie VCP) - if (serialIsConnected(tracePort)) { - serialPrint(tracePort, buf); - if (synchronous) { - waitForSerialPortToFinishTransmitting(tracePort); - } - } - } else if (mspTracePort) { - mspSerialPushPort(MSP_DEBUGMSG, (uint8_t*)buf, size, mspTracePort, MSP_V2_NATIVE); - } -} - -static size_t debugTraceFormatPrefix(char *buf, const timeMs_t timeMs) -{ - // Write timestamp - return tfp_sprintf(buf, DEBUG_TRACE_PREFIX, timeMs / 1000, timeMs % 1000); -} - -void debugTracePrintf(bool synchronous, const char *format, ...) -{ - char buf[128]; - char *bufPtr; - int charCount; - - STATIC_ASSERT(MSP_PORT_OUTBUF_SIZE >= sizeof(buf), MSP_PORT_OUTBUF_SIZE_not_big_enough_for_debug_trace); - - if (!feature(FEATURE_DEBUG_TRACE)) - return; - - charCount = debugTraceFormatPrefix(buf, millis()); - bufPtr = &buf[charCount]; - - // Write message - va_list va; - va_start(va, format); - charCount += tfp_format(&bufPtr, debugTracePutcp, format, va); - debugTracePutcp(&bufPtr, '\n'); - debugTracePutcp(&bufPtr, 0); - charCount += 2; - va_end(va); - - debugTracePrint(synchronous, buf, charCount); -} - -void debugTracePrintBufferHex(bool synchronous, const void *buffer, size_t size) -{ - // Print lines of up to maxBytes bytes. We need 5 characters per byte - // 0xAB[space|\n] - const size_t charsPerByte = 5; - const size_t maxBytes = 8; - char buf[DEBUG_TRACE_PREFIX_FORMATTED_SIZE + charsPerByte * maxBytes + 1]; // +1 for the null terminator - size_t bufPos = DEBUG_TRACE_PREFIX_FORMATTED_SIZE; - const uint8_t *inputPtr = buffer; - - debugTraceFormatPrefix(buf, millis()); - - for (size_t ii = 0; ii < size; ii++) { - tfp_sprintf(buf + bufPos, "0x%02x ", inputPtr[ii]); - bufPos += charsPerByte; - if (bufPos == sizeof(buf)-1) { - buf[bufPos-1] = '\n'; - buf[bufPos] = '\0'; - debugTracePrint(synchronous, buf, bufPos + 1); - bufPos = DEBUG_TRACE_PREFIX_FORMATTED_SIZE; - } - } - - if (bufPos > DEBUG_TRACE_PREFIX_FORMATTED_SIZE) { - buf[bufPos-1] = '\n'; - buf[bufPos] = '\0'; - debugTracePrint(synchronous, buf, bufPos + 1); - } -} - - -#endif diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 5b66b08a673..91f6f78b66c 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -61,7 +61,6 @@ typedef enum { DEBUG_FPORT, DEBUG_ALWAYS, DEBUG_STAGE2, - DEBUG_WIND_ESTIMATOR, DEBUG_SAG_COMP_VOLTAGE, DEBUG_VIBE, DEBUG_CRUISE, @@ -69,20 +68,7 @@ typedef enum { DEBUG_SMARTAUDIO, DEBUG_ACC, DEBUG_GENERIC, + DEBUG_ITERM_RELAX, + DEBUG_D_BOOST, DEBUG_COUNT } debugType_e; - -#if defined(USE_DEBUG_TRACE) -void debugTraceInit(void); -void debugTracePrintf(bool synchronous, const char *format, ...); -void debugTracePrintBufferHex(bool synchronous, const void *buffer, size_t size); -#define DEBUG_TRACE(fmt, ...) debugTracePrintf(false, fmt, ##__VA_ARGS__) -#define DEBUG_TRACE_SYNC(fmt, ...) debugTracePrintf(true, fmt, ##__VA_ARGS__) -#define DEBUG_TRACE_BUFFER_HEX(buf, size) debugTracePrintBufferHex(false, buf, size) -#define DEBUG_TRACE_BUFFER_HEX_SYNC(buf, size) debugTracePrintBufferHex(true, buf, size) -#else -#define DEBUG_TRACE(fmt, ...) -#define DEBUG_TRACE_SYNC(fmt, ...) -#define DEBUG_TRACE_BUFFER_HEX(buf, size) -#define DEBUG_TRACE_BUFFER_HEX_SYNC(buf, size) -#endif diff --git a/src/main/build/version.h b/src/main/build/version.h index a6fc172ff55..0ae617056b1 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -16,7 +16,7 @@ */ #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_MINOR 2 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index 7df0004705c..6d70284c9d1 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -23,7 +23,6 @@ //#define CMS_PAGE_DEBUG // For multi-page/menu debugging //#define CMS_MENU_DEBUG // For external menu content creators - #include #include #include @@ -39,6 +38,7 @@ #include "cms/cms.h" #include "cms/cms_menu_builtin.h" +#include "cms/cms_menu_saveexit.h" #include "cms/cms_menu_osd.h" #include "cms/cms_types.h" @@ -76,8 +76,8 @@ #define CMS_MAX_DEVICE 4 #endif - // Should be as big as the maximum number of rows displayed - // simultaneously in the tallest supported screen. +// Should be as big as the maximum number of rows displayed +// simultaneously in the tallest supported screen. static uint8_t entry_flags[32]; #define IS_PRINTVALUE(p, row) (entry_flags[row] & PRINT_VALUE) @@ -91,6 +91,8 @@ static uint8_t entry_flags[32]; #define IS_DYNAMIC(p) ((p)->flags & DYNAMIC) #define IS_READONLY(p) ((p)->flags & READONLY) +#define SETTING_INVALID_VALUE_NAME "INVALID" + static displayPort_t *pCurrentDisplay; static displayPort_t *cmsDisplayPorts[CMS_MAX_DEVICE]; @@ -129,6 +131,24 @@ static displayPort_t *cmsDisplayPortSelectNext(void) return cmsDisplayPorts[cmsCurrentDevice]; } +bool cmsDisplayPortSelect(displayPort_t *instance) +{ + if (cmsDeviceCount == 0) { + return false; + } + for (int i = 0; i < cmsDeviceCount; i++) { + if (cmsDisplayPortSelectNext() == instance) { + return true; + } + } + return false; +} + +displayPort_t *cmsDisplayPortGetCurrent(void) +{ + return pCurrentDisplay; +} + #define CMS_UPDATE_INTERVAL_US 50000 // Interval of key scans (microsec) #define CMS_POLL_INTERVAL_US 100000 // Interval of polling dynamic values (microsec) @@ -148,9 +168,13 @@ static displayPort_t *cmsDisplayPortSelectNext(void) // 21 cols x 8 rows // -#define LEFT_MENU_COLUMN 1 -#define RIGHT_MENU_COLUMN(p) ((p)->cols - 8) -#define MAX_MENU_ITEMS(p) ((p)->rows - 2) +#define NORMAL_SCREEN_MIN_COLS 18 // Less is a small screen +static bool smallScreen; +static uint8_t leftMenuColumn; +static uint8_t rightMenuColumn; +static uint8_t maxMenuItems; +static uint8_t linesPerMenuItem; +static cms_key_e externKey = CMS_KEY_NONE; bool cmsInMenu = false; @@ -163,9 +187,9 @@ typedef struct cmsCtx_s { static cmsCtx_t menuStack[10]; static uint8_t menuStackIdx = 0; -static int8_t pageCount; // Number of pages in the current menu -static const OSD_Entry *pageTop; // First entry for the current page -static uint8_t pageMaxRow; // Max row in the current page +static int8_t pageCount; // Number of pages in the current menu +static const OSD_Entry *pageTop; // First entry for the current page +static uint8_t pageMaxRow; // Max row in the current page static cmsCtx_t currentCtx; @@ -203,14 +227,18 @@ static const CMS_Menu menuErr = { static void cmsUpdateMaxRow(displayPort_t *instance) { + UNUSED(instance); pageMaxRow = 0; for (const OSD_Entry *ptr = pageTop; ptr->type != OME_END; ptr++) { pageMaxRow++; + if (ptr->type == OME_BACK_AND_END) { + break; + } } - if (pageMaxRow > MAX_MENU_ITEMS(instance)) { - pageMaxRow = MAX_MENU_ITEMS(instance); + if (pageMaxRow > maxMenuItems) { + pageMaxRow = maxMenuItems; } pageMaxRow--; @@ -218,13 +246,14 @@ static void cmsUpdateMaxRow(displayPort_t *instance) static uint8_t cmsCursorAbsolute(displayPort_t *instance) { - return currentCtx.cursorRow + currentCtx.page * MAX_MENU_ITEMS(instance); + UNUSED(instance); + return currentCtx.cursorRow + currentCtx.page * maxMenuItems; } static void cmsPageSelect(displayPort_t *instance, int8_t newpage) { currentCtx.page = (newpage + pageCount) % pageCount; - pageTop = ¤tCtx.menu->entries[currentCtx.page * MAX_MENU_ITEMS(instance)]; + pageTop = ¤tCtx.menu->entries[currentCtx.page * maxMenuItems]; cmsUpdateMaxRow(instance); displayClearScreen(instance); } @@ -271,52 +300,78 @@ static void cmsFormatFloat(int32_t value, char *floatString) floatString[0] = ' '; } -static void cmsPadToSize(char *buf, int size) +// Pad buffer to the left, i.e. align right +static void cmsPadLeftToSize(char *buf, int size) { - int i; + int i, j; + int len = strlen(buf); - for (i = 0 ; i < size ; i++) { - if (buf[i] == 0) - break; + for (i = size - 1, j = size - len; i - j >= 0; i--) { + buf[i] = buf[i - j]; } - for ( ; i < size ; i++) { + for (; i >= 0; i--) { buf[i] = ' '; } buf[size] = 0; } +static void cmsPadToSize(char *buf, int size) +{ + // Make absolutely sure the string terminated. + buf[size] = 0x00, + + cmsPadLeftToSize(buf, size); +} + +static int cmsDrawMenuItemValue(displayPort_t *pDisplay, char *buff, uint8_t row, uint8_t maxSize) +{ + int colpos; + int cnt; + + cmsPadToSize(buff, maxSize); + colpos = rightMenuColumn - maxSize; + cnt = displayWrite(pDisplay, colpos, row, buff); + return cnt; +} + static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t row, uint8_t screenRow) { - #define CMS_DRAW_BUFFER_LEN 32u - char buff[CMS_DRAW_BUFFER_LEN]; +#define CMS_DRAW_BUFFER_LEN 12 +#define CMS_NUM_FIELD_LEN 5 +#define CMS_CURSOR_BLINK_DELAY_MS 500 + + char buff[CMS_DRAW_BUFFER_LEN + 1]; // Make room for null terminator. int cnt = 0; + if (smallScreen) { + row++; + } + switch (p->type) { case OME_String: if (IS_PRINTVALUE(p, screenRow) && p->data) { - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, p->data); + strncpy(buff, p->data, CMS_DRAW_BUFFER_LEN); + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_DRAW_BUFFER_LEN); CLR_PRINTVALUE(p, screenRow); } break; case OME_Submenu: case OME_Funcall: - if (IS_PRINTVALUE(p, screenRow)) { - - int colPos = RIGHT_MENU_COLUMN(pDisplay); - + if (IS_PRINTVALUE(p, screenRow)) { + buff[0] = 0x0; if ((p->type == OME_Submenu) && p->func && (p->flags & OPTSTRING)) { // Special case of sub menu entry with optional value display. char *str = p->menufunc(); - cnt = displayWrite(pDisplay, colPos, row, str); - colPos += strlen(str); + strncpy(buff, str, CMS_DRAW_BUFFER_LEN); } + strncat(buff, ">", CMS_DRAW_BUFFER_LEN); - cnt += displayWrite(pDisplay, colPos, row, ">"); - + row = smallScreen ? row - 1 : row; + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, strlen(buff)); CLR_PRINTVALUE(p, screenRow); } break; @@ -324,10 +379,12 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t case OME_Bool: if (IS_PRINTVALUE(p, screenRow) && p->data) { if (*((uint8_t *)(p->data))) { - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES"); + strcpy(buff, "YES"); } else { - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO "); + strcpy(buff, "NO"); } + + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, 3); CLR_PRINTVALUE(p, screenRow); } break; @@ -335,7 +392,13 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t case OME_BoolFunc: if (IS_PRINTVALUE(p, screenRow) && p->data) { bool (*func)(bool *arg) = p->data; - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, func(NULL) ? "YES" : "NO "); + if (func(NULL)) { + strcpy(buff, "YES"); + } else { + strcpy(buff, "NO"); + } + + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, 3); CLR_PRINTVALUE(p, screenRow); } break; @@ -344,9 +407,8 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t if (IS_PRINTVALUE(p, screenRow)) { const OSD_TAB_t *ptr = p->data; char * str = (char *)ptr->names[*ptr->val]; - memcpy(buff, str, MAX(CMS_DRAW_BUFFER_LEN, strlen(str))); - cmsPadToSize(buff, CMS_DRAW_BUFFER_LEN); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + strncpy(buff, str, CMS_DRAW_BUFFER_LEN); + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_DRAW_BUFFER_LEN); CLR_PRINTVALUE(p, screenRow); } break; @@ -361,8 +423,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t val = ptr->val; } itoa(*val, buff, 10); - cmsPadToSize(buff, 5); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN); CLR_PRINTVALUE(p, screenRow); } break; @@ -377,8 +438,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t val = ptr->val; } itoa(*val, buff, 10); - cmsPadToSize(buff, 5); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN); CLR_PRINTVALUE(p, screenRow); } break; @@ -393,8 +453,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t val = ptr->val; } itoa(*val, buff, 10); - cmsPadToSize(buff, 5); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN); CLR_PRINTVALUE(p, screenRow); } break; @@ -409,8 +468,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t val = ptr->val; } itoa(*val, buff, 10); - cmsPadToSize(buff, 5); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN); CLR_PRINTVALUE(p, screenRow); } break; @@ -419,18 +477,18 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t if (IS_PRINTVALUE(p, screenRow) && p->data) { const OSD_FLOAT_t *ptr = p->data; cmsFormatFloat(*ptr->val * ptr->multipler, buff); - cmsPadToSize(buff, 5); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ??? + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, CMS_NUM_FIELD_LEN); CLR_PRINTVALUE(p, screenRow); } break; case OME_Setting: if (IS_PRINTVALUE(p, screenRow) && p->data) { + uint8_t maxSize = CMS_NUM_FIELD_LEN; buff[0] = '\0'; const OSD_SETTING_t *ptr = p->data; const setting_t *var = settingGet(ptr->val); - int32_t value; + int32_t value = 0; const void *valuePointer = settingGetValuePointer(var); switch (SETTING_TYPE(var)) { case VAR_UINT8: @@ -478,7 +536,8 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t case MODE_LOOKUP: { const char *str = settingLookupValueName(var, value); - strncpy(buff, str ? str : "INVALID", sizeof(buff) - 1); + strncpy(buff, str ? str : SETTING_INVALID_VALUE_NAME, sizeof(buff) - 1); + maxSize = MAX(settingGetValueNameMaxSize(var), strlen(SETTING_INVALID_VALUE_NAME)); } break; } @@ -486,8 +545,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t strcat(buff, suffix); } } - cmsPadToSize(buff, 8); - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff); + cnt = cmsDrawMenuItemValue(pDisplay, buff, row, maxSize); CLR_PRINTVALUE(p, screenRow); } break; @@ -507,7 +565,8 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t } } if (text) { - cnt = displayWrite(pDisplay, LEFT_MENU_COLUMN + 2 + strlen(p->text), row, text); + cnt = displayWrite(pDisplay, + leftMenuColumn + 1 + (uint8_t) strlen(p->text), row, text); } CLR_PRINTVALUE(p, screenRow); } @@ -516,6 +575,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t case OME_OSD_Exit: case OME_END: case OME_Back: + case OME_BACK_AND_END: break; case OME_MENU: @@ -523,7 +583,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t default: #ifdef CMS_MENU_DEBUG // Shouldn't happen. Notify creator of this menu content. - cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "BADENT"); + cnt = displayWrite(pDisplay, rightMenuColumn - 6), row, "BADENT"); #endif break; } @@ -538,7 +598,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) uint8_t i; const OSD_Entry *p; - uint8_t top = (pDisplay->rows - pageMaxRow) / 2 - 1; + uint8_t top = smallScreen ? 1 : (pDisplay->rows - pageMaxRow) / 2; // Polled (dynamic) value display denominator. @@ -557,7 +617,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) memset(entry_flags, PRINT_LABEL | PRINT_VALUE, sizeof(entry_flags)); pDisplay->cleared = false; } else if (drawPolled) { - for (p = pageTop, i = 0 ; p <= pageTop + pageMaxRow ; p++, i++) { + for (p = pageTop, i = 0; p <= pageTop + pageMaxRow; p++, i++) { if (IS_DYNAMIC(p)) SET_PRINTVALUE(p, i); } @@ -571,14 +631,14 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) cmsPageDebug(); if (pDisplay->cursorRow >= 0 && currentCtx.cursorRow != pDisplay->cursorRow) { - room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, pDisplay->cursorRow + top, " "); + room -= displayWrite(pDisplay, leftMenuColumn, top + pDisplay->cursorRow * linesPerMenuItem, " "); } if (room < 30) return; if (pDisplay->cursorRow != currentCtx.cursorRow) { - room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, currentCtx.cursorRow + top, " >"); + room -= displayWrite(pDisplay, leftMenuColumn, top + currentCtx.cursorRow * linesPerMenuItem, ">"); pDisplay->cursorRow = currentCtx.cursorRow; } @@ -586,36 +646,48 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs) return; // Print text labels - for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { + for (i = 0, p = pageTop; i < maxMenuItems && p->type != OME_END; i++, p++) { if (IS_PRINTLABEL(p, i)) { - uint8_t coloff = LEFT_MENU_COLUMN; - coloff += cmsElementIsLabel(p->type) ? 1 : 2; - room -= displayWrite(pDisplay, coloff, i + top, p->text); + uint8_t coloff = leftMenuColumn; + coloff += cmsElementIsLabel(p->type) ? 0 : 1; + room -= displayWrite(pDisplay, coloff, top + i * linesPerMenuItem, p->text); CLR_PRINTLABEL(p, i); - if (room < 30) + if (room < 30) { return; + } + } + if (p->type == OME_BACK_AND_END) { + break; } } - // Print values // XXX Polled values at latter positions in the list may not be // XXX printed if not enough room in the middle of the list. - - for (i = 0, p = pageTop; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) { + for (i = 0, p = pageTop; i < maxMenuItems && p->type != OME_END; i++, p++) { if (IS_PRINTVALUE(p, i)) { - room -= cmsDrawMenuEntry(pDisplay, p, top + i, i); - if (room < 30) + room -= cmsDrawMenuEntry(pDisplay, p, top + i * linesPerMenuItem, i); + if (room < 30) { return; + } + } + if (p->type == OME_BACK_AND_END) { + break; } } } static void cmsMenuCountPage(displayPort_t *pDisplay) { + UNUSED(pDisplay); const OSD_Entry *p; - for (p = currentCtx.menu->entries; p->type != OME_END; p++); - pageCount = (p - currentCtx.menu->entries - 1) / MAX_MENU_ITEMS(pDisplay) + 1; + for (p = currentCtx.menu->entries; p->type != OME_END; p++) { + if (p->type == OME_BACK_AND_END) { + p++; + break; + } + } + pageCount = (p - currentCtx.menu->entries - 1) / maxMenuItems + 1; } STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay); // Forward; will be resolved after merging @@ -657,9 +729,9 @@ long cmsMenuChange(displayPort_t *pDisplay, const CMS_Menu *pMenu, const OSD_Ent // currentCtx.cursorRow has been saved as absolute; convert it back to page + relative int8_t cursorAbs = currentCtx.cursorRow; - currentCtx.cursorRow = cursorAbs % MAX_MENU_ITEMS(pDisplay); + currentCtx.cursorRow = cursorAbs % maxMenuItems; cmsMenuCountPage(pDisplay); - cmsPageSelect(pDisplay, cursorAbs / MAX_MENU_ITEMS(pDisplay)); + cmsPageSelect(pDisplay, cursorAbs / maxMenuItems); } cmsPageDebug(); @@ -689,7 +761,7 @@ STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay) return 0; } -STATIC_UNIT_TESTED void cmsMenuOpen(void) +void cmsMenuOpen(void) { if (!cmsInMenu) { // New open @@ -713,15 +785,39 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void) } } displayGrab(pCurrentDisplay); // grab the display for use by the CMS + + if (pCurrentDisplay->cols < NORMAL_SCREEN_MIN_COLS) { + smallScreen = true; + linesPerMenuItem = 2; + leftMenuColumn = 0; + rightMenuColumn = pCurrentDisplay->cols; + maxMenuItems = (pCurrentDisplay->rows) / linesPerMenuItem; + } else { + smallScreen = false; + linesPerMenuItem = 1; + leftMenuColumn = 2; + rightMenuColumn = pCurrentDisplay->cols - 2; + maxMenuItems = pCurrentDisplay->rows - 2; + } + + if (pCurrentDisplay->useFullscreen) { + leftMenuColumn = 0; + rightMenuColumn = pCurrentDisplay->cols; + maxMenuItems = pCurrentDisplay->rows; + } + cmsMenuChange(pCurrentDisplay, currentCtx.menu, NULL); } static void cmsTraverseGlobalExit(const CMS_Menu *pMenu) { - for (const OSD_Entry *p = pMenu->entries; p->type != OME_END ; p++) { + for (const OSD_Entry *p = pMenu->entries; p->type != OME_END; p++) { if (p->type == OME_Submenu) { cmsTraverseGlobalExit(p->data); } + if (p->type == OME_BACK_AND_END) { + break; + } } if (pMenu->onGlobalExit) { @@ -735,12 +831,23 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) switch (exitType) { case CMS_EXIT_SAVE: case CMS_EXIT_SAVEREBOOT: + case CMS_POPUP_SAVE: + case CMS_POPUP_SAVEREBOOT: cmsTraverseGlobalExit(&menuMain); if (currentCtx.menu->onExit) currentCtx.menu->onExit((OSD_Entry *)NULL); // Forced exit + if ((exitType == CMS_POPUP_SAVE) || (exitType == CMS_POPUP_SAVEREBOOT)) { + // traverse through the menu stack and call their onExit functions + for (int i = menuStackIdx - 1; i >= 0; i--) { + if (menuStack[i].menu->onExit) { + menuStack[i].menu->onExit((OSD_Entry *) NULL); + } + } + } + saveConfigAndNotify(); break; @@ -753,7 +860,7 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) displayRelease(pDisplay); currentCtx.menu = NULL; - if (exitType == CMS_EXIT_SAVEREBOOT) { + if ((exitType == CMS_EXIT_SAVEREBOOT) || (exitType == CMS_POPUP_SAVEREBOOT)) { displayClearScreen(pDisplay); displayWrite(pDisplay, 5, 3, "REBOOTING..."); @@ -780,17 +887,9 @@ void cmsYieldDisplay(displayPort_t *pPort, timeMs_t duration) // Stick/key detection and key codes -#define IS_HI(X) (rcData[X] > 1750) -#define IS_LO(X) (rcData[X] < 1250) -#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) - -#define KEY_NONE 0 -#define KEY_UP 1 -#define KEY_DOWN 2 -#define KEY_LEFT 3 -#define KEY_RIGHT 4 -#define KEY_ESC 5 -#define KEY_MENU 6 +#define IS_HI(X) (rxGetChannelValue(X) > 1750) +#define IS_LO(X) (rxGetChannelValue(X) < 1250) +#define IS_MID(X) (rxGetChannelValue(X) > 1250 && rxGetChannelValue(X) < 1750) #define BUTTON_TIME 250 // msec #define BUTTON_PAUSE 500 // msec @@ -803,17 +902,22 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) if (!currentCtx.menu) return res; - if (key == KEY_MENU) { + if (key == CMS_KEY_MENU) { cmsMenuOpen(); return BUTTON_PAUSE; } - if (key == KEY_ESC) { + if (key == CMS_KEY_ESC) { cmsMenuBack(pDisplay); return BUTTON_PAUSE; } - if (key == KEY_DOWN) { + if (key == CMS_KEY_SAVEMENU) { + cmsMenuChange(pDisplay, &cmsx_menuSaveExit, NULL); + return BUTTON_PAUSE; + } + + if (key == CMS_KEY_DOWN) { if (currentCtx.cursorRow < pageMaxRow) { currentCtx.cursorRow++; } else { @@ -822,7 +926,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } } - if (key == KEY_UP) { + if (key == CMS_KEY_UP) { currentCtx.cursorRow--; // Skip non-title labels @@ -836,21 +940,21 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } } - if (key == KEY_DOWN || key == KEY_UP) + if (key == CMS_KEY_DOWN || key == CMS_KEY_UP) return res; p = pageTop + currentCtx.cursorRow; switch (p->type) { case OME_Submenu: - if (key == KEY_RIGHT) { + if (key == CMS_KEY_RIGHT) { cmsMenuChange(pDisplay, p->data, p); res = BUTTON_PAUSE; } break; case OME_Funcall: - if (p->func && key == KEY_RIGHT) { + if (p->func && key == CMS_KEY_RIGHT) { long retval = p->func(pDisplay, p->data); if (retval == MENU_CHAIN_BACK) cmsMenuBack(pDisplay); @@ -859,13 +963,14 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) break; case OME_OSD_Exit: - if (p->func && key == KEY_RIGHT) { + if (p->func && key == CMS_KEY_RIGHT) { p->func(pDisplay, p->data); res = BUTTON_PAUSE; } break; case OME_Back: + case OME_BACK_AND_END: cmsMenuBack(pDisplay); res = BUTTON_PAUSE; break; @@ -873,7 +978,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) case OME_Bool: if (p->data) { uint8_t *val = (uint8_t *)p->data; - if (key == KEY_RIGHT) + if (key == CMS_KEY_RIGHT) *val = 1; else *val = 0; @@ -887,7 +992,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) case OME_BoolFunc: if (p->data) { bool (*func)(bool *arg) = p->data; - bool val = key == KEY_RIGHT; + bool val = key == CMS_KEY_RIGHT; func(&val); SET_PRINTVALUE(p, currentCtx.cursorRow); } @@ -900,11 +1005,10 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } if (p->data) { const OSD_UINT8_t *ptr = p->data; - if (key == KEY_RIGHT) { + if (key == CMS_KEY_RIGHT) { if (*ptr->val < ptr->max) *ptr->val += ptr->step; - } - else { + } else { if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } @@ -919,16 +1023,16 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) if (p->type == OME_TAB) { const OSD_TAB_t *ptr = p->data; - if (key == KEY_RIGHT) { + if (key == CMS_KEY_RIGHT) { if (*ptr->val < ptr->max) *ptr->val += 1; - } - else { + } else { if (*ptr->val > 0) *ptr->val -= 1; } - if (p->func) + if (p->func) { p->func(pDisplay, p->data); + } SET_PRINTVALUE(p, currentCtx.cursorRow); } break; @@ -939,11 +1043,10 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } if (p->data) { const OSD_INT8_t *ptr = p->data; - if (key == KEY_RIGHT) { + if (key == CMS_KEY_RIGHT) { if (*ptr->val < ptr->max) *ptr->val += ptr->step; - } - else { + } else { if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } @@ -960,11 +1063,10 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } if (p->data) { const OSD_UINT16_t *ptr = p->data; - if (key == KEY_RIGHT) { + if (key == CMS_KEY_RIGHT) { if (*ptr->val < ptr->max) *ptr->val += ptr->step; - } - else { + } else { if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } @@ -981,11 +1083,10 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) } if (p->data) { const OSD_INT16_t *ptr = p->data; - if (key == KEY_RIGHT) { + if (key == CMS_KEY_RIGHT) { if (*ptr->val < ptr->max) *ptr->val += ptr->step; - } - else { + } else { if (*ptr->val > ptr->min) *ptr->val -= ptr->step; } @@ -1003,7 +1104,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) setting_min_t min = settingGetMin(var); setting_max_t max = settingGetMax(var); float step = ptr->step ?: 1; - if (key != KEY_RIGHT) { + if (key != CMS_KEY_RIGHT) { step = -step; } const void *valuePointer = settingGetValuePointer(var); @@ -1050,7 +1151,6 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) *(float *)valuePointer = val; break; } - break; case VAR_STRING: break; } @@ -1076,11 +1176,18 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) return res; } -uint16_t cmsHandleKeyWithRepeat(displayPort_t *pDisplay, uint8_t key, int repeatCount) +void cmsSetExternKey(cms_key_e extKey) +{ + if (externKey == CMS_KEY_NONE) + externKey = extKey; +} + +uint16_t cmsHandleKeyWithRepeat(displayPort_t *pDisplay, uint8_t key, +int repeatCount) { uint16_t ret = 0; - for (int i = 0 ; i < repeatCount ; i++) { + for (int i = 0; i < repeatCount; i++) { ret = cmsHandleKey(pDisplay, key); } @@ -1097,79 +1204,82 @@ static uint16_t cmsScanKeys(timeMs_t currentTimeMs, timeMs_t lastCalledMs, int16 // Scan 'key' first // - uint8_t key = KEY_NONE; - - if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { - key = KEY_MENU; - } - else if (IS_HI(PITCH)) { - key = KEY_UP; - } - else if (IS_LO(PITCH)) { - key = KEY_DOWN; - } - else if (IS_LO(ROLL)) { - key = KEY_LEFT; - } - else if (IS_HI(ROLL)) { - key = KEY_RIGHT; - } - else if (IS_HI(YAW) || IS_LO(YAW)) - { - key = KEY_ESC; - } + uint8_t key = CMS_KEY_NONE; - if (key == KEY_NONE) { - // No 'key' pressed, reset repeat control - holdCount = 1; - repeatCount = 1; - repeatBase = 0; + if (externKey != CMS_KEY_NONE) { + rcDelayMs = cmsHandleKey(pCurrentDisplay, externKey); + externKey = CMS_KEY_NONE; } else { - // The 'key' is being pressed; keep counting - ++holdCount; - } + if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + key = CMS_KEY_MENU; + } else if (IS_HI(PITCH)) { + key = CMS_KEY_UP; + } else if (IS_LO(PITCH)) { + key = CMS_KEY_DOWN; + } else if (IS_LO(ROLL)) { + key = CMS_KEY_LEFT; + } else if (IS_HI(ROLL)) { + key = CMS_KEY_RIGHT; + } else if (IS_LO(YAW)) { + key = CMS_KEY_ESC; + } else if (IS_HI(YAW)) { + key = CMS_KEY_SAVEMENU; + } + + if (key == CMS_KEY_NONE) { + // No 'key' pressed, reset repeat control + holdCount = 1; + repeatCount = 1; + repeatBase = 0; + } else { + // The 'key' is being pressed; keep counting + ++holdCount; + } - if (rcDelayMs > 0) { - rcDelayMs -= (currentTimeMs - lastCalledMs); - } else if (key) { - rcDelayMs = cmsHandleKeyWithRepeat(pCurrentDisplay, key, repeatCount); + if (rcDelayMs > 0) { + rcDelayMs -= (currentTimeMs - lastCalledMs); + } else if (key) { + rcDelayMs = cmsHandleKeyWithRepeat(pCurrentDisplay, key, + repeatCount); - // Key repeat effect is implemented in two phases. - // First phldase is to decrease rcDelayMs reciprocal to hold time. - // When rcDelayMs reached a certain limit (scheduling interval), - // repeat rate will not raise anymore, so we call key handler - // multiple times (repeatCount). - // - // XXX Caveat: Most constants are adjusted pragmatically. - // XXX Rewrite this someday, so it uses actual hold time instead - // of holdCount, which depends on the scheduling interval. + // Key repeat effect is implemented in two phases. + // First phldase is to decrease rcDelayMs reciprocal to hold time. + // When rcDelayMs reached a certain limit (scheduling interval), + // repeat rate will not raise anymore, so we call key handler + // multiple times (repeatCount). + // + // XXX Caveat: Most constants are adjusted pragmatically. + // XXX Rewrite this someday, so it uses actual hold time instead + // of holdCount, which depends on the scheduling interval. - if (((key == KEY_LEFT) || (key == KEY_RIGHT)) && (holdCount > 20)) { + if (((key == CMS_KEY_LEFT) || (key == CMS_KEY_RIGHT)) && (holdCount > 20)) { - // Decrease rcDelayMs reciprocally + // Decrease rcDelayMs reciprocally - rcDelayMs /= (holdCount - 20); + rcDelayMs /= (holdCount - 20); - // When we reach the scheduling limit, + // When we reach the scheduling limit, - if (rcDelayMs <= 50) { + if (rcDelayMs <= 50) { - // start calling handler multiple times. + // start calling handler multiple times. - if (repeatBase == 0) + if (repeatBase == 0) repeatBase = holdCount; - if (holdCount < 100) { - repeatCount = repeatCount + (holdCount - repeatBase) / 5; + if (holdCount < 100) { + repeatCount = repeatCount + + (holdCount - repeatBase) / 5; - if (repeatCount > 5) { - repeatCount= 5; - } - } else { - repeatCount = repeatCount + holdCount - repeatBase; + if (repeatCount > 5) { + repeatCount = 5; + } + } else { + repeatCount = repeatCount + holdCount - repeatBase; - if (repeatCount > 50) { - repeatCount = 50; + if (repeatCount > 50) { + repeatCount = 50; + } } } } diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index fb7dbd3d423..f1f8d8017c8 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -6,6 +6,18 @@ #include "cms/cms_types.h" +typedef enum { + CMS_KEY_NONE, + CMS_KEY_UP, + CMS_KEY_DOWN, + CMS_KEY_LEFT, + CMS_KEY_RIGHT, + CMS_KEY_ESC, + CMS_KEY_MENU, + CMS_KEY_SAVEMENU +} cms_key_e; + + extern bool cmsInMenu; // Device management @@ -15,17 +27,24 @@ bool cmsDisplayPortRegister(displayPort_t *pDisplay); void cmsInit(void); void cmsHandler(timeUs_t currentTimeUs); +bool cmsDisplayPortSelect(displayPort_t *instance); +displayPort_t *cmsDisplayPortGetCurrent(void); + +void cmsMenuOpen(void); long cmsMenuChange(displayPort_t *pPort, const CMS_Menu *menu, const OSD_Entry *from); long cmsMenuExit(displayPort_t *pPort, const void *ptr); void cmsYieldDisplay(displayPort_t *pPort, timeMs_t duration); void cmsUpdate(uint32_t currentTimeUs); +void cmsSetExternKey(cms_key_e extKey); #define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" #define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" #define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP" // cmsMenuExit special ptr values -#define CMS_EXIT (0) -#define CMS_EXIT_SAVE (1) -#define CMS_EXIT_SAVEREBOOT (2) +#define CMS_EXIT (0) +#define CMS_EXIT_SAVE (1) +#define CMS_EXIT_SAVEREBOOT (2) +#define CMS_POPUP_SAVE (3) +#define CMS_POPUP_SAVEREBOOT (4) diff --git a/src/main/cms/cms_menu_battery.c b/src/main/cms/cms_menu_battery.c index fd6efaa304e..c6abcdace1b 100644 --- a/src/main/cms/cms_menu_battery.c +++ b/src/main/cms/cms_menu_battery.c @@ -109,8 +109,7 @@ static const OSD_Entry menuBattSettingsEntries[]= OSD_SETTING_ENTRY("CAP WARN", SETTING_BATTERY_CAPACITY_WARNING), OSD_SETTING_ENTRY("CAP CRIT", SETTING_BATTERY_CAPACITY_CRITICAL), - OSD_BACK_ENTRY, - OSD_END_ENTRY + OSD_BACK_AND_END_ENTRY, }; static CMS_Menu cmsx_menuBattSettings = { @@ -134,8 +133,7 @@ static OSD_Entry menuBatteryEntries[]= OSD_UINT8_CALLBACK_ENTRY("PROF", cmsx_onBatteryProfileIndexChange, (&(const OSD_UINT8_t){ &battDispProfileIndex, 1, MAX_BATTERY_PROFILE_COUNT, 1})), OSD_SUBMENU_ENTRY("SETTINGS", &cmsx_menuBattSettings), - OSD_BACK_ENTRY, - OSD_END_ENTRY + OSD_BACK_AND_END_ENTRY, }; CMS_Menu cmsx_menuBattery = { diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index cab1fe8737a..6fdaae69546 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -86,8 +86,7 @@ static const OSD_Entry cmsx_menuBlackboxEntries[] = OSD_FUNC_CALL_ENTRY("ERASE FLASH", cmsx_EraseFlash), #endif // USE_FLASHFS - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; const CMS_Menu cmsx_menuBlackbox = { diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index df9feb7acf7..f5378268afb 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -42,6 +42,7 @@ #include "cms/cms_menu_imu.h" #include "cms/cms_menu_blackbox.h" +#include "cms/cms_menu_mixer_servo.h" #include "cms/cms_menu_navigation.h" #include "cms/cms_menu_vtx.h" #include "cms/cms_menu_osd.h" @@ -86,8 +87,7 @@ static const OSD_Entry menuInfoEntries[] = { OSD_STRING_ENTRY("GITREV", infoGitRev), OSD_STRING_ENTRY("TARGET", infoTargetName), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu menuInfo = { @@ -107,6 +107,7 @@ static const OSD_Entry menuFeaturesEntries[] = { OSD_LABEL_ENTRY("--- FEATURES ---"), OSD_SUBMENU_ENTRY("BLACKBOX", &cmsx_menuBlackbox), + OSD_SUBMENU_ENTRY("MIXER & SERVOS", &cmsx_menuMixerServo), #if defined(USE_NAV) OSD_SUBMENU_ENTRY("NAVIGATION", &cmsx_menuNavigation), #endif @@ -128,8 +129,7 @@ static const OSD_Entry menuFeaturesEntries[] = OSD_SUBMENU_ENTRY("LED STRIP", &cmsx_menuLedstrip), #endif // LED_STRIP - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu menuFeatures = { @@ -158,8 +158,8 @@ static const OSD_Entry menuMainEntries[] = OSD_SUBMENU_ENTRY("FC+FW INFO", &menuInfo), OSD_SUBMENU_ENTRY("MISC", &cmsx_menuMisc), - {"SAVE+REBOOT", OME_OSD_Exit, {.func = cmsMenuExit}, (void*)CMS_EXIT_SAVEREBOOT, 0}, - {"EXIT", OME_OSD_Exit, {.func = cmsMenuExit}, (void*)CMS_EXIT, 0}, + {"SAVE+REBOOT", {.func = cmsMenuExit}, (void*)CMS_EXIT_SAVEREBOOT, OME_OSD_Exit, 0}, + {"EXIT" , {.func = cmsMenuExit}, (void*)CMS_EXIT, OME_OSD_Exit, 0}, #ifdef CMS_MENU_DEBUG OSD_SUBMENU_ENTRY("ERR SAMPLE", &menuInfoEntries[0]), #endif diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 25accaa35c1..690d5d160da 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -47,24 +47,39 @@ // -// PID +// PIDFF // +#define PIDFF_MIN 0 +#define PIDFF_STEP 1 + +#define RPY_PIDFF_MAX 200 +#define OTHER_PIDDF_MAX 255 + +#define PIDFF_ENTRY(label, ptr, max) OSD_UINT8_ENTRY(label, (&(const OSD_UINT8_t){ ptr, PIDFF_MIN, max, PIDFF_STEP })) +#define RPY_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, RPY_PIDFF_MAX) +#define OTHER_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, OTHER_PIDDF_MAX) + +static pid8_t cmsx_pidRoll; +static pid8_t cmsx_pidPitch; +static pid8_t cmsx_pidYaw; +static pid8_t cmsx_pidPosZ; +static pid8_t cmsx_pidVelZ; +static pid8_t cmsx_pidHead; +static pid8_t cmsx_pidPosXY; +static pid8_t cmsx_pidVelXY; + static uint8_t tmpProfileIndex; static uint8_t profileIndex; static char profileIndexString[] = " p"; -static void cmsx_ReadPidToArray(uint8_t *dst, int pidIndex) +static void cmsx_ReadPidToArray(pid8_t *dst, int pidIndex) { - dst[0] = pidBank()->pid[pidIndex].P; - dst[1] = pidBank()->pid[pidIndex].I; - dst[2] = pidBank()->pid[pidIndex].D; + memcpy(dst, &pidBank()->pid[pidIndex], sizeof(*dst)); } -static void cmsx_WritebackPidFromArray(uint8_t *src, int pidIndex) +static void cmsx_WritebackPidFromArray(const pid8_t *src, int pidIndex) { - pidBankMutable()->pid[pidIndex].P = src[0]; - pidBankMutable()->pid[pidIndex].I = src[1]; - pidBankMutable()->pid[pidIndex].D = src[2]; + memcpy(&pidBankMutable()->pid[pidIndex], src, sizeof(*src)); } static long cmsx_menuImu_onEnter(const OSD_Entry *from) @@ -90,15 +105,11 @@ static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *pt return 0; } -static uint8_t cmsx_pidRoll[3]; -static uint8_t cmsx_pidPitch[3]; -static uint8_t cmsx_pidYaw[3]; - static long cmsx_PidRead(void) { - cmsx_ReadPidToArray(cmsx_pidRoll, PID_ROLL); - cmsx_ReadPidToArray(cmsx_pidPitch, PID_PITCH); - cmsx_ReadPidToArray(cmsx_pidYaw, PID_YAW); + cmsx_ReadPidToArray(&cmsx_pidRoll, PID_ROLL); + cmsx_ReadPidToArray(&cmsx_pidPitch, PID_PITCH); + cmsx_ReadPidToArray(&cmsx_pidYaw, PID_YAW); return 0; } @@ -117,9 +128,9 @@ static long cmsx_PidWriteback(const OSD_Entry *self) { UNUSED(self); - cmsx_WritebackPidFromArray(cmsx_pidRoll, PID_ROLL); - cmsx_WritebackPidFromArray(cmsx_pidPitch, PID_PITCH); - cmsx_WritebackPidFromArray(cmsx_pidYaw, PID_YAW); + cmsx_WritebackPidFromArray(&cmsx_pidRoll, PID_ROLL); + cmsx_WritebackPidFromArray(&cmsx_pidPitch, PID_PITCH); + cmsx_WritebackPidFromArray(&cmsx_pidYaw, PID_YAW); schedulePidGainsUpdate(); @@ -130,20 +141,22 @@ static const OSD_Entry cmsx_menuPidEntries[] = { OSD_LABEL_DATA_ENTRY("-- PID --", profileIndexString), - OSD_UINT8_ENTRY("ROLL P", (&(const OSD_UINT8_t){ &cmsx_pidRoll[0], 0, 200, 1 })), - OSD_UINT8_ENTRY("ROLL I", (&(const OSD_UINT8_t){ &cmsx_pidRoll[1], 0, 200, 1 })), - OSD_UINT8_ENTRY("ROLL D", (&(const OSD_UINT8_t){ &cmsx_pidRoll[2], 0, 200, 1 })), + RPY_PIDFF_ENTRY("ROLL P", &cmsx_pidRoll.P), + RPY_PIDFF_ENTRY("ROLL I", &cmsx_pidRoll.I), + RPY_PIDFF_ENTRY("ROLL D", &cmsx_pidRoll.D), + RPY_PIDFF_ENTRY("ROLL FF", &cmsx_pidRoll.FF), - OSD_UINT8_ENTRY("PITCH P", (&(const OSD_UINT8_t){ &cmsx_pidPitch[0], 0, 200, 1 })), - OSD_UINT8_ENTRY("PITCH I", (&(const OSD_UINT8_t){ &cmsx_pidPitch[1], 0, 200, 1 })), - OSD_UINT8_ENTRY("PITCH D", (&(const OSD_UINT8_t){ &cmsx_pidPitch[2], 0, 200, 1 })), + RPY_PIDFF_ENTRY("PITCH P", &cmsx_pidPitch.P), + RPY_PIDFF_ENTRY("PITCH I", &cmsx_pidPitch.I), + RPY_PIDFF_ENTRY("PITCH D", &cmsx_pidPitch.D), + RPY_PIDFF_ENTRY("PITCH FF", &cmsx_pidPitch.FF), - OSD_UINT8_ENTRY("YAW P", (&(const OSD_UINT8_t){ &cmsx_pidYaw[0], 0, 200, 1 })), - OSD_UINT8_ENTRY("YAW I", (&(const OSD_UINT8_t){ &cmsx_pidYaw[1], 0, 200, 1 })), - OSD_UINT8_ENTRY("YAW D", (&(const OSD_UINT8_t){ &cmsx_pidYaw[2], 0, 200, 1 })), + RPY_PIDFF_ENTRY("YAW P", &cmsx_pidYaw.P), + RPY_PIDFF_ENTRY("YAW I", &cmsx_pidYaw.I), + RPY_PIDFF_ENTRY("YAW D", &cmsx_pidYaw.D), + RPY_PIDFF_ENTRY("YAW FF", &cmsx_pidYaw.FF), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu cmsx_menuPid = { @@ -157,17 +170,13 @@ static const CMS_Menu cmsx_menuPid = { .entries = cmsx_menuPidEntries }; -static uint8_t cmsx_pidPosZ[3]; -static uint8_t cmsx_pidVelZ[3]; -static uint8_t cmsx_pidHead[3]; - static long cmsx_menuPidAltMag_onEnter(const OSD_Entry *from) { UNUSED(from); - cmsx_ReadPidToArray(cmsx_pidPosZ, PID_POS_Z); - cmsx_ReadPidToArray(cmsx_pidVelZ, PID_VEL_Z); - cmsx_pidHead[0] = pidBank()->pid[PID_HEADING].P; + cmsx_ReadPidToArray(&cmsx_pidPosZ, PID_POS_Z); + cmsx_ReadPidToArray(&cmsx_pidVelZ, PID_VEL_Z); + cmsx_ReadPidToArray(&cmsx_pidHead, PID_HEADING); return 0; } @@ -176,9 +185,9 @@ static long cmsx_menuPidAltMag_onExit(const OSD_Entry *self) { UNUSED(self); - cmsx_WritebackPidFromArray(cmsx_pidPosZ, PID_POS_Z); - cmsx_WritebackPidFromArray(cmsx_pidVelZ, PID_VEL_Z); - pidBankMutable()->pid[PID_HEADING].P = cmsx_pidHead[0]; + cmsx_WritebackPidFromArray(&cmsx_pidPosZ, PID_POS_Z); + cmsx_WritebackPidFromArray(&cmsx_pidVelZ, PID_VEL_Z); + cmsx_WritebackPidFromArray(&cmsx_pidHead, PID_HEADING); navigationUsePIDs(); @@ -189,17 +198,17 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] = { OSD_LABEL_DATA_ENTRY("-- ALT&MAG --", profileIndexString), - OSD_UINT8_ENTRY("ALT P", (&(const OSD_UINT8_t){ &cmsx_pidPosZ[0], 0, 255, 1 })), - OSD_UINT8_ENTRY("ALT I", (&(const OSD_UINT8_t){ &cmsx_pidPosZ[1], 0, 255, 1 })), - OSD_UINT8_ENTRY("ALT D", (&(const OSD_UINT8_t){ &cmsx_pidPosZ[2], 0, 255, 1 })), - OSD_UINT8_ENTRY("VEL P", (&(const OSD_UINT8_t){ &cmsx_pidVelZ[0], 0, 255, 1 })), - OSD_UINT8_ENTRY("VEL I", (&(const OSD_UINT8_t){ &cmsx_pidVelZ[1], 0, 255, 1 })), - OSD_UINT8_ENTRY("VEL D", (&(const OSD_UINT8_t){ &cmsx_pidVelZ[2], 0, 255, 1 })), + OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P), + OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I), + OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D), + + OTHER_PIDFF_ENTRY("VEL P", &cmsx_pidVelZ.P), + OTHER_PIDFF_ENTRY("VEL I", &cmsx_pidVelZ.I), + OTHER_PIDFF_ENTRY("VEL D", &cmsx_pidVelZ.D), - OSD_UINT8_ENTRY("MAG P", (&(const OSD_UINT8_t){ &cmsx_pidHead[0], 0, 255, 1 })), + OTHER_PIDFF_ENTRY("MAG P", &cmsx_pidHead.P), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu cmsx_menuPidAltMag = { @@ -213,15 +222,12 @@ static const CMS_Menu cmsx_menuPidAltMag = { .entries = cmsx_menuPidAltMagEntries, }; -static uint8_t cmsx_pidPosXY[3]; -static uint8_t cmsx_pidVelXY[3]; - static long cmsx_menuPidGpsnav_onEnter(const OSD_Entry *from) { UNUSED(from); - cmsx_ReadPidToArray(cmsx_pidPosXY, PID_POS_XY); - cmsx_ReadPidToArray(cmsx_pidVelXY, PID_VEL_XY); + cmsx_ReadPidToArray(&cmsx_pidPosXY, PID_POS_XY); + cmsx_ReadPidToArray(&cmsx_pidVelXY, PID_VEL_XY); return 0; } @@ -230,8 +236,8 @@ static long cmsx_menuPidGpsnav_onExit(const OSD_Entry *self) { UNUSED(self); - cmsx_WritebackPidFromArray(cmsx_pidPosXY, PID_POS_XY); - cmsx_WritebackPidFromArray(cmsx_pidVelXY, PID_VEL_XY); + cmsx_WritebackPidFromArray(&cmsx_pidPosXY, PID_POS_XY); + cmsx_WritebackPidFromArray(&cmsx_pidVelXY, PID_VEL_XY); navigationUsePIDs(); @@ -242,14 +248,14 @@ static const OSD_Entry cmsx_menuPidGpsnavEntries[] = { OSD_LABEL_DATA_ENTRY("-- GPSNAV --", profileIndexString), - OSD_UINT8_ENTRY("POS P", (&(const OSD_UINT8_t){ &cmsx_pidPosXY[0], 0, 255, 1 })), - OSD_UINT8_ENTRY("POS I", (&(const OSD_UINT8_t){ &cmsx_pidPosXY[1], 0, 255, 1 })), - OSD_UINT8_ENTRY("POSR P", (&(const OSD_UINT8_t){ &cmsx_pidVelXY[0], 0, 255, 1 })), - OSD_UINT8_ENTRY("POSR I", (&(const OSD_UINT8_t){ &cmsx_pidVelXY[1], 0, 255, 1 })), - OSD_UINT8_ENTRY("POSR D", (&(const OSD_UINT8_t){ &cmsx_pidVelXY[2], 0, 255, 1 })), + OTHER_PIDFF_ENTRY("POS P", &cmsx_pidPosXY.P), + OTHER_PIDFF_ENTRY("POS I", &cmsx_pidPosXY.I), + + OTHER_PIDFF_ENTRY("POSR P", &cmsx_pidVelXY.P), + OTHER_PIDFF_ENTRY("POSR I", &cmsx_pidVelXY.I), + OTHER_PIDFF_ENTRY("POSR D", &cmsx_pidVelXY.D), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu cmsx_menuPidGpsnav = { @@ -277,8 +283,7 @@ static const OSD_Entry cmsx_menuManualRateProfileEntries[] = OSD_SETTING_ENTRY("MANU RC EXPO", SETTING_MANUAL_RC_EXPO), OSD_SETTING_ENTRY("MANU RC YAW EXP", SETTING_MANUAL_RC_YAW_EXPO), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu cmsx_menuManualRateProfile = { @@ -317,8 +322,7 @@ static const OSD_Entry cmsx_menuRateProfileEntries[] = OSD_SETTING_ENTRY("THRPID ATT", SETTING_TPA_RATE), OSD_SETTING_ENTRY_STEP("TPA BRKPT", SETTING_TPA_BREAKPOINT, 10), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu cmsx_menuRateProfile = { @@ -376,8 +380,7 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = { { "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_horizonStrength, 0, 200, 1 } , 0 }, { "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_horizonTransition, 0, 200, 1 } , 0 }, - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu cmsx_menuProfileOther = { @@ -404,8 +407,7 @@ static const OSD_Entry cmsx_menuFilterPerProfileEntries[] = OSD_SETTING_ENTRY("YAW P LIM", SETTING_YAW_P_LIMIT), OSD_SETTING_ENTRY("YAW LPF", SETTING_YAW_LPF_HZ), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu cmsx_menuFilterPerProfile = { @@ -426,8 +428,7 @@ static const OSD_Entry cmsx_menuGyroEntries[] = OSD_SETTING_ENTRY("GYRO SYNC", SETTING_GYRO_SYNC), OSD_SETTING_ENTRY("GYRO LPF", SETTING_GYRO_HARDWARE_LPF), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu cmsx_menuGyro = { @@ -466,8 +467,7 @@ static const OSD_Entry cmsx_menuImuEntries[] = {"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, #endif - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; const CMS_Menu cmsx_menuImu = { diff --git a/src/main/cms/cms_menu_ledstrip.c b/src/main/cms/cms_menu_ledstrip.c index d460c3d0d1b..2fc6552f4b4 100644 --- a/src/main/cms/cms_menu_ledstrip.c +++ b/src/main/cms/cms_menu_ledstrip.c @@ -60,8 +60,7 @@ static const OSD_Entry cmsx_menuLedstripEntries[] = OSD_LABEL_ENTRY("-- LED STRIP --"), OSD_BOOL_FUNC_ENTRY("ENABLED", cmsx_FeatureLedStrip_Enabled), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; const CMS_Menu cmsx_menuLedstrip = { diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 7a8b5af4305..2a3a7610d71 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -38,50 +38,6 @@ #include "sensors/battery.h" -// -// Misc -// - -static long cmsx_menuRcConfirmBack(const OSD_Entry *self) -{ - if (self && self->type == OME_Back) - return 0; - else - return -1; -} - -// -// RC preview -// -static const OSD_Entry cmsx_menuRcEntries[] = -{ - OSD_LABEL_ENTRY("-- RC PREV --"), - - OSD_INT16_RO_ENTRY("ROLL", &rcData[ROLL]), - OSD_INT16_RO_ENTRY("PITCH", &rcData[PITCH]), - OSD_INT16_RO_ENTRY("THR", &rcData[THROTTLE]), - OSD_INT16_RO_ENTRY("YAW", &rcData[YAW]), - - OSD_INT16_RO_ENTRY("AUX1", &rcData[AUX1]), - OSD_INT16_RO_ENTRY("AUX2", &rcData[AUX2]), - OSD_INT16_RO_ENTRY("AUX3", &rcData[AUX3]), - OSD_INT16_RO_ENTRY("AUX4", &rcData[AUX4]), - - OSD_BACK_ENTRY, - OSD_END_ENTRY, -}; - -static const CMS_Menu cmsx_menuRcPreview = { -#ifdef CMS_MENU_DEBUG - .GUARD_text = "XRCPREV", - .GUARD_type = OME_MENU, -#endif - .onEnter = NULL, - .onExit = cmsx_menuRcConfirmBack, - .onGlobalExit = NULL, - .entries = cmsx_menuRcEntries -}; - static const OSD_Entry menuMiscEntries[]= { OSD_LABEL_ENTRY("-- MISC --"), @@ -92,10 +48,7 @@ static const OSD_Entry menuMiscEntries[]= OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT), #endif - OSD_SUBMENU_ENTRY("RC PREV", &cmsx_menuRcPreview), - - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; const CMS_Menu cmsx_menuMisc = { diff --git a/src/main/cms/cms_menu_mixer_servo.c b/src/main/cms/cms_menu_mixer_servo.c new file mode 100644 index 00000000000..e83869b57f0 --- /dev/null +++ b/src/main/cms/cms_menu_mixer_servo.c @@ -0,0 +1,311 @@ +/* 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 . + */ + +// +// Mixer and servo menu +// + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_CMS + +#include "common/utils.h" + +#include "build/version.h" + +#include "flight/mixer.h" +#include "flight/servos.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms_menu_mixer_servo.h" + +#include "fc/runtime_config.h" +#include "fc/settings.h" + +static uint8_t currentMotorMixerIndex = 0; +static uint8_t tmpcurrentMotorMixerIndex = 1; +static uint16_t tmpMotorMixerThrottle; +static int16_t tmpMotorMixerRoll; +static int16_t tmpMotorMixerYaw; +static int16_t tmpMotorMixerPitch; +static uint8_t currentServoMixerIndex = 0; +static uint8_t tmpcurrentServoMixerIndex = 1; +static servoMixer_t tmpServoMixer; +static uint8_t currentServoIndex = 0; +static uint8_t oldServoIndex = 0; +static servoParam_t tmpServoParam; + +static void loadServoSettings(void) +{ + tmpServoParam.middle = servoParams(currentServoIndex)->middle; + tmpServoParam.min = servoParams(currentServoIndex)->min; + tmpServoParam.max = servoParams(currentServoIndex)->max; + tmpServoParam.rate = servoParams(currentServoIndex)->rate; +} + +static void saveServoSettings(uint8_t idx) +{ + servoParamsMutable(idx)->middle = tmpServoParam.middle; + servoParamsMutable(idx)->min = tmpServoParam.min; + servoParamsMutable(idx)->max = tmpServoParam.max; + servoParamsMutable(idx)->rate = tmpServoParam.rate; +} + +static long cmsx_menuServo_onEnter(const OSD_Entry *from) +{ + UNUSED(from); + + loadServoSettings(); + + return 0; +} + +static long cmsx_menuServo_onExit(const OSD_Entry *from) +{ + UNUSED(from); + + saveServoSettings(currentServoIndex); + + return 0; +} + +static long cmsx_menuServoIndexOnChange(displayPort_t *displayPort, const void *ptr) +{ + UNUSED(displayPort); + UNUSED(ptr); + + saveServoSettings(oldServoIndex); + loadServoSettings(); + oldServoIndex = currentServoIndex; + + return 0; +} + +static const OSD_Entry cmsx_menuServoEntries[] = +{ + OSD_LABEL_ENTRY("-- SERVOS --"), + OSD_UINT8_CALLBACK_ENTRY("SERVO", cmsx_menuServoIndexOnChange, (&(const OSD_UINT8_t){ ¤tServoIndex, 0, MAX_SUPPORTED_SERVOS - 1, 1})), + OSD_INT16_DYN_ENTRY("MID", (&(const OSD_INT16_t){&tmpServoParam.middle, 500, 2500, 1})), + OSD_INT16_DYN_ENTRY("MIN", (&(const OSD_INT16_t){&tmpServoParam.min, 500, 2500, 1})), + OSD_INT16_DYN_ENTRY("MAX", (&(const OSD_INT16_t){&tmpServoParam.max, 500, 2500, 1})), + OSD_INT8_DYN_ENTRY("RATE", (&(const OSD_INT8_t){&tmpServoParam.rate, -125, 125, 1})), + OSD_BACK_AND_END_ENTRY +}; + +const CMS_Menu cmsx_menuServo = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUSERVO", + .GUARD_type = OME_MENU, +#endif + .onEnter = cmsx_menuServo_onEnter, + .onExit = cmsx_menuServo_onExit, + .onGlobalExit = NULL, + .entries = cmsx_menuServoEntries +}; + +#define SERVO_MIXER_INPUT_CMS_NAMES_COUNT 23 +const char * const servoMixerInputCmsNames[SERVO_MIXER_INPUT_CMS_NAMES_COUNT] = { + "STAB ROLL", + "STAB PITCH", + "STAB YAW", + "STAB THROTTLE", + "RC ROLL", + "RC PITCH", + "RC YAW", + "RC THROTTLE", + "RC CHAN 5", + "RC CHAN 6", + "RC CHAN 7", + "RC CHAN 8", + "GIMB PITCH", + "GIMB ROLL", + "FLAPS", + "RC CHAN 9", + "RC CHAN 10", + "RC CHAN 11", + "RC CHAN 12", + "RC CHAN 13", + "RC CHAN 14", + "RC CHAN 15", + "RC CHAN 16" +}; + +static void loadServoMixerSettings(void) +{ + tmpServoMixer.targetChannel = customServoMixers(currentServoMixerIndex)->targetChannel; + tmpServoMixer.inputSource = customServoMixers(currentServoMixerIndex)->inputSource; + tmpServoMixer.rate = customServoMixers(currentServoMixerIndex)->rate; + tmpServoMixer.speed = customServoMixers(currentServoMixerIndex)->speed; +} + +static void saveServoMixerSettings(void) +{ + customServoMixersMutable(currentServoMixerIndex)->targetChannel = tmpServoMixer.targetChannel; + customServoMixersMutable(currentServoMixerIndex)->inputSource = tmpServoMixer.inputSource; + customServoMixersMutable(currentServoMixerIndex)->rate = tmpServoMixer.rate; + customServoMixersMutable(currentServoMixerIndex)->speed = tmpServoMixer.speed; + +} + +static long cmsx_menuServoMixer_onEnter(const OSD_Entry *from) +{ + UNUSED(from); + + loadServoMixerSettings(); + + return 0; +} + +static long cmsx_menuServoMixer_onExit(const OSD_Entry *from) +{ + UNUSED(from); + + saveServoMixerSettings(); + + return 0; +} + +static long cmsx_menuServoMixerIndexOnChange(displayPort_t *displayPort, const void *ptr) +{ + UNUSED(displayPort); + UNUSED(ptr); + + saveServoMixerSettings(); + currentServoMixerIndex = tmpcurrentServoMixerIndex - 1; + loadServoMixerSettings(); + + return 0; +} + +static const OSD_Entry cmsx_menuServoMixerEntries[] = +{ + OSD_LABEL_ENTRY("-- SERVO MIXER --"), + OSD_UINT8_CALLBACK_ENTRY("SERVO MIX", cmsx_menuServoMixerIndexOnChange, (&(const OSD_UINT8_t){ &tmpcurrentServoMixerIndex, 1, MAX_SERVO_RULES, 1})), + OSD_UINT8_DYN_ENTRY("SERVO", (&(const OSD_UINT8_t){ &tmpServoMixer.targetChannel, 0, MAX_SUPPORTED_SERVOS, 1})), + OSD_TAB_DYN_ENTRY("INPUT", (&(const OSD_TAB_t){ &tmpServoMixer.inputSource, SERVO_MIXER_INPUT_CMS_NAMES_COUNT - 1, servoMixerInputCmsNames})), + OSD_INT16_DYN_ENTRY("WEIGHT", (&(const OSD_INT16_t){&tmpServoMixer.rate, 0, 1000, 1})), + OSD_UINT8_DYN_ENTRY("SPEED", (&(const OSD_UINT8_t){&tmpServoMixer.speed, 0, 255, 1})), + OSD_BACK_AND_END_ENTRY +}; + +const CMS_Menu cmsx_menuServoMixer = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUSERVOMIXER", + .GUARD_type = OME_MENU, +#endif + .onEnter = cmsx_menuServoMixer_onEnter, + .onExit = cmsx_menuServoMixer_onExit, + .onGlobalExit = NULL, + .entries = cmsx_menuServoMixerEntries +}; + +static void loadMotorMixerSettings(void) +{ + tmpMotorMixerThrottle = primaryMotorMixer(currentMotorMixerIndex)->throttle * 1000; + tmpMotorMixerRoll = primaryMotorMixer(currentMotorMixerIndex)->roll * 1000; + tmpMotorMixerPitch = primaryMotorMixer(currentMotorMixerIndex)->pitch * 1000; + tmpMotorMixerYaw = primaryMotorMixer(currentMotorMixerIndex)->yaw * 1000; +} + +static void saveMotorMixerSettings(void) +{ + primaryMotorMixerMutable(currentMotorMixerIndex)->throttle = tmpMotorMixerThrottle / 1000.0f; + primaryMotorMixerMutable(currentMotorMixerIndex)->roll = tmpMotorMixerRoll / 1000.0f; + primaryMotorMixerMutable(currentMotorMixerIndex)->pitch = tmpMotorMixerPitch / 1000.0f; + primaryMotorMixerMutable(currentMotorMixerIndex)->yaw = tmpMotorMixerYaw / 1000.0f; +} + +static long cmsx_menuMotorMixer_onEnter(const OSD_Entry *from) +{ + UNUSED(from); + + loadMotorMixerSettings(); + + return 0; +} + +static long cmsx_menuMotorMixer_onExit(const OSD_Entry *self) +{ + UNUSED(self); + + saveMotorMixerSettings(); + + return 0; +} + +static long cmsx_menuMotorMixerIndexOnChange(displayPort_t *displayPort, const void *ptr) +{ + UNUSED(displayPort); + UNUSED(ptr); + + saveMotorMixerSettings(); + currentMotorMixerIndex = tmpcurrentMotorMixerIndex - 1; + loadMotorMixerSettings(); + + return 0; +} + +static const OSD_Entry cmsx_motorMixerEntries[] = +{ + OSD_LABEL_ENTRY("-- MOTOR MIXER --"), + OSD_UINT8_CALLBACK_ENTRY ("MOTOR", cmsx_menuMotorMixerIndexOnChange, (&(const OSD_UINT8_t){ &tmpcurrentMotorMixerIndex, 1, MAX_SUPPORTED_MOTORS, 1})), + OSD_UINT16_DYN_ENTRY("THROTTLE", (&(const OSD_UINT16_t){ &tmpMotorMixerThrottle, 0, 1000, 1 })), + OSD_INT16_DYN_ENTRY("ROLL", (&(const OSD_INT16_t){ &tmpMotorMixerRoll, -2000, 2000, 1 })), + OSD_INT16_DYN_ENTRY("PITCH", (&(const OSD_INT16_t){ &tmpMotorMixerPitch, -2000, 2000, 1 })), + OSD_INT16_DYN_ENTRY("YAW", (&(const OSD_INT16_t){ &tmpMotorMixerYaw, -2000, 2000, 1 })), + OSD_BACK_AND_END_ENTRY +}; + +const CMS_Menu cmsx_menuMotorMixer = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUMMOTORMIXER", + .GUARD_type = OME_MENU, +#endif + .onEnter = cmsx_menuMotorMixer_onEnter, + .onExit = cmsx_menuMotorMixer_onExit, + .onGlobalExit = NULL, + .entries = cmsx_motorMixerEntries +}; + +static const OSD_Entry cmsx_mixerServoEntries[] = +{ + OSD_LABEL_ENTRY("-- MIXER AND SERVOS --"), + OSD_SETTING_ENTRY("PLATFORM", SETTING_PLATFORM_TYPE), + OSD_SETTING_ENTRY("HAS FLAPS", SETTING_HAS_FLAPS), + OSD_SUBMENU_ENTRY("MOTOR MIXER", &cmsx_menuMotorMixer), + OSD_SUBMENU_ENTRY("SERVO MIXER", &cmsx_menuServoMixer), + OSD_SUBMENU_ENTRY("SERVOS", &cmsx_menuServo), + OSD_BACK_AND_END_ENTRY +}; + +const CMS_Menu cmsx_menuMixerServo = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUMIXERSERVO", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_mixerServoEntries +}; + +#endif diff --git a/src/main/drivers/vcd.h b/src/main/cms/cms_menu_mixer_servo.h old mode 100755 new mode 100644 similarity index 83% rename from src/main/drivers/vcd.h rename to src/main/cms/cms_menu_mixer_servo.h index 085da791555..bf6a7dd3c2f --- a/src/main/drivers/vcd.h +++ b/src/main/cms/cms_menu_mixer_servo.h @@ -17,10 +17,6 @@ #pragma once -// Video Character Display parameters +#include "cms/cms_types.h" -typedef enum { - VIDEO_SYSTEM_AUTO = 0, - VIDEO_SYSTEM_PAL, - VIDEO_SYSTEM_NTSC -} videoSystem_e; +extern const CMS_Menu cmsx_menuMixerServo; diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index e7d6f7eda93..01532efb8fb 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -52,8 +52,7 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] = OSD_SETTING_ENTRY("MID THR FOR AH", SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD), OSD_SETTING_ENTRY("MC HOVER THR", SETTING_NAV_MC_HOVER_THR), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu cmsx_menuNavSettings = { @@ -83,8 +82,7 @@ static const CMS_Menu cmsx_menuNavSettings = { OSD_SETTING_ENTRY("RTH ABORT THRES", SETTING_NAV_RTH_ABORT_THRESHOLD), OSD_SETTING_ENTRY("EMERG LANDING SPEED", SETTING_NAV_EMERG_LANDING_SPEED), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu cmsx_menuRTH = { @@ -111,8 +109,7 @@ static const OSD_Entry cmsx_menuFixedWingEntries[] = OSD_SETTING_ENTRY("PITCH TO THR RATIO", SETTING_NAV_FW_PITCH2THR), OSD_SETTING_ENTRY("LOITER RADIUS", SETTING_NAV_FW_LOITER_RADIUS), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu cmsx_menuFixedWing = { @@ -134,8 +131,7 @@ static const OSD_Entry cmsx_menuNavigationEntries[] = OSD_SUBMENU_ENTRY("RTH", &cmsx_menuRTH), OSD_SUBMENU_ENTRY("FIXED WING", &cmsx_menuFixedWing), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; const CMS_Menu cmsx_menuNavigation = { diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 7690b5aec32..5ba507addde 100755 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -35,7 +35,7 @@ #include "io/osd.h" -#define OSD_ITEM_ENTRY(label, item_id) ((OSD_Entry){ label, OME_Submenu, {.itemId = item_id}, &cmsx_menuOsdElementActions, 0 }) +#define OSD_ITEM_ENTRY(label, item_id) ((OSD_Entry){ label, {.itemId = item_id}, &cmsx_menuOsdElementActions, OME_Submenu, 0 }) static int osdCurrentLayout = -1; static int osdCurrentItem = -1; @@ -57,8 +57,7 @@ static const OSD_Entry cmsx_menuAlarmsEntries[] = { OSD_SETTING_ENTRY("MAX DIST", SETTING_OSD_DIST_ALARM), OSD_SETTING_ENTRY("MAX NEG ALT", SETTING_OSD_NEG_ALT_ALARM), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu cmsx_menuAlarms = { @@ -101,8 +100,7 @@ static const OSD_Entry menuOsdElemActionsEntries[] = { OSD_UINT8_CALLBACK_ENTRY("COLUMN", cmsx_osdElementOnChange, (&(const OSD_UINT8_t){ &osdCurrentElementColumn, 0, OSD_Y(OSD_POS_MAX), 1 })), OSD_FUNC_CALL_ENTRY("PREVIEW", osdElementPreview), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const OSD_Entry menuOsdFixedElemActionsEntries[] = { @@ -110,8 +108,7 @@ static const OSD_Entry menuOsdFixedElemActionsEntries[] = { OSD_BOOL_CALLBACK_ENTRY("ENABLED", cmsx_osdElementOnChange, &osdCurrentElementVisible), OSD_FUNC_CALL_ENTRY("PREVIEW", osdElementPreview), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static CMS_Menu cmsx_menuOsdElementActions = { @@ -197,7 +194,7 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("VARIO NUM", OSD_VARIO_NUM), #endif // defined OSD_ELEMENT_ENTRY("ALTITUDE", OSD_ALTITUDE), - OSD_ELEMENT_ENTRY("ALTITUDE MSL", OSD_ALTITUDE_MSL), + OSD_ELEMENT_ENTRY("ALTITUDE MSL", OSD_ALTITUDE_MSL), #if defined(USE_PITOT) OSD_ELEMENT_ENTRY("AIR SPEED", OSD_AIR_SPEED), #endif @@ -205,6 +202,8 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("MAP NORTH", OSD_MAP_NORTH), OSD_ELEMENT_ENTRY("MAP TAKE OFF", OSD_MAP_TAKEOFF), OSD_ELEMENT_ENTRY("RADAR", OSD_RADAR), + OSD_ELEMENT_ENTRY("MAP SCALE", OSD_MAP_SCALE), + OSD_ELEMENT_ENTRY("MAP REFERENCE", OSD_MAP_REFERENCE), #endif OSD_ELEMENT_ENTRY("EXPO", OSD_RC_EXPO), OSD_ELEMENT_ENTRY("YAW EXPO", OSD_RC_YAW_EXPO), @@ -245,11 +244,22 @@ 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("G-FORCE", OSD_GFORCE), + OSD_ELEMENT_ENTRY("G-FORCE X", OSD_GFORCE), + OSD_ELEMENT_ENTRY("G-FORCE Y", OSD_GFORCE), + OSD_ELEMENT_ENTRY("G-FORCE Z", OSD_GFORCE), + +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + OSD_ELEMENT_ENTRY("RC SOURCE", OSD_RC_SOURCE), +#endif + OSD_ELEMENT_ENTRY("IMU TEMP", OSD_IMU_TEMPERATURE), #ifdef USE_BARO OSD_ELEMENT_ENTRY("BARO TEMP", OSD_BARO_TEMPERATURE), #endif + OSD_ELEMENT_ENTRY("VTX POWER", OSD_VTX_POWER), + #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), @@ -261,15 +271,14 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("SENSOR 7 TEMP", OSD_TEMP_SENSOR_7_TEMPERATURE), #endif - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; -#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 +#if defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) && defined(USE_TEMPERATURE_SENSOR) && defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) +// All CMS OSD elements should be enabled in this case. The menu has 2 extra +// elements (label, back+end), but there's an OSD element that we intentionally // don't show here (OSD_DEBUG). -_Static_assert(ARRAYLEN(menuOsdElemsEntries) - 3 + 1 == OSD_ITEM_COUNT, "missing OSD elements in CMS"); +_Static_assert(ARRAYLEN(menuOsdElemsEntries) - 2 + 1 == OSD_ITEM_COUNT, "missing OSD elements in CMS"); #endif const CMS_Menu menuOsdElements = { @@ -301,8 +310,7 @@ static const OSD_Entry cmsx_menuOsdLayoutEntries[] = #endif #endif - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; const CMS_Menu cmsx_menuOsdLayout = { @@ -339,14 +347,11 @@ static const OSD_Entry menuOsdSettingsEntries[] = { OSD_SETTING_ENTRY("VOLT. DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS), OSD_SETTING_ENTRY("COORD. DIGITS", SETTING_OSD_COORDINATE_DIGITS), - OSD_SETTING_ENTRY("CROSSHAIRS STYLE", SETTING_OSD_CROSSHAIRS_STYLE), OSD_SETTING_ENTRY("LEFT SCROLL", SETTING_OSD_LEFT_SIDEBAR_SCROLL), OSD_SETTING_ENTRY("RIGHT SCROLL", SETTING_OSD_RIGHT_SIDEBAR_SCROLL), OSD_SETTING_ENTRY("SCROLL ARROWS", SETTING_OSD_SIDEBAR_SCROLL_ARROWS), - - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu cmsx_menuOsdSettings = { @@ -360,15 +365,66 @@ static const CMS_Menu cmsx_menuOsdSettings = { .entries = menuOsdSettingsEntries, }; +static const OSD_Entry menuOsdHud2Entries[] = { + OSD_LABEL_ENTRY("--- HUD ITEMS ---"), + + OSD_SETTING_ENTRY("HOMING ARROWS", SETTING_OSD_HUD_HOMING), + OSD_SETTING_ENTRY("HOME POINT", SETTING_OSD_HUD_HOMEPOINT), + OSD_SETTING_ENTRY("RADAR MAX AIRCRAFT", SETTING_OSD_HUD_RADAR_DISP), + OSD_SETTING_ENTRY("RADAR MIN RANGE", SETTING_OSD_HUD_RADAR_RANGE_MIN), + OSD_SETTING_ENTRY("RADAR MAX RANGE", SETTING_OSD_HUD_RADAR_RANGE_MAX), + OSD_SETTING_ENTRY("RADAR DET. NEAREST", SETTING_OSD_HUD_RADAR_NEAREST), + OSD_BACK_ENTRY, + OSD_END_ENTRY, +}; + +static const CMS_Menu cmsx_menuOsdHud2 = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUOSDH2", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuOsdHud2Entries, +}; + +static const OSD_Entry menuOsdHudEntries[] = { + OSD_LABEL_ENTRY("--- HUD ---"), + + OSD_SETTING_ENTRY("CROSSHAIRS STYLE", SETTING_OSD_CROSSHAIRS_STYLE), + OSD_SETTING_ENTRY("HORIZON OFFSET", SETTING_OSD_HORIZON_OFFSET), + OSD_SETTING_ENTRY("CAMERA UPTILT", SETTING_OSD_CAMERA_UPTILT), + OSD_SETTING_ENTRY("CAMERA FOV HORI", SETTING_OSD_CAMERA_FOV_H), + OSD_SETTING_ENTRY("CAMERA FOV VERT", SETTING_OSD_CAMERA_FOV_V), + OSD_SETTING_ENTRY("HUD MARGIN HORI", SETTING_OSD_HUD_MARGIN_H), + OSD_SETTING_ENTRY("HUD MARGIN VERT", SETTING_OSD_HUD_MARGIN_V), + OSD_SUBMENU_ENTRY("DISPLAYED ITEMS", &cmsx_menuOsdHud2), + + OSD_BACK_ENTRY, + OSD_END_ENTRY, +}; + +static const CMS_Menu cmsx_menuOsdHud = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUOSDH", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuOsdHudEntries, +}; + static const OSD_Entry menuOsdEntries[] = { OSD_LABEL_ENTRY("--- OSD ---"), OSD_SUBMENU_ENTRY("LAYOUTS", &cmsx_menuOsdLayout), OSD_SUBMENU_ENTRY("SETTINGS", &cmsx_menuOsdSettings), OSD_SUBMENU_ENTRY("ALARMS", &cmsx_menuAlarms), + OSD_SUBMENU_ENTRY("HUD", &cmsx_menuOsdHud), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/cms/cms_menu_saveexit.c b/src/main/cms/cms_menu_saveexit.c new file mode 100644 index 00000000000..7d0ef331ecd --- /dev/null +++ b/src/main/cms/cms_menu_saveexit.c @@ -0,0 +1,55 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_CMS +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_saveexit.h" + +#include "config/feature.h" + +#include "fc/config.h" +static const OSD_Entry cmsx_menuSaveExitEntries[] = +{ + OSD_LABEL_ENTRY("-- SAVE/EXIT --"), + + {"EXIT", {.func = cmsMenuExit}, (void *)CMS_EXIT, OME_OSD_Exit, 0}, + {"SAVE+EXIT", {.func = cmsMenuExit}, (void *)CMS_POPUP_SAVE, OME_OSD_Exit, 0}, + {"SAVE+REBOOT", {.func = cmsMenuExit}, (void *)CMS_POPUP_SAVEREBOOT, OME_OSD_Exit, 0}, + {"BACK", {.func = NULL}, NULL, OME_Back, 0 }, + OSD_END_ENTRY, +}; + +const CMS_Menu cmsx_menuSaveExit = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "MENUSAVE", + .GUARD_type = OME_MENU, +#endif + .entries = cmsx_menuSaveExitEntries +}; + +#endif diff --git a/src/main/cms/cms_menu_saveexit.h b/src/main/cms/cms_menu_saveexit.h new file mode 100644 index 00000000000..cf7a886a0f7 --- /dev/null +++ b/src/main/cms/cms_menu_saveexit.h @@ -0,0 +1,23 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 software. + * + * If not, see . + */ + +#pragma once + +extern const CMS_Menu cmsx_menuSaveExit; diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c index a537ba02600..aedac475ba3 100644 --- a/src/main/cms/cms_menu_vtx.c +++ b/src/main/cms/cms_menu_vtx.c @@ -128,8 +128,7 @@ static const OSD_Entry cmsx_menuVtxEntries[] = OSD_BOOL_ENTRY("LOW POWER", &masterConfig.vtx_power), #endif // USE_RTC6705 - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; const CMS_Menu cmsx_menuVtx = { diff --git a/src/main/cms/cms_menu_vtx_ffpv.c b/src/main/cms/cms_menu_vtx_ffpv.c index f0c17d335d1..d2b5719ee79 100644 --- a/src/main/cms/cms_menu_vtx_ffpv.c +++ b/src/main/cms/cms_menu_vtx_ffpv.c @@ -173,8 +173,7 @@ static const OSD_Entry ffpvCmsMenuCommenceEntries[] = OSD_LABEL_ENTRY("CONFIRM"), OSD_FUNC_CALL_ENTRY("YES", ffpvCmsCommence), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu ffpvCmsMenuCommence = { @@ -199,8 +198,7 @@ static const OSD_Entry ffpvMenuEntries[] = OSD_TAB_CALLBACK_ENTRY("POWER", ffpvCmsConfigPower, &ffpvCmsEntPower), OSD_SUBMENU_ENTRY("SET", &ffpvCmsMenuCommence), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; const CMS_Menu cmsx_menuVtxFFPV = { diff --git a/src/main/cms/cms_menu_vtx_smartaudio.c b/src/main/cms/cms_menu_vtx_smartaudio.c index a7e81bd989a..151e417437c 100644 --- a/src/main/cms/cms_menu_vtx_smartaudio.c +++ b/src/main/cms/cms_menu_vtx_smartaudio.c @@ -364,8 +364,7 @@ static const OSD_Entry saCmsMenuStatsEntries[] = { OSD_UINT16_RO_ENTRY("CRCERR", &saStat.crc), OSD_UINT16_RO_ENTRY("OOOERR", &saStat.ooopresp), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu saCmsMenuStats = { @@ -547,8 +546,7 @@ static const OSD_Entry saCmsMenuPORFreqEntries[] = OSD_UINT16_ENTRY("NEW FREQ", (&(const OSD_UINT16_t){ &saCmsORFreqNew, 5000, 5900, 1 })), OSD_FUNC_CALL_ENTRY("SET", saCmsSetPORFreq), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu saCmsMenuPORFreq = @@ -571,8 +569,7 @@ static const OSD_Entry saCmsMenuUserFreqEntries[] = OSD_UINT16_ENTRY("NEW FREQ", (&(const OSD_UINT16_t){ &saCmsUserFreqNew, 5000, 5900, 1 })), OSD_FUNC_CALL_ENTRY("SET", saCmsConfigUserFreq), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu saCmsMenuUserFreq = @@ -593,16 +590,15 @@ static const OSD_Entry saCmsMenuConfigEntries[] = { OSD_LABEL_ENTRY("- SA CONFIG -"), - { "OP MODEL", OME_TAB, {.func = saCmsConfigOpmodelByGvar}, &(const OSD_TAB_t){ &saCmsOpmodel, 2, saCmsOpmodelNames }, DYNAMIC }, - { "FSEL MODE", OME_TAB, {.func = saCmsConfigFreqModeByGvar}, &saCmsEntFselMode, DYNAMIC }, + { "OP MODEL", {.func = saCmsConfigOpmodelByGvar}, &(const OSD_TAB_t){ &saCmsOpmodel, 2, saCmsOpmodelNames }, OME_TAB, DYNAMIC }, + { "FSEL MODE", {.func = saCmsConfigFreqModeByGvar}, &saCmsEntFselMode, OME_TAB, DYNAMIC }, OSD_TAB_CALLBACK_ENTRY("PIT FMODE", saCmsConfigPitFModeByGvar, &saCmsEntPitFMode), - { "POR FREQ", OME_Submenu, {.menufunc = saCmsORFreqGetString}, (void *)&saCmsMenuPORFreq, OPTSTRING }, + { "POR FREQ", {.menufunc = saCmsORFreqGetString}, (void *)&saCmsMenuPORFreq, OME_Submenu, OPTSTRING }, #ifdef USE_EXTENDED_CMS_MENUS OSD_SUBMENU_ENTRY("STATX", &saCmsMenuStats), #endif /* USE_EXTENDED_CMS_MENUS */ - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu saCmsMenuConfig = { @@ -621,8 +617,7 @@ static const OSD_Entry saCmsMenuCommenceEntries[] = OSD_LABEL_ENTRY("CONFIRM"), OSD_FUNC_CALL_ENTRY("YES", saCmsCommence), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu saCmsMenuCommence = { @@ -646,13 +641,12 @@ static const OSD_Entry saCmsMenuFreqModeEntries[] = OSD_LABEL_ENTRY("- SMARTAUDIO -"), OSD_LABEL_FUNC_DYN_ENTRY("", saCmsDrawStatusString), - { "FREQ", OME_Submenu, {.menufunc = saCmsUserFreqGetString}, &saCmsMenuUserFreq, OPTSTRING }, + { "FREQ", {.menufunc = saCmsUserFreqGetString}, &saCmsMenuUserFreq, OME_Submenu, OPTSTRING }, OSD_TAB_CALLBACK_ENTRY("POWER", saCmsConfigPowerByGvar, &saCmsEntPower), OSD_SUBMENU_ENTRY("SET", &saCmsMenuCommence), OSD_SUBMENU_ENTRY("CONFIG", &saCmsMenuConfig), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; #pragma GCC diagnostic pop @@ -668,8 +662,7 @@ static const OSD_Entry saCmsMenuChanModeEntries[] = OSD_SUBMENU_ENTRY("SET", &saCmsMenuCommence), OSD_SUBMENU_ENTRY("CONFIG", &saCmsMenuConfig), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const OSD_Entry saCmsMenuOfflineEntries[] = @@ -681,8 +674,7 @@ static const OSD_Entry saCmsMenuOfflineEntries[] = OSD_SUBMENU_ENTRY("STATX", &saCmsMenuStats), #endif /* USE_EXTENDED_CMS_MENUS */ - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; CMS_Menu cmsx_menuVtxSmartAudio; // Forward diff --git a/src/main/cms/cms_menu_vtx_smartaudio.h b/src/main/cms/cms_menu_vtx_smartaudio.h index d8aeea3ab58..e29f05d891b 100644 --- a/src/main/cms/cms_menu_vtx_smartaudio.h +++ b/src/main/cms/cms_menu_vtx_smartaudio.h @@ -26,4 +26,4 @@ extern CMS_Menu cmsx_menuVtxSmartAudio; void saCmsUpdate(void); -void saCmsResetOpmodel(); +void saCmsResetOpmodel(void); diff --git a/src/main/cms/cms_menu_vtx_tramp.c b/src/main/cms/cms_menu_vtx_tramp.c index 4c6a1345dc0..540a78421e0 100644 --- a/src/main/cms/cms_menu_vtx_tramp.c +++ b/src/main/cms/cms_menu_vtx_tramp.c @@ -211,8 +211,7 @@ static const OSD_Entry trampCmsMenuCommenceEntries[] = OSD_LABEL_ENTRY("CONFIRM"), OSD_FUNC_CALL_ENTRY("YES", trampCmsCommence), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; static const CMS_Menu trampCmsMenuCommence = { @@ -239,8 +238,7 @@ static const OSD_Entry trampMenuEntries[] = OSD_INT16_RO_ENTRY("T(C)", &trampData.temperature), OSD_SUBMENU_ENTRY("SET", &trampCmsMenuCommence), - OSD_BACK_ENTRY, - OSD_END_ENTRY, + OSD_BACK_AND_END_ENTRY, }; const CMS_Menu cmsx_menuVtxTramp = { diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h index 5a639db503d..d56ff80e55d 100644 --- a/src/main/cms/cms_types.h +++ b/src/main/cms/cms_types.h @@ -46,6 +46,7 @@ typedef enum //wlasciwosci elementow OME_TAB, OME_END, + OME_BACK_AND_END, // Debug aid OME_MENU, @@ -60,7 +61,6 @@ typedef char * (*CMSMenuOptFuncPtr)(void); typedef struct { const char * const text; - const OSD_MenuElement type; union { const CMSEntryFuncPtr func; @@ -68,8 +68,9 @@ typedef struct int itemId; }; const void * const data; + const uint8_t type; // from OSD_MenuElement uint8_t flags; -} OSD_Entry; +} __attribute__((packed)) OSD_Entry; // Bits in flags #define PRINT_VALUE (1 << 0) // Value has been changed, need to redraw @@ -78,29 +79,33 @@ typedef struct #define OPTSTRING (1 << 3) // (Temporary) Flag for OME_Submenu, indicating func should be called to get a string to display. #define READONLY (1 << 4) // Indicates that the value is read-only and p->data points directly to it - applies to [U]INT(8|16) -#define OSD_LABEL_ENTRY(label) ((OSD_Entry){ label, OME_Label, {.func = NULL}, NULL, 0 }) -#define OSD_LABEL_DATA_ENTRY(label, data) ((OSD_Entry){ label, OME_Label, {.func = NULL}, data, 0 }) -#define OSD_LABEL_DATA_DYN_ENTRY(label, data) ((OSD_Entry){ label, OME_Label, {.func = NULL}, data, DYNAMIC }) -#define OSD_LABEL_FUNC_DYN_ENTRY(label, fn) ((OSD_Entry){ label, OME_LabelFunc, {.func = NULL}, fn, DYNAMIC }) -#define OSD_BACK_ENTRY ((OSD_Entry){ "BACK", OME_Back, {.func = NULL}, NULL, 0 }) -#define OSD_SUBMENU_ENTRY(label, menu) ((OSD_Entry){ label, OME_Submenu, {.func = NULL}, menu, 0 }) -#define OSD_FUNC_CALL_ENTRY(label, fn) ((OSD_Entry){ label, OME_Funcall, {.func = fn}, NULL, 0 }) -#define OSD_BOOL_ENTRY(label, val) ((OSD_Entry){ label, OME_Bool, {.func = NULL}, val, 0 }) -#define OSD_BOOL_CALLBACK_ENTRY(label, cb, val) ((OSD_Entry){ label, OME_Bool, {.func = cb}, val, 0 }) -#define OSD_BOOL_FUNC_ENTRY(label, fn) ((OSD_Entry){ label, OME_BoolFunc, {.func = NULL}, fn, 0 }) -#define OSD_UINT8_ENTRY(label, val) ((OSD_Entry){ label, OME_UINT8, {.func = NULL}, val, 0 }) -#define OSD_UINT8_CALLBACK_ENTRY(label, cb, val)((OSD_Entry){ label, OME_UINT8, {.func = cb}, val, 0 }) -#define OSD_UINT16_ENTRY(label, val) ((OSD_Entry){ label, OME_UINT16, {.func = NULL}, val, 0 }) -#define OSD_UINT16_DYN_ENTRY(label, val) ((OSD_Entry){ label, OME_UINT16, {.func = NULL}, val, DYNAMIC }) -#define OSD_UINT16_RO_ENTRY(label, val) ((OSD_Entry){ label, OME_UINT16, {.func = NULL}, val, DYNAMIC | READONLY }) -#define OSD_INT16_DYN_ENTRY(label, val) ((OSD_Entry){ label, OME_INT16, {.func = NULL}, val, DYNAMIC }) -#define OSD_INT16_RO_ENTRY(label, val) ((OSD_Entry){ label, OME_INT16, {.func = NULL}, val, DYNAMIC | READONLY }) -#define OSD_STRING_ENTRY(label, str) ((OSD_Entry){ label, OME_String, {.func = NULL}, str, 0 }) -#define OSD_TAB_ENTRY(label, val) ((OSD_Entry){ label, OME_TAB, {.func = NULL}, val, 0 }) -#define OSD_TAB_DYN_ENTRY(label, val) ((OSD_Entry){ label, OME_TAB, {.func = NULL}, val, DYNAMIC }) -#define OSD_TAB_CALLBACK_ENTRY(label, cb, val) ((OSD_Entry){ label, OME_TAB, {.func = cb}, val, 0 }) - -#define OSD_END_ENTRY ((OSD_Entry){ NULL, OME_END, {.func = NULL}, NULL, 0 }) +#define OSD_LABEL_ENTRY(label) ((OSD_Entry){ label, {.func = NULL}, NULL, OME_Label, 0 }) +#define OSD_LABEL_DATA_ENTRY(label, data) ((OSD_Entry){ label, {.func = NULL}, data, OME_Label, 0 }) +#define OSD_LABEL_DATA_DYN_ENTRY(label, data) ((OSD_Entry){ label, {.func = NULL}, data, OME_Label, DYNAMIC }) +#define OSD_LABEL_FUNC_DYN_ENTRY(label, fn) ((OSD_Entry){ label, {.func = NULL}, fn, OME_LabelFunc, DYNAMIC }) +#define OSD_BACK_ENTRY ((OSD_Entry){ "BACK", {.func = NULL}, NULL, OME_Back, 0 }) +#define OSD_BACK_AND_END_ENTRY ((OSD_Entry){ "BACK", {.func = NULL}, NULL, OME_BACK_AND_END, 0 }) +#define OSD_SUBMENU_ENTRY(label, menu) ((OSD_Entry){ label, {.func = NULL}, menu, OME_Submenu, 0 }) +#define OSD_FUNC_CALL_ENTRY(label, fn) ((OSD_Entry){ label, {.func = fn}, NULL, OME_Funcall, 0 }) +#define OSD_BOOL_ENTRY(label, val) ((OSD_Entry){ label, {.func = NULL}, val, OME_Bool, 0 }) +#define OSD_BOOL_CALLBACK_ENTRY(label, cb, val) ((OSD_Entry){ label, {.func = cb}, val, OME_Bool, 0 }) +#define OSD_BOOL_FUNC_ENTRY(label, fn) ((OSD_Entry){ label, {.func = NULL}, fn, OME_BoolFunc, 0 }) +#define OSD_INT8_DYN_ENTRY(label, val) ((OSD_Entry){ label, {.func = NULL}, val, OME_INT8, DYNAMIC }) +#define OSD_UINT8_ENTRY(label, val) ((OSD_Entry){ label, {.func = NULL}, val, OME_UINT8, 0 }) +#define OSD_UINT8_DYN_ENTRY(label, val) ((OSD_Entry){ label, {.func = NULL}, val, OME_UINT8, DYNAMIC }) +#define OSD_UINT8_CALLBACK_ENTRY(label, cb, val)((OSD_Entry){ label, {.func = cb}, val, OME_UINT8, 0 }) +#define OSD_UINT16_ENTRY(label, val) ((OSD_Entry){ label, {.func = NULL}, val, OME_UINT16, 0 }) +#define OSD_UINT16_DYN_ENTRY(label, val) ((OSD_Entry){ label, {.func = NULL}, val, OME_UINT16, DYNAMIC }) +#define OSD_UINT16_RO_ENTRY(label, val) ((OSD_Entry){ label, {.func = NULL}, val, OME_UINT16, DYNAMIC | READONLY }) +#define OSD_INT16_ENTRY(label, val) ((OSD_Entry){ label, {.func = NULL}, val, OME_INT16, 0 }) +#define OSD_INT16_DYN_ENTRY(label, val) ((OSD_Entry){ label, {.func = NULL}, val, OME_INT16, DYNAMIC }) +#define OSD_INT16_RO_ENTRY(label, val) ((OSD_Entry){ label, {.func = NULL}, val, OME_INT16, DYNAMIC | READONLY }) +#define OSD_STRING_ENTRY(label, str) ((OSD_Entry){ label, {.func = NULL}, str, OME_String, 0 }) +#define OSD_TAB_ENTRY(label, val) ((OSD_Entry){ label, {.func = NULL}, val, OME_TAB, 0 }) +#define OSD_TAB_DYN_ENTRY(label, val) ((OSD_Entry){ label, {.func = NULL}, val, OME_TAB, DYNAMIC }) +#define OSD_TAB_CALLBACK_ENTRY(label, cb, val) ((OSD_Entry){ label, {.func = cb}, val, OME_TAB, 0 }) + +#define OSD_END_ENTRY ((OSD_Entry){ NULL, {.func = NULL}, NULL, OME_END, 0 }) // Data type for OME_Setting. Uses upper 4 bits // of flags, leaving 16 data types. @@ -185,7 +190,7 @@ typedef struct OSD_SETTING_s { const uint8_t step; } __attribute__((packed)) OSD_SETTING_t; -#define OSD_SETTING_ENTRY_STEP_TYPE(name, setting, step, type) { name, OME_Setting, NULL, &(const OSD_SETTING_t){ setting, step }, type } +#define OSD_SETTING_ENTRY_STEP_TYPE(name, setting, step, type) { name, NULL, &(const OSD_SETTING_t){ setting, step }, OME_Setting, type } #define OSD_SETTING_ENTRY_TYPE(name, setting, type) OSD_SETTING_ENTRY_STEP_TYPE(name, setting, 0, type) #define OSD_SETTING_ENTRY_STEP(name, setting, step) OSD_SETTING_ENTRY_STEP_TYPE(name, setting, step, 0) #define OSD_SETTING_ENTRY(name, setting) OSD_SETTING_ENTRY_STEP(name, setting, 0) diff --git a/src/main/common/constants.h b/src/main/common/constants.h new file mode 100644 index 00000000000..6b2f9f69b64 --- /dev/null +++ b/src/main/common/constants.h @@ -0,0 +1,24 @@ +/* + * 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 . + */ + +#pragma once + +#define FEET_PER_MILE 5280 +#define FEET_PER_KILOFEET 1000 // Used for altitude +#define METERS_PER_KILOMETER 1000 +#define METERS_PER_MILE 1609 + diff --git a/src/main/common/filter.c b/src/main/common/filter.c index a2a7551cdb9..0f52c2f0100 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -20,6 +20,8 @@ #include #include +#include "platform.h" + #include "common/filter.h" #include "common/maths.h" #include "common/utils.h" @@ -58,7 +60,7 @@ float pt1FilterGetLastOutput(pt1Filter_t *filter) { return filter->state; } -float pt1FilterApply(pt1Filter_t *filter, float input) +float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input) { filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state); return filter->state; @@ -198,7 +200,7 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp } // Computes a biquad_t filter on a sample -float biquadFilterApply(biquadFilter_t *filter, float input) +float FAST_CODE biquadFilterApply(biquadFilter_t *filter, float input) { const float result = filter->b0 * input + filter->d1; filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2; diff --git a/src/main/common/filter.h b/src/main/common/filter.h index ae417364379..491796e4345 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -33,6 +33,11 @@ typedef struct biquadFilter_s { float d1, d2; } biquadFilter_t; +typedef union { + biquadFilter_t biquad; + pt1Filter_t pt1; +} filter_t; + typedef enum { FILTER_PT1 = 0, FILTER_BIQUAD, diff --git a/src/main/common/log.c b/src/main/common/log.c new file mode 100644 index 00000000000..cc13c2a786c --- /dev/null +++ b/src/main/common/log.c @@ -0,0 +1,193 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include +#include +#include + +#include "build/version.h" + +#include "drivers/serial.h" +#include "drivers/time.h" + +#include "common/log.h" +#include "common/printf.h" +#include "common/utils.h" + +#include "config/feature.h" +#include "config/parameter_group_ids.h" + +#include "io/serial.h" + +#include "fc/config.h" + +#include "msp/msp.h" +#include "msp/msp_serial.h" +#include "msp/msp_protocol.h" + +#if defined(USE_LOG) + +#define LOG_PREFIX "[%6d.%03d] " +#define LOG_PREFIX_FORMATTED_SIZE 13 + +static serialPort_t * logPort = NULL; +static mspPort_t * mspLogPort = NULL; + +PG_REGISTER(logConfig_t, logConfig, PG_LOG_CONFIG, 0); + +void logInit(void) +{ + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_LOG); + if (!portConfig) { + return; + } + + bool portIsSharedWithMSP = false; + + if (determinePortSharing(portConfig, FUNCTION_LOG) == PORTSHARING_SHARED) { + // We support sharing a LOG port only with MSP + if (portConfig->functionMask != (FUNCTION_LOG | FUNCTION_MSP)) { + return; + } + portIsSharedWithMSP = true; + } + + // If the port is shared with MSP, reuse the port + if (portIsSharedWithMSP) { + const serialPort_t *logAndMspPort = findSharedSerialPort(FUNCTION_LOG, FUNCTION_MSP); + if (!logAndMspPort) { + return; + } + + mspLogPort = mspSerialPortFind(logAndMspPort); + if (!mspLogPort) { + return; + } + + } else { + logPort = openSerialPort(portConfig->identifier, FUNCTION_LOG, NULL, NULL, baudRates[BAUD_921600], MODE_TX, SERIAL_NOT_INVERTED); + if (!logPort) { + return; + } + } + // Initialization done + LOG_I(SYSTEM, "%s/%s %s %s / %s (%s)", + FC_FIRMWARE_NAME, + targetName, + FC_VERSION_STRING, + buildDate, + buildTime, + shortGitRevision + ); +} + +static void logPutcp(void *p, char ch) +{ + *(*((char **) p))++ = ch; +} + +static void logPrint(const char *buf, size_t size) +{ + if (logPort) { + // Send data via UART (if configured & connected - a safeguard against zombie VCP) + if (serialIsConnected(logPort)) { + serialPrint(logPort, buf); + } + } else if (mspLogPort) { + mspSerialPushPort(MSP_DEBUGMSG, (uint8_t*)buf, size, mspLogPort, MSP_V2_NATIVE); + } +} + +static size_t logFormatPrefix(char *buf, const timeMs_t timeMs) +{ + // Write timestamp + return tfp_sprintf(buf, LOG_PREFIX, (int)(timeMs / 1000), (int)(timeMs % 1000)); +} + +static bool logHasOutput(void) +{ + return logPort || mspLogPort; +} + +static bool logIsEnabled(logTopic_e topic, unsigned level) +{ + return logHasOutput() && (level <= logConfig()->level || (logConfig()->topics & (1 << topic))); +} + +void _logf(logTopic_e topic, unsigned level, const char *fmt, ...) +{ + char buf[128]; + char *bufPtr; + int charCount; + + STATIC_ASSERT(MSP_PORT_OUTBUF_SIZE >= sizeof(buf), MSP_PORT_OUTBUF_SIZE_not_big_enough_for_log); + + if (!logIsEnabled(topic, level)) { + return; + } + + charCount = logFormatPrefix(buf, millis()); + bufPtr = &buf[charCount]; + + // Write message + va_list va; + va_start(va, fmt); + charCount += tfp_format(&bufPtr, logPutcp, fmt, va); + logPutcp(&bufPtr, '\n'); + logPutcp(&bufPtr, 0); + charCount += 2; + va_end(va); + + logPrint(buf, charCount); +} + +void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t size) +{ + // Print lines of up to maxBytes bytes. We need 5 characters per byte + // 0xAB[space|\n] + const size_t charsPerByte = 5; + const size_t maxBytes = 8; + char buf[LOG_PREFIX_FORMATTED_SIZE + charsPerByte * maxBytes + 1]; // +1 for the null terminator + size_t bufPos = LOG_PREFIX_FORMATTED_SIZE; + const uint8_t *inputPtr = buffer; + + if (!logIsEnabled(topic, level)) { + return; + } + + logFormatPrefix(buf, millis()); + + for (size_t ii = 0; ii < size; ii++) { + tfp_sprintf(buf + bufPos, "0x%02x ", inputPtr[ii]); + bufPos += charsPerByte; + if (bufPos == sizeof(buf)-1) { + buf[bufPos-1] = '\n'; + buf[bufPos] = '\0'; + logPrint(buf, bufPos + 1); + bufPos = LOG_PREFIX_FORMATTED_SIZE; + } + } + + if (bufPos > LOG_PREFIX_FORMATTED_SIZE) { + buf[bufPos-1] = '\n'; + buf[bufPos] = '\0'; + logPrint(buf, bufPos + 1); + } +} + +#endif diff --git a/src/main/common/log.h b/src/main/common/log.h new file mode 100644 index 00000000000..0d3f23fa9eb --- /dev/null +++ b/src/main/common/log.h @@ -0,0 +1,92 @@ +#pragma once + +#include +#include +#include + +#include "platform.h" + +#include "config/parameter_group.h" + +#include "common/utils.h" + +// Log levels. Defined as preprocessor constants instead of +// a number to allow compile-time comparisons. +#define LOG_LEVEL_ERROR 0 +#define LOG_LEVEL_WARNING 1 +#define LOG_LEVEL_INFO 2 +#define LOG_LEVEL_VERBOSE 3 +#define LOG_LEVEL_DEBUG 4 + +typedef enum { + LOG_TOPIC_SYSTEM, // 0, mask = 1 + LOG_TOPIC_GYRO, // 1, mask = 2 + LOG_TOPIC_BARO, // 2, mask = 4 + LOG_TOPIC_PITOT, // 3, mask = 8 + LOG_TOPIC_PWM, // 4, mask = 16 + LOG_TOPIC_TIMER, // 5, mask = 32 + LOG_TOPIC_IMU, // 6, mask = 64 + LOG_TOPIC_TEMPERATURE, // 7, mask = 128 + LOG_TOPIC_POS_ESTIMATOR, // 8, mask = 256 + + LOG_TOPIC_COUNT, +} logTopic_e; + +STATIC_ASSERT(LOG_TOPIC_COUNT < 32, too_many_log_topics); + +typedef struct logConfig_s { + uint8_t level; // from LOG_LEVEL_ constants. All messages equal or below this verbosity level are printed. + uint32_t topics; // All messages with topics in this bitmask (1 << topic) will be printed regardless of their level. +} logConfig_t; + +PG_DECLARE(logConfig_t, logConfig); + +void logInit(void); +void _logf(logTopic_e topic, unsigned level, const char *fmt, ...) __attribute__ ((format (printf, 3, 4))); +void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t size); + +// LOG_* macro definitions + +#if !defined(LOG_LEVEL_MAXIMUM) +#define LOG_LEVEL_MAXIMUM LOG_LEVEL_DEBUG +#endif + +#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_ERROR +#define LOG_E(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, fmt, ##__VA_ARGS__) +#define LOG_BUFFER_E(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, buf, size) +#else +#define LOG_E(...) +#define LOG_BUFFER_E(...) +#endif + +#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_WARNING +#define LOG_W(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, fmt, ##__VA_ARGS__) +#define LOG_BUF_W(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, buf, size) +#else +#define LOG_W(...) +#define LOG_BUF_W(...) +#endif + +#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_INFO +#define LOG_I(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, fmt, ##__VA_ARGS__) +#define LOG_BUF_I(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, buf, size) +#else +#define LOG_I(...) +#define LOG_BUF_I(...) +#endif + +#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_VERBOSE +#define LOG_V(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, fmt, ##__VA_ARGS__) +#define LOG_BUF_V(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, buf, size) +#else +#define LOG_V(...) +#define LOG_BUF_V(...) +#endif + +#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_DEBUG +#define LOG_D(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, fmt, ##__VA_ARGS__) +#define LOG_BUF_D(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, buf, size) +#else +#define LOG_D(...) +#define LOG_BUF_D(...) +#endif diff --git a/src/main/common/logic_condition.c b/src/main/common/logic_condition.c new file mode 100644 index 00000000000..7fa3055c83c --- /dev/null +++ b/src/main/common/logic_condition.c @@ -0,0 +1,272 @@ +/* + * 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 + +#include "config/config_reset.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "common/logic_condition.h" +#include "common/utils.h" +#include "rx/rx.h" +#include "maths.h" +#include "fc/fc_core.h" +#include "fc/rc_controls.h" +#include "navigation/navigation.h" +#include "sensors/battery.h" +#include "sensors/pitotmeter.h" +#include "flight/imu.h" + +PG_REGISTER_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 0); + +void pgResetFn_logicConditions(logicCondition_t *instance) +{ + for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) { + RESET_CONFIG(logicCondition_t, &instance[i], + .enabled = 0, + .operation = LOGIC_CONDITION_TRUE, + .operandA = { + .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE, + .value = 0 + }, + .operandB = { + .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE, + .value = 0 + }, + .flags = 0 + ); + } +} + +logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS]; + +void logicConditionProcess(uint8_t i) { + + if (logicConditions(i)->enabled) { + const int operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value); + const int operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value); + logicConditionStates[i].value = logicConditionCompute(logicConditions(i)->operation, operandAValue, operandBValue); + } else { + logicConditionStates[i].value = false; + } +} + +int logicConditionCompute( + logicOperation_e operation, + int operandA, + int operandB +) { + switch (operation) { + + case LOGIC_CONDITION_TRUE: + return true; + break; + + case LOGIC_CONDITION_EQUAL: + return operandA == operandB; + break; + + case LOGIC_CONDITION_GREATER_THAN: + return operandA > operandB; + break; + + case LOGIC_CONDITION_LOWER_THAN: + return operandA < operandB; + break; + + case LOGIC_CONDITION_LOW: + return operandA < 1333; + break; + + case LOGIC_CONDITION_MID: + return operandA >= 1333 && operandA <= 1666; + break; + + case LOGIC_CONDITION_HIGH: + return operandA > 1666; + break; + + case LOGIC_CONDITION_AND: + return (operandA && operandB); + break; + + case LOGIC_CONDITION_OR: + return (operandA || operandB); + break; + + case LOGIC_CONDITION_XOR: + return (operandA != operandB); + break; + + case LOGIC_CONDITION_NAND: + return !(operandA && operandB); + break; + + case LOGIC_CONDITION_NOR: + return !(operandA || operandB); + break; + + case LOGIC_CONDITION_NOT: + return !operandA; + break; + + default: + return false; + break; + } +} + +static int logicConditionGetFlightOperandValue(int operand) { + + switch (operand) { + + case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s + return constrain((uint32_t)getFlightTime(), 0, 32767); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m + return constrain(GPS_distanceToHome, 0, 32767); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m + return constrain(getTotalTravelDistance() / 100, 0, 32767); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI: + return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 10 + return getBatteryVoltage(); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE: // V / 10 + return getBatteryAverageCellVoltage(); + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100 + return getAmperage(); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh + return getMAhDrawn(); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS: + return gpsSol.numSat; + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED: // cm/s + return gpsSol.groundSpeed; + break; + + //FIXME align with osdGet3DSpeed + case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s + return (int) sqrtf(sq(gpsSol.groundSpeed) + sq((int)getEstimatedActualVelocity(Z))); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s + #ifdef USE_PITOT + return constrain(pitot.airSpeed, 0, 32767); + #else + return false; + #endif + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE: // cm + return constrain(getEstimatedActualPosition(Z), -32678, 32767); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s + return constrain(getEstimatedActualVelocity(Z), 0, 32767); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // % + return (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL: // deg + return constrain(attitude.values.roll / 10, -180, 180); + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH: // deg + return constrain(attitude.values.pitch / 10, -180, 180); + break; + + default: + return 0; + break; + } +} + +int logicConditionGetOperandValue(logicOperandType_e type, int operand) { + int retVal = 0; + + switch (type) { + + case LOGIC_CONDITION_OPERAND_TYPE_VALUE: + retVal = operand; + break; + + case LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL: + //Extract RC channel raw value + if (operand >= 1 && operand <= 16) { + retVal = rxGetChannelValue(operand - 1); + } + break; + + case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT: + retVal = logicConditionGetFlightOperandValue(operand); + break; + + case LOGIC_CONDITION_OPERAND_TYPE_LC: + if (operand >= 0 && operand < MAX_LOGIC_CONDITIONS) { + retVal = logicConditionGetValue(operand); + } + break; + + default: + break; + } + + return retVal; +} + +/* + * conditionId == -1 is always evaluated as true + */ +int logicConditionGetValue(int8_t conditionId) { + if (conditionId >= 0) { + return logicConditionStates[conditionId].value; + } else { + return true; + } +} + +void logicConditionUpdateTask(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); + for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) { + logicConditionProcess(i); + } +} diff --git a/src/main/common/logic_condition.h b/src/main/common/logic_condition.h new file mode 100644 index 00000000000..958c806a828 --- /dev/null +++ b/src/main/common/logic_condition.h @@ -0,0 +1,111 @@ +/* + * 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 "config/parameter_group.h" +#include "common/time.h" + +#define MAX_LOGIC_CONDITIONS 16 + +typedef enum { + LOGIC_CONDITION_TRUE = 0, // 0 + LOGIC_CONDITION_EQUAL, // 1 + LOGIC_CONDITION_GREATER_THAN, // 2 + LOGIC_CONDITION_LOWER_THAN, // 3 + LOGIC_CONDITION_LOW, // 4 + LOGIC_CONDITION_MID, // 5 + LOGIC_CONDITION_HIGH, // 6 + LOGIC_CONDITION_AND, // 7 + LOGIC_CONDITION_OR, // 8 + LOGIC_CONDITION_XOR, // 9 + LOGIC_CONDITION_NAND, // 10 + LOGIC_CONDITION_NOR, // 11 + LOGIC_CONDITION_NOT, // 12 + LOGIC_CONDITION_LAST +} logicOperation_e; + +typedef enum logicOperandType_s { + LOGIC_CONDITION_OPERAND_TYPE_VALUE = 0, + LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL, + LOGIC_CONDITION_OPERAND_TYPE_FLIGHT, + LOGIC_CONDITION_OPERAND_TYPE_LC, // Result of different LC and LC operand + LOGIC_CONDITION_OPERAND_TYPE_LAST +} logicOperandType_e; + +typedef enum { + LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER = 0, // in s + LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE, //in m + LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE, //in m + LOGIC_CONDITION_OPERAND_FLIGHT_RSSI, + LOGIC_CONDITION_OPERAND_FLIGHT_VBAT, // Volt / 10 + LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE, // Volt / 10 + LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT, // Amp / 100 + LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN, // mAh + LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS, + LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED, // cm/s + LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED, // cm/s + LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED, // cm/s + LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE, // cm + LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED, // cm/s + LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS, // % + LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL, // deg + LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH, // deg +} logicFlightOperands_e; + +typedef enum { + LOGIC_CONDITION_FLAG_LATCH = 1 << 0, +} logicConditionFlags_e; + +typedef struct logicOperand_s { + logicOperandType_e type; + int32_t value; +} logicOperand_t; + +typedef struct logicCondition_s { + uint8_t enabled; + logicOperation_e operation; + logicOperand_t operandA; + logicOperand_t operandB; + uint8_t flags; +} logicCondition_t; + +PG_DECLARE_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions); + +typedef struct logicConditionState_s { + int value; + uint8_t flags; +} logicConditionState_t; + +void logicConditionProcess(uint8_t i); + +int logicConditionCompute( + logicOperation_e operation, + int operandA, + int operandB +); + +int logicConditionGetOperandValue(logicOperandType_e type, int operand); + +int logicConditionGetValue(int8_t conditionId); +void logicConditionUpdateTask(timeUs_t currentTimeUs); \ No newline at end of file diff --git a/src/main/common/maths.c b/src/main/common/maths.c index ea415cfeea2..71d3d741424 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -139,6 +139,15 @@ int32_t applyDeadband(int32_t value, int32_t deadband) return value; } +float fapplyDeadbandf(float value, float deadband) +{ + if (fabsf(value) < deadband) { + return 0; + } + + return value >= 0 ? value - deadband : value + deadband; +} + int constrain(int amt, int low, int high) { if (amt < low) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 270d02b4499..4a4cee9fe8e 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -55,6 +55,10 @@ #define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD) #define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD) +#define CENTIMETERS_TO_CENTIFEET(cm) (cm * (328 / 100.0)) +#define CENTIMETERS_TO_FEET(cm) (cm * (328 / 10000.0)) +#define CENTIMETERS_TO_METERS(cm) (cm / 100) + // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70 #define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar) \ ( __extension__ ({ \ @@ -123,6 +127,7 @@ void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu int gcd(int num, int denom); int32_t applyDeadband(int32_t value, int32_t deadband); +float fapplyDeadbandf(float value, float deadband); int constrain(int amt, int low, int high); float constrainf(float amt, float low, float high); diff --git a/src/main/common/memory.c b/src/main/common/memory.c index f6aea5c057b..45817ca9f61 100644 --- a/src/main/common/memory.c +++ b/src/main/common/memory.c @@ -27,9 +27,11 @@ #include "platform.h" -#include "build/debug.h" -#include "drivers/resource.h" +#include "common/log.h" #include "common/memory.h" + +#include "drivers/resource.h" + #include "fc/runtime_config.h" #if !defined(DYNAMIC_HEAP_SIZE) @@ -50,11 +52,11 @@ void * memAllocate(size_t wantedSize, resourceOwner_e owner) retPointer = &dynHeap[dynHeapFreeWord]; dynHeapFreeWord += wantedWords; dynHeapUsage[owner] += wantedWords * sizeof(uint32_t); - DEBUG_TRACE("Memory allocated. Free memory = %d", memGetAvailableBytes()); + LOG_D(SYSTEM, "Memory allocated. Free memory = %d", memGetAvailableBytes()); } else { // OOM - DEBUG_TRACE("Out of memory"); + LOG_E(SYSTEM, "Out of memory"); ENABLE_ARMING_FLAG(ARMING_DISABLED_OOM); } diff --git a/src/main/common/printf.c b/src/main/common/printf.c index 3170e6ac7e5..340847f5683 100644 --- a/src/main/common/printf.c +++ b/src/main/common/printf.c @@ -60,7 +60,7 @@ static void *stdout_putp; // print bf, padded from left to at least n characters. // padding is zero ('0') if z!=0, space (' ') otherwise -static int putchw(void *putp, putcf putf, int n, char z, char *bf) +static int putchw(void *putp, const void *end, putcf putf, int n, char z, char *bf) { int written = 0; char fc = z ? '0' : ' '; @@ -75,30 +75,49 @@ static int putchw(void *putp, putcf putf, int n, char z, char *bf) n--; if (pr == 0) { while (n-- > 0) { - putf(putp, fc); written++; + if (putp < end) { + putf(putp, fc); + } + written++; } } while ((ch = *bf++)) { - putf(putp, ch); written++; + if (putp < end) { + putf(putp, ch); + } + written++; } if (pr == 1) { while (n-- > 0) { - putf(putp, fc); written++; + if (putp < end) { + putf(putp, fc); + } + written++; } } return written; } -// retrun number of bytes written int tfp_format(void *putp, putcf putf, const char *fmt, va_list va) +{ + return tfp_nformat(putp, -1, putf, fmt, va); +} + +// return number of bytes written +int tfp_nformat(void *putp, int size, void (*putf) (void *, char), const char *fmt, va_list va) { char bf[12]; int written = 0; char ch; + const void *end = size < 0 ? (void*)UINT32_MAX : ((char *)putp + size - 1); + while ((ch = *(fmt++))) { if (ch != '%') { - putf(putp, ch); written++; + if (putp < end) { + putf(putp, ch); + } + written++; } else { char lz = 0; char pr = 0; // padding at the right? @@ -137,7 +156,7 @@ int tfp_format(void *putp, putcf putf, const char *fmt, va_list va) else #endif ui2a(va_arg(va, unsigned int), 10, 0, bf); - written += putchw(putp, putf, w, lz, bf); + written += putchw(putp, end, putf, w, lz, bf); break; } case 'd':{ @@ -147,7 +166,7 @@ int tfp_format(void *putp, putcf putf, const char *fmt, va_list va) else #endif i2a(va_arg(va, int), bf); - written += putchw(putp, putf, w, lz, bf); + written += putchw(putp, end, putf, w, lz, bf); break; } case 'x': @@ -158,23 +177,29 @@ int tfp_format(void *putp, putcf putf, const char *fmt, va_list va) else #endif ui2a(va_arg(va, unsigned int), 16, (ch == 'X'), bf); - written += putchw(putp, putf, w, lz, bf); + written += putchw(putp, end, putf, w, lz, bf); break; case 'c': - putf(putp, (char) (va_arg(va, int))); written++; + if (putp < end) { + putf(putp, (char) (va_arg(va, int))); + } + written++; break; case 's': - written += putchw(putp, putf, w, 0, va_arg(va, char *)); + written += putchw(putp, end, putf, w, 0, va_arg(va, char *)); break; case '%': - putf(putp, ch); written++; + if (putp < end) { + putf(putp, ch); + } + written++; break; case 'n': *va_arg(va, int*) = written; break; case 'f': ftoa(va_arg(va, double), bf); - written += putchw(putp, putf, w, lz, bf); + written += putchw(putp, end, putf, w, lz, bf); break; default: break; @@ -211,12 +236,33 @@ int tfp_sprintf(char *s, const char *fmt, ...) va_list va; va_start(va, fmt); - int written = tfp_format(&s, putcp, fmt, va); - putcp(&s, 0); + int written = tfp_vsprintf(s, fmt, va); va_end(va); return written; } +int tfp_snprintf(char *s, int size, const char *fmt, ...) +{ + va_list va; + + va_start(va, fmt); + int written = tfp_vsnprintf(s, size, fmt, va); + va_end(va); + return written; +} + +int tfp_vsprintf(char *s, const char *fmt, va_list va) +{ + return tfp_vsnprintf(s, -1, fmt, va); +} + +int tfp_vsnprintf(char *s, int size, const char *fmt, va_list va) +{ + int written = tfp_nformat(&s, size, putcp, fmt, va); + putcp(&s, 0); + return written; +} + static void _putc(void *p, char c) { diff --git a/src/main/common/printf.h b/src/main/common/printf.h index 86f110a4ba0..6381434e3a1 100644 --- a/src/main/common/printf.h +++ b/src/main/common/printf.h @@ -109,13 +109,14 @@ regs Kusti, 23.10.2004 void init_printf(void *putp, void (*putf) (void *, char)); -int tfp_printf(const char *fmt, ...); -int tfp_sprintf(char *s, const char *fmt, ...); +int tfp_printf(const char *fmt, ...) __attribute__ ((format (printf, 1, 2))); +int tfp_sprintf(char *s, const char *fmt, ...) __attribute__ ((format (printf, 2, 3))); +int tfp_snprintf(char *s, int size, const char *fmt, ...) __attribute__ ((format (printf, 3, 4))); +int tfp_vsprintf(char *s, const char *fmt, va_list va) __attribute__ ((format (printf, 2, 0))); +int tfp_vsnprintf(char *s, int size, const char *fmt, va_list va) __attribute__ ((format (printf, 3, 0))); int tfp_format(void *putp, void (*putf) (void *, char), const char *fmt, va_list va); - -#define printf tfp_printf -#define sprintf tfp_sprintf +int tfp_nformat(void *putp, int n, void (*putf) (void *, char), const char *fmt, va_list va); void printfSupportInit(void); struct serialPort_s; diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 3d522e336a9..7f817d45111 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -76,7 +76,7 @@ #define PG_VTX_CONFIG 54 #define PG_ELERES_CONFIG 55 #define PG_TEMP_SENSOR_CONFIG 56 -#define PG_CF_END 57 +#define PG_CF_END 56 // Driver configuration //#define PG_DRIVER_PWM_RX_CONFIG 100 @@ -103,7 +103,10 @@ #define PG_DISPLAY_CONFIG 1013 #define PG_LIGHTS_CONFIG 1014 #define PG_PINIOBOX_CONFIG 1015 -#define PG_INAV_END 1015 +#define PG_LOGIC_CONDITIONS 1016 +#define PG_LOG_CONFIG 1017 +#define PG_RCDEVICE_CONFIG 1018 +#define PG_INAV_END 1018 // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/drivers/1-wire.c b/src/main/drivers/1-wire.c index a0c04e634d0..0811fed1744 100644 --- a/src/main/drivers/1-wire.c +++ b/src/main/drivers/1-wire.c @@ -4,9 +4,6 @@ #include "drivers/1-wire.h" #include "drivers/1-wire/ds2482.h" -#include "drivers/logging.h" - - #ifdef USE_1WIRE #ifdef USE_1WIRE_DS2482 @@ -17,7 +14,6 @@ static owDev_t ds2482Dev; 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 diff --git a/src/main/drivers/accgyro/accgyro.c b/src/main/drivers/accgyro/accgyro.c index be46ec729c1..23bd5c0b161 100644 --- a/src/main/drivers/accgyro/accgyro.c +++ b/src/main/drivers/accgyro/accgyro.c @@ -24,8 +24,8 @@ #include "build/atomic.h" #include "build/build_config.h" -#include "build/debug.h" +#include "common/log.h" #include "common/maths.h" #include "common/utils.h" @@ -62,7 +62,7 @@ const gyroFilterAndRateConfig_t * chooseGyroConfig(uint8_t desiredLpf, uint16_t } } - DEBUG_TRACE("GYRO CONFIG { %d, %d } -> { %d, %d}; regs 0x%02X, 0x%02X", + LOG_V(GYRO, "GYRO CONFIG { %d, %d } -> { %d, %d}; regs 0x%02X, 0x%02X", desiredLpf, desiredRateHz, candidate->gyroLpf, candidate->gyroRateHz, candidate->gyroConfigValues[0], candidate->gyroConfigValues[1]); diff --git a/src/main/drivers/bus.c b/src/main/drivers/bus.c index d1ec5de353d..e9b8b8f4336 100755 --- a/src/main/drivers/bus.c +++ b/src/main/drivers/bus.c @@ -27,6 +27,8 @@ #include "platform.h" #include "build/debug.h" +#include "common/memory.h" + #include "drivers/bus.h" #include "drivers/io.h" @@ -88,7 +90,9 @@ static bool busDevInit_SPI(busDevice_t * dev, const busDeviceDescriptor_t * desc dev->irqPin = IOGetByTag(descriptor->irqPin); dev->busdev.spi.spiBus = descriptor->busdev.spi.spiBus; dev->busdev.spi.csnPin = IOGetByTag(descriptor->busdev.spi.csnPin); - if (dev->busdev.spi.csnPin) { + + if (dev->busdev.spi.csnPin && spiBusInitHost(dev)) { + // Init CSN pin IOInit(dev->busdev.spi.csnPin, owner, RESOURCE_SPI_CS, 0); IOConfigGPIO(dev->busdev.spi.csnPin, SPI_IO_CS_CFG); IOHi(dev->busdev.spi.csnPin); @@ -114,13 +118,13 @@ busDevice_t * busDeviceInit(busType_e bus, devHardwareType_e hw, uint8_t tag, re if (hw == descriptor->devHwType && (bus == descriptor->busType || bus == BUSTYPE_ANY) && (tag == descriptor->tag)) { // We have a candidate - initialize device context memory busDevice_t * dev = descriptor->devicePtr; - memset(dev, 0, sizeof(busDevice_t)); + if (dev) { + memset(dev, 0, sizeof(busDevice_t)); - dev->descriptorPtr = descriptor; - dev->busType = descriptor->busType; - dev->flags = descriptor->flags; + dev->descriptorPtr = descriptor; + dev->busType = descriptor->busType; + dev->flags = descriptor->flags; - if (dev) { switch (descriptor->busType) { default: case BUSTYPE_NONE: @@ -199,16 +203,25 @@ void busSetSpeed(const busDevice_t * dev, busSpeed_e speed) uint32_t busDeviceReadScratchpad(const busDevice_t * dev) { - return dev->scratchpad[0]; + uint32_t * mem = busDeviceGetScratchpadMemory(dev); + return (mem != NULL) ? mem[0] : 0; } void busDeviceWriteScratchpad(busDevice_t * dev, uint32_t value) { - dev->scratchpad[0] = value; + uint32_t * mem = busDeviceGetScratchpadMemory(dev); + + if (mem != NULL) { + mem[0] = value; + } } void * busDeviceGetScratchpadMemory(const busDevice_t * dev) { + if (dev->scratchpad == NULL) { + ((busDevice_t *)dev)->scratchpad = memAllocate(BUS_SCRATCHPAD_MEMORY_SIZE, OWNER_SYSTEM); + } + return (void *)dev->scratchpad; } diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index a2f40d0822f..f040e869788 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -107,7 +107,8 @@ typedef enum { DEVHW_HMC5883, DEVHW_AK8963, DEVHW_AK8975, - DEVHW_IST8310, + DEVHW_IST8310_0, + DEVHW_IST8310_1, DEVHW_IST8308, DEVHW_QMC5883, DEVHW_MAG3110, @@ -191,9 +192,9 @@ typedef struct busDevice_s { #endif } busdev; IO_t irqPin; // Device IRQ pin. Bus system will only assign IO_t object to this var. Initialization is up to device driver - uint32_t scratchpad[BUS_SCRATCHPAD_MEMORY_SIZE / sizeof(uint32_t)]; // Memory where device driver can store persistent data. Zeroed out when initializing the device - // for the first time. Useful when once device is shared between several sensors - // (like MPU/ICM acc-gyro sensors) + uint32_t * scratchpad; // Memory where device driver can store persistent data. Zeroed out when initializing the device + // for the first time. Useful when once device is shared between several sensors + // (like MPU/ICM acc-gyro sensors) } busDevice_t; #ifdef __APPLE__ @@ -268,6 +269,7 @@ bool i2cBusWriteRegister(const busDevice_t * dev, uint8_t reg, uint8_t data); bool i2cBusReadBuffer(const busDevice_t * dev, uint8_t reg, uint8_t * data, uint8_t length); bool i2cBusReadRegister(const busDevice_t * dev, uint8_t reg, uint8_t * data); +bool spiBusInitHost(const busDevice_t * dev); bool spiBusIsBusy(const busDevice_t * dev); void spiBusSetSpeed(const busDevice_t * dev, busSpeed_e speed); bool spiBusTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length); diff --git a/src/main/drivers/bus_busdev_spi.c b/src/main/drivers/bus_busdev_spi.c index b5616bdd998..7fb4df2a5cf 100755 --- a/src/main/drivers/bus_busdev_spi.c +++ b/src/main/drivers/bus_busdev_spi.c @@ -29,6 +29,12 @@ #include "drivers/bus_spi.h" #include "drivers/time.h" +bool spiBusInitHost(const busDevice_t * dev) +{ + const bool spiLeadingEdge = (dev->flags & DEVFLAGS_SPI_MODE_0); + return spiInitDevice(dev->busdev.spi.spiBus, spiLeadingEdge); +} + void spiBusSelectDevice(const busDevice_t * dev) { IOLo(dev->busdev.spi.csnPin); diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c index d64e0cf51e8..1f8c6f28380 100644 --- a/src/main/drivers/bus_i2c_hal.c +++ b/src/main/drivers/bus_i2c_hal.c @@ -82,7 +82,11 @@ static i2cDevice_t i2cHardwareMap[] = { static volatile uint16_t i2cErrorCount = 0; -#define I2C_DEFAULT_TIMEOUT 10000 +// Note that I2C_TIMEOUT is in us, while the HAL +// functions expect the timeout to be in ticks. +// Since we're setting up the ticks a 1khz, each +// tick equals 1ms. +#define I2C_DEFAULT_TIMEOUT (I2C_TIMEOUT / 1000) typedef struct i2cState_s { volatile bool initialised; diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index c6b4936d421..0a3e682f6fd 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -72,23 +72,8 @@ #define SPI3_NSS_PIN NONE #endif -#ifdef SPI1_CLOCK_LEADING_EDGE -# define SPI1_LEADING_EDGE true -#else -# define SPI1_LEADING_EDGE false -#endif -#ifdef SPI2_CLOCK_LEADING_EDGE -# define SPI2_LEADING_EDGE true -#else -# define SPI2_LEADING_EDGE false -#endif -#ifdef SPI3_CLOCK_LEADING_EDGE -# define SPI3_LEADING_EDGE true -#else -# define SPI3_LEADING_EDGE false -#endif - #if defined(STM32F3) +#if defined(USE_SPI_DEVICE_1) static const uint16_t spiDivisorMapFast[] = { SPI_BaudRatePrescaler_256, // SPI_CLOCK_INITIALIZATON 281.25 KBits/s SPI_BaudRatePrescaler_128, // SPI_CLOCK_SLOW 562.5 KBits/s @@ -96,7 +81,9 @@ static const uint16_t spiDivisorMapFast[] = { SPI_BaudRatePrescaler_4, // SPI_CLOCK_FAST 18.0 MBits/s SPI_BaudRatePrescaler_4 // SPI_CLOCK_ULTRAFAST 18.0 MBits/s }; +#endif +#if defined(USE_SPI_DEVICE_2) || defined(USE_SPI_DEVICE_3) static const uint16_t spiDivisorMapSlow[] = { SPI_BaudRatePrescaler_256, // SPI_CLOCK_INITIALIZATON 140.625 KBits/s SPI_BaudRatePrescaler_64, // SPI_CLOCK_SLOW 562.5 KBits/s @@ -104,13 +91,28 @@ static const uint16_t spiDivisorMapSlow[] = { SPI_BaudRatePrescaler_2, // SPI_CLOCK_FAST 18.0 MBits/s SPI_BaudRatePrescaler_2 // SPI_CLOCK_ULTRAFAST 18.0 MBits/s }; +#endif static spiDevice_t spiHardwareMap[] = { - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, .leadingEdge = SPI1_LEADING_EDGE, .divisorMap = spiDivisorMapFast }, - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, .leadingEdge = SPI2_LEADING_EDGE, .divisorMap = spiDivisorMapSlow }, - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, .leadingEdge = SPI3_LEADING_EDGE, .divisorMap = spiDivisorMapSlow } +#ifdef USE_SPI_DEVICE_1 + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, .divisorMap = spiDivisorMapFast }, +#else + { .dev = NULL }, // No SPI1 +#endif +#ifdef USE_SPI_DEVICE_2 + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = NULL }, // No SPI2 +#endif +#ifdef USE_SPI_DEVICE_3 + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = NULL }, // No SPI3 +#endif + { .dev = NULL }, // No SPI4 }; #elif defined(STM32F4) +#if defined(USE_SPI_DEVICE_1) static const uint16_t spiDivisorMapFast[] = { SPI_BaudRatePrescaler_256, // SPI_CLOCK_INITIALIZATON 328.125 KBits/s SPI_BaudRatePrescaler_128, // SPI_CLOCK_SLOW 656.25 KBits/s @@ -118,7 +120,9 @@ static const uint16_t spiDivisorMapFast[] = { SPI_BaudRatePrescaler_4, // SPI_CLOCK_FAST 21.0 MBits/s SPI_BaudRatePrescaler_2 // SPI_CLOCK_ULTRAFAST 42.0 MBits/s }; +#endif +#if defined(USE_SPI_DEVICE_2) || defined(USE_SPI_DEVICE_3) static const uint16_t spiDivisorMapSlow[] = { SPI_BaudRatePrescaler_256, // SPI_CLOCK_INITIALIZATON 164.062 KBits/s SPI_BaudRatePrescaler_64, // SPI_CLOCK_SLOW 656.25 KBits/s @@ -126,11 +130,25 @@ static const uint16_t spiDivisorMapSlow[] = { SPI_BaudRatePrescaler_2, // SPI_CLOCK_FAST 21.0 MBits/s SPI_BaudRatePrescaler_2 // SPI_CLOCK_ULTRAFAST 21.0 MBits/s }; +#endif static spiDevice_t spiHardwareMap[] = { - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, .leadingEdge = SPI1_LEADING_EDGE, .divisorMap = spiDivisorMapFast }, - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, .leadingEdge = SPI2_LEADING_EDGE, .divisorMap = spiDivisorMapSlow }, - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, .leadingEdge = SPI3_LEADING_EDGE, .divisorMap = spiDivisorMapSlow } +#ifdef USE_SPI_DEVICE_1 + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, .divisorMap = spiDivisorMapFast }, +#else + { .dev = NULL }, // No SPI1 +#endif +#ifdef USE_SPI_DEVICE_2 + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = NULL }, // No SPI2 +#endif +#ifdef USE_SPI_DEVICE_3 + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = NULL }, // No SPI3 +#endif + { .dev = NULL }, // No SPI4 }; #else #error "Invalid CPU" @@ -150,10 +168,18 @@ SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) return SPIINVALID; } -void spiInitDevice(SPIDevice device) +bool spiInitDevice(SPIDevice device, bool leadingEdge) { spiDevice_t *spi = &(spiHardwareMap[device]); + if (!spi->dev) { + return false; + } + + if (spi->initDone) { + return true; + } + // Enable SPI clock RCC_ClockCmd(spi->rcc, ENABLE); RCC_ResetCmd(spi->rcc, ENABLE); @@ -163,7 +189,7 @@ void spiInitDevice(SPIDevice device) IOInit(IOGetByTag(spi->mosi), OWNER_SPI, RESOURCE_SPI_MOSI, device + 1); #if defined(STM32F3) || defined(STM32F4) - if (spi->leadingEdge) { + if (leadingEdge) { IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG, spi->af); IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af); IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); @@ -190,7 +216,7 @@ void spiInitDevice(SPIDevice device) spiInit.SPI_CRCPolynomial = 7; spiInit.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; - if (spi->leadingEdge) { + if (leadingEdge) { // SPI_MODE0 spiInit.SPI_CPOL = SPI_CPOL_Low; spiInit.SPI_CPHA = SPI_CPHA_1Edge; @@ -212,43 +238,9 @@ void spiInitDevice(SPIDevice device) // Drive NSS high to disable connected SPI device. IOHi(IOGetByTag(spi->nss)); } -} -bool spiInit(SPIDevice device) -{ - switch (device) { - case SPIINVALID: - return false; - case SPIDEV_1: -#ifdef USE_SPI_DEVICE_1 - spiInitDevice(device); - return true; -#else - break; -#endif - case SPIDEV_2: -#ifdef USE_SPI_DEVICE_2 - spiInitDevice(device); - return true; -#else - break; -#endif - case SPIDEV_3: -#if defined(USE_SPI_DEVICE_3) && (defined(STM32F303xC) || defined(STM32F4)) - spiInitDevice(device); - return true; -#else - break; -#endif - case SPIDEV_4: -#if defined(USE_SPI_DEVICE_4) - spiInitDevice(device); - return true; -#else - break; -#endif - } - return false; + spi->initDone = true; + return true; } uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance) diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 616a89c548f..a0970f119c0 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -70,12 +70,12 @@ typedef struct SPIDevice_s { ioTag_t miso; rccPeriphTag_t rcc; uint8_t af; - bool leadingEdge; const uint16_t * divisorMap; volatile uint16_t errorCount; + bool initDone; } spiDevice_t; -bool spiInit(SPIDevice device); +bool spiInitDevice(SPIDevice device, bool leadingEdge); bool spiIsBusBusy(SPI_TypeDef *instance); void spiSetSpeed(SPI_TypeDef *instance, SPIClockSpeed_e speed); uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in); diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index b4d5be9724d..c6f7be08e76 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -68,27 +68,7 @@ #define SPI4_NSS_PIN NONE #endif -#ifdef SPI1_CLOCK_LEADING_EDGE -# define SPI1_LEADING_EDGE true -#else -# define SPI1_LEADING_EDGE false -#endif -#ifdef SPI2_CLOCK_LEADING_EDGE -# define SPI2_LEADING_EDGE true -#else -# define SPI2_LEADING_EDGE false -#endif -#ifdef SPI3_CLOCK_LEADING_EDGE -# define SPI3_LEADING_EDGE true -#else -# define SPI3_LEADING_EDGE false -#endif -#ifdef SPI4_CLOCK_LEADING_EDGE -# define SPI4_LEADING_EDGE true -#else -# define SPI4_LEADING_EDGE false -#endif - +#if defined(USE_SPI_DEVICE_1) static const uint16_t spiDivisorMapFast[] = { LL_SPI_BAUDRATEPRESCALER_DIV256, // SPI_CLOCK_INITIALIZATON 421.875 KBits/s LL_SPI_BAUDRATEPRESCALER_DIV32, // SPI_CLOCK_SLOW 843.75 KBits/s @@ -96,6 +76,9 @@ static const uint16_t spiDivisorMapFast[] = { LL_SPI_BAUDRATEPRESCALER_DIV8, // SPI_CLOCK_FAST 13.5 MBits/s LL_SPI_BAUDRATEPRESCALER_DIV4 // SPI_CLOCK_ULTRAFAST 27.0 MBits/s }; +#endif + +#if defined(USE_SPI_DEVICE_2) || defined(USE_SPI_DEVICE_3) || defined(USE_SPI_DEVICE_4) static const uint16_t spiDivisorMapSlow[] = { LL_SPI_BAUDRATEPRESCALER_DIV256, // SPI_CLOCK_INITIALIZATON 210.937 KBits/s LL_SPI_BAUDRATEPRESCALER_DIV64, // SPI_CLOCK_SLOW 843.75 KBits/s @@ -103,12 +86,29 @@ static const uint16_t spiDivisorMapSlow[] = { LL_SPI_BAUDRATEPRESCALER_DIV4, // SPI_CLOCK_FAST 13.5 MBits/s LL_SPI_BAUDRATEPRESCALER_DIV2 // SPI_CLOCK_ULTRAFAST 27.0 MBits/s }; +#endif static spiDevice_t spiHardwareMap[] = { - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = SPI1_LEADING_EDGE, .divisorMap = spiDivisorMapFast }, - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = SPI2_LEADING_EDGE, .divisorMap = spiDivisorMapSlow }, - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = SPI3_LEADING_EDGE, .divisorMap = spiDivisorMapSlow }, - { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = SPI4_LEADING_EDGE, .divisorMap = spiDivisorMapSlow } +#ifdef USE_SPI_DEVICE_1 + { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .divisorMap = spiDivisorMapFast }, +#else + { .dev = NULL }, // No SPI1 +#endif +#ifdef USE_SPI_DEVICE_2 + { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = NULL }, // No SPI2 +#endif +#ifdef USE_SPI_DEVICE_3 + { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .divisorMap = spiDivisorMapSlow }, +#else + { .dev = NULL }, // No SPI3 +#endif +#ifdef USE_SPI_DEVICE_4 + { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .divisorMap = spiDivisorMapSlow } +#else + { .dev = NULL } // No SPI4 +#endif }; SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) @@ -138,12 +138,16 @@ void spiTimeoutUserCallback(SPI_TypeDef *instance) spiHardwareMap[device].errorCount++; } -void spiInitDevice(SPIDevice device) +bool spiInitDevice(SPIDevice device, bool leadingEdge) { spiDevice_t *spi = &(spiHardwareMap[device]); if (!spi->dev) { - return; + return false; + } + + if (spi->initDone) { + return true; } // Enable SPI clock @@ -154,7 +158,7 @@ void spiInitDevice(SPIDevice device) IOInit(IOGetByTag(spi->miso), OWNER_SPI, RESOURCE_SPI_MISO, device + 1); IOInit(IOGetByTag(spi->mosi), OWNER_SPI, RESOURCE_SPI_MOSI, device + 1); - if (spi->leadingEdge == true) { + if (leadingEdge) { IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->af); } else { IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->af); @@ -175,8 +179,8 @@ void spiInitDevice(SPIDevice device) .TransferDirection = SPI_DIRECTION_2LINES, .Mode = SPI_MODE_MASTER, .DataWidth = SPI_DATASIZE_8BIT, - .ClockPolarity = spi->leadingEdge ? SPI_POLARITY_LOW : SPI_POLARITY_HIGH, - .ClockPhase = spi->leadingEdge ? SPI_PHASE_1EDGE : SPI_PHASE_2EDGE, + .ClockPolarity = leadingEdge ? SPI_POLARITY_LOW : SPI_POLARITY_HIGH, + .ClockPhase = leadingEdge ? SPI_PHASE_1EDGE : SPI_PHASE_2EDGE, .NSS = SPI_NSS_SOFT, .BaudRate = SPI_BAUDRATEPRESCALER_8, .BitOrder = SPI_FIRSTBIT_MSB, @@ -193,44 +197,9 @@ void spiInitDevice(SPIDevice device) if (spi->nss) { IOHi(IOGetByTag(spi->nss)); } -} -bool spiInit(SPIDevice device) -{ - switch (device) - { - case SPIINVALID: - return false; - case SPIDEV_1: -#if defined(USE_SPI_DEVICE_1) - spiInitDevice(device); - return true; -#else - break; -#endif - case SPIDEV_2: -#if defined(USE_SPI_DEVICE_2) - spiInitDevice(device); - return true; -#else - break; -#endif - case SPIDEV_3: -#if defined(USE_SPI_DEVICE_3) - spiInitDevice(device); - return true; -#else - break; -#endif - case SPIDEV_4: -#if defined(USE_SPI_DEVICE_4) - spiInitDevice(device); - return true; -#else - break; -#endif - } - return false; + spi->initDone = true; + return true; } uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte) diff --git a/src/main/drivers/compass/compass_hmc5883l.c b/src/main/drivers/compass/compass_hmc5883l.c index fde3ba40f5a..efef08a0ac6 100644 --- a/src/main/drivers/compass/compass_hmc5883l.c +++ b/src/main/drivers/compass/compass_hmc5883l.c @@ -36,8 +36,6 @@ #include "drivers/bus.h" #include "drivers/light_led.h" -#include "drivers/logging.h" - #include "drivers/sensor.h" #include "drivers/compass/compass.h" diff --git a/src/main/drivers/compass/compass_ist8310.c b/src/main/drivers/compass/compass_ist8310.c index 6deb2842518..f196a4d0554 100644 --- a/src/main/drivers/compass/compass_ist8310.c +++ b/src/main/drivers/compass/compass_ist8310.c @@ -167,20 +167,22 @@ static bool deviceDetect(magDev_t * mag) bool ist8310Detect(magDev_t * mag) { - mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_IST8310, mag->magSensorToUse, OWNER_COMPASS); - if (mag->busDev == NULL) { - return false; - } + for (uint8_t index = 0; index < 2; ++index) { + mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_IST8310_0 + index, mag->magSensorToUse, OWNER_COMPASS); + if (mag->busDev == NULL) { + continue; + } - if (!deviceDetect(mag)) { - busDeviceDeInit(mag->busDev); - return false; + if (deviceDetect(mag)) { + mag->init = ist8310Init; + mag->read = ist8310Read; + return true; + } else { + busDeviceDeInit(mag->busDev); + } } - mag->init = ist8310Init; - mag->read = ist8310Read; - - return true; + return false; } #endif diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index b26838225ab..80ada2c25ea 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -229,10 +229,19 @@ bool displayGetFontMetadata(displayFontMetadata_t *metadata, const displayPort_t return false; } +int displayWriteFontCharacter(displayPort_t *instance, uint16_t addr, const osdCharacter_t *chr) +{ + if (instance->vTable->writeFontCharacter) { + return instance->vTable->writeFontCharacter(instance, addr, chr); + } + return -1; +} + void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable) { instance->vTable = vTable; instance->vTable->clearScreen(instance); + instance->useFullscreen = false; instance->cleared = true; instance->grabCount = 0; instance->cursorRow = -1; diff --git a/src/main/drivers/display.h b/src/main/drivers/display.h index 4b76a6349a3..ce46c058286 100644 --- a/src/main/drivers/display.h +++ b/src/main/drivers/display.h @@ -22,6 +22,8 @@ #include "config/parameter_group.h" +typedef struct osdCharacter_s osdCharacter_t; + typedef struct displayConfig_s { bool force_sw_blink; // Enable SW blinking. Used for chips which don't work correctly with HW blink. } displayConfig_t; @@ -69,6 +71,7 @@ typedef struct displayPort_s { uint8_t posY; // CMS state + bool useFullscreen; bool cleared; int8_t cursorRow; int8_t grabCount; @@ -91,6 +94,7 @@ typedef struct displayPortVTable_s { uint32_t (*txBytesFree)(const displayPort_t *displayPort); textAttributes_t (*supportedTextAttributes)(const displayPort_t *displayPort); bool (*getFontMetadata)(displayFontMetadata_t *metadata, const displayPort_t *displayPort); + int (*writeFontCharacter)(displayPort_t *instance, uint16_t addr, const osdCharacter_t *chr); } displayPortVTable_t; typedef struct displayPortProfile_s { @@ -119,4 +123,5 @@ void displayHeartbeat(displayPort_t *instance); void displayResync(displayPort_t *instance); uint16_t displayTxBytesFree(const displayPort_t *instance); bool displayGetFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *instance); +int displayWriteFontCharacter(displayPort_t *instance, uint16_t addr, const osdCharacter_t *chr); void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable); diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index d91d5d5bc5d..ce19cea6713 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -43,14 +43,16 @@ #include "drivers/timer.h" #include "drivers/light_ws2811strip.h" -static uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +#define WS2811_PERIOD (WS2811_TIMER_HZ / WS2811_CARRIER_HZ) +#define WS2811_BIT_COMPARE_1 ((WS2811_PERIOD * 2) / 3) +#define WS2811_BIT_COMPARE_0 (WS2811_PERIOD / 3) + +static timerDMASafeType_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; + static IO_t ws2811IO = IO_NONE; static TCH_t * ws2811TCH = NULL; static bool ws2811Initialised = false; -static uint16_t BIT_COMPARE_1 = 0; -static uint16_t BIT_COMPARE_0 = 0; - static hsvColor_t ledColorBuffer[WS2811_LED_STRIP_LENGTH]; void setLedHsv(uint16_t index, const hsvColor_t *color) @@ -103,10 +105,7 @@ void ws2811LedStripInit(void) } /* Compute the prescaler value */ - uint16_t period = WS2811_TIMER_HZ / WS2811_CARRIER_HZ; - - BIT_COMPARE_1 = period * 2 / 3; - BIT_COMPARE_0 = period / 3; + uint8_t period = WS2811_TIMER_HZ / WS2811_CARRIER_HZ; ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); @@ -116,7 +115,7 @@ void ws2811LedStripInit(void) timerPWMConfigChannel(ws2811TCH, 0); // If DMA failed - abort - if (!timerPWMConfigChannelDMA(ws2811TCH, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE)) { + if (!timerPWMConfigChannelDMA(ws2811TCH, ledStripDMABuffer, sizeof(ledStripDMABuffer[0]), WS2811_DMA_BUFFER_SIZE)) { ws2811Initialised = false; return; } @@ -141,7 +140,7 @@ STATIC_UNIT_TESTED void fastUpdateLEDDMABuffer(rgbColor24bpp_t *color) uint32_t grb = (color->rgb.g << 16) | (color->rgb.r << 8) | (color->rgb.b); for (int8_t index = 23; index >= 0; index--) { - ledStripDMABuffer[dmaBufferOffset++] = (grb & (1 << index)) ? BIT_COMPARE_1 : BIT_COMPARE_0; + ledStripDMABuffer[dmaBufferOffset++] = (grb & (1 << index)) ? WS2811_BIT_COMPARE_1 : WS2811_BIT_COMPARE_0; } } diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 85337a9d466..d70a255d88a 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -26,7 +26,7 @@ #define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH) // number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes) -#define WS2811_TIMER_HZ 24000000 +#define WS2811_TIMER_HZ 2400000 #define WS2811_CARRIER_HZ 800000 void ws2811LedStripInit(void); diff --git a/src/main/drivers/logging.c b/src/main/drivers/logging.c deleted file mode 100755 index 703ad7d7eb0..00000000000 --- a/src/main/drivers/logging.c +++ /dev/null @@ -1,151 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include -#include - -#include "platform.h" - -#include "common/utils.h" - -#include "drivers/logging.h" - -#ifdef USE_BOOTLOG - -#include "drivers/time.h" - -static bootLogEntry_t events[MAX_BOOTLOG_ENTRIES]; -static int eventCount; -static bool eventOverflow; - -#ifdef BOOTLOG_DESCRIPTIONS -static const char * eventDescription[BOOT_EVENT_CODE_COUNT] = { - [BOOT_EVENT_CONFIG_LOADED] = "CONFIG_LOADED", - [BOOT_EVENT_SYSTEM_INIT_DONE] = "SYSTEM_INIT_DONE", - [BOOT_EVENT_PWM_INIT_DONE] = "PWM_INIT_DONE", - [BOOT_EVENT_EXTRA_BOOT_DELAY] = "EXTRA_BOOT_DELAY", - [BOOT_EVENT_SENSOR_INIT_DONE] = "SENSOR_INIT_DONE", - [BOOT_EVENT_GPS_INIT_DONE] = "GPS_INIT_DONE", - [BOOT_EVENT_LEDSTRIP_INIT_DONE] = "LEDSTRIP_INIT_DONE", - [BOOT_EVENT_TELEMETRY_INIT_DONE] = "TELEMETRY_INIT_DONE", - [BOOT_EVENT_SYSTEM_READY] = "SYSTEM_READY", - [BOOT_EVENT_GYRO_DETECTION] = "GYRO_DETECTION", - [BOOT_EVENT_ACC_DETECTION] = "ACC_DETECTION", - [BOOT_EVENT_BARO_DETECTION] = "BARO_DETECTION", - [BOOT_EVENT_MAG_DETECTION] = "MAG_DETECTION", - [BOOT_EVENT_RANGEFINDER_DETECTION] = "RANGEFINDER_DETECTION", - [BOOT_EVENT_MAG_INIT_FAILED] = "MAG_INIT_FAILED", - [BOOT_EVENT_HMC5883L_READ_OK_COUNT] = "HMC5883L_READ_OK_COUNT", - [BOOT_EVENT_HMC5883L_READ_FAILED] = "HMC5883L_READ_FAILED", - [BOOT_EVENT_HMC5883L_SATURATION] = "HMC5883L_SATURATION", - [BOOT_EVENT_TIMER_CH_SKIPPED] = "TIMER_CHANNEL_SKIPPED", - [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", -}; - -const char * getBootlogEventDescription(bootLogEventCode_e eventCode) -{ - if (eventCode < BOOT_EVENT_CODE_COUNT) { - return eventDescription[eventCode]; - } - else { - return NULL; - } -} -#endif - -void initBootlog(void) -{ - memset(events, 0, sizeof(events)); - eventCount = 0; - eventOverflow = false; -} - -int getBootlogEventCount(void) -{ - return eventCount; -} - -bootLogEntry_t * getBootlogEvent(int index) -{ - if (index <= eventCount) { - return &events[index]; - } - else { - return 0; - } -} - -static void addBootlogEntry(bootLogEntry_t * event) -{ - if (eventCount >= MAX_BOOTLOG_ENTRIES) { - eventOverflow = true; - return; - } - - memcpy(&events[eventCount], event, sizeof(bootLogEntry_t)); - events[eventCount].timestamp = millis(); - - eventCount++; -} - -void addBootlogEvent2(bootLogEventCode_e eventCode, uint16_t eventFlags) -{ - bootLogEntry_t event; - event.eventCode = eventCode; - event.eventFlags = eventFlags; - addBootlogEntry(&event); -} - -void addBootlogEvent4(bootLogEventCode_e eventCode, uint16_t eventFlags, uint32_t param1, uint32_t param2) -{ - bootLogEntry_t event; - event.eventCode = eventCode; - event.eventFlags = eventFlags | BOOT_EVENT_FLAGS_PARAM32; - event.params.u32[0] = param1; - event.params.u32[1] = param2; - addBootlogEntry(&event); -} - -void addBootlogEvent6(bootLogEventCode_e eventCode, uint16_t eventFlags, uint16_t param1, uint16_t param2, uint16_t param3, uint16_t param4) -{ - bootLogEntry_t event; - event.eventCode = eventCode; - event.eventFlags = eventFlags | BOOT_EVENT_FLAGS_PARAM16; - event.params.u16[0] = param1; - event.params.u16[1] = param2; - event.params.u16[2] = param3; - event.params.u16[3] = param4; - addBootlogEntry(&event); -} -#else -const char * getBootlogEventDescription(bootLogEventCode_e eventCode) {UNUSED(eventCode);return NULL;} -void initBootlog(void) {} -int getBootlogEventCount(void) {return 0;} -bootLogEntry_t * getBootlogEvent(int index) {UNUSED(index);return NULL;} -void addBootlogEvent2(bootLogEventCode_e eventCode, uint16_t eventFlags) - {UNUSED(eventCode);UNUSED(eventFlags);} -void addBootlogEvent4(bootLogEventCode_e eventCode, uint16_t eventFlags, uint32_t param1, uint32_t param2) - {UNUSED(eventCode);UNUSED(eventFlags);UNUSED(param1);UNUSED(param2);} -void addBootlogEvent6(bootLogEventCode_e eventCode, uint16_t eventFlags, uint16_t param1, uint16_t param2, uint16_t param3, uint16_t param4) - {UNUSED(eventCode);UNUSED(eventFlags);UNUSED(param1);UNUSED(param2);UNUSED(param3);UNUSED(param4);} -#endif diff --git a/src/main/drivers/logging.h b/src/main/drivers/logging.h deleted file mode 100755 index 11ec091650f..00000000000 --- a/src/main/drivers/logging.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -#include "logging_codes.h" - -typedef struct bootLogEntry_s { - uint32_t timestamp; - uint16_t eventCode; - uint16_t eventFlags; - union { - uint16_t u16[4]; - uint32_t u32[2]; - } params; -} bootLogEntry_t; - -void initBootlog(void); -int getBootlogEventCount(void); -bootLogEntry_t * getBootlogEvent(int index); -const char * getBootlogEventDescription(bootLogEventCode_e eventCode); -void addBootlogEvent2(bootLogEventCode_e eventCode, bootLogFlags_e eventFlags); -void addBootlogEvent4(bootLogEventCode_e eventCode, bootLogFlags_e eventFlags, uint32_t param1, uint32_t param2); -void addBootlogEvent6(bootLogEventCode_e eventCode, bootLogFlags_e eventFlags, uint16_t param1, uint16_t param2, uint16_t param3, uint16_t param4); diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 24c52cc8b20..02906f5a13b 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -29,15 +29,13 @@ #include "common/utils.h" #include "drivers/bus.h" -#include "drivers/light_led.h" +#include "drivers/dma.h" #include "drivers/io.h" -#include "drivers/time.h" +#include "drivers/light_led.h" #include "drivers/nvic.h" -#include "drivers/dma.h" -#include "drivers/vcd.h" +#include "drivers/time.h" #include "max7456.h" -#include "max7456_symbols.h" // VM0 bits #define VIDEO_BUFFER_DISABLE 0x01 @@ -637,7 +635,7 @@ void max7456RefreshAll(void) } } -void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr) +void max7456ReadNvm(uint16_t char_address, osdCharacter_t *chr) { // Check if device is available if (state.dev == NULL) { @@ -660,7 +658,7 @@ void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr) max7456WaitUntilNoBusy(); - for (unsigned ii = 0; ii < sizeof(chr->data); ii++) { + for (unsigned ii = 0; ii < OSD_CHAR_VISIBLE_BYTES; ii++) { busWrite(state.dev, MAX7456ADD_CMAL, ii); busRead(state.dev, MAX7456ADD_CMDO, &chr->data[ii]); } @@ -669,7 +667,7 @@ void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr) max7456Unlock(); } -void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr) +void max7456WriteNvm(uint16_t char_address, const osdCharacter_t *chr) { uint8_t spiBuff[(sizeof(chr->data) * 2 + 2) * 2]; int bufPtr = 0; @@ -701,7 +699,7 @@ void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr) or_val = addr_h << 6; } - for (unsigned x = 0; x < sizeof(chr->data); x++) { + for (unsigned x = 0; x < OSD_CHAR_VISIBLE_BYTES; x++) { bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMAL, x | or_val); //set start address low bufPtr = max7456PrepareBuffer(spiBuff, bufPtr, MAX7456ADD_CMDI, chr->data[x]); } diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index be894adde13..12728d706b5 100644 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -18,7 +18,8 @@ #pragma once #include -#include "drivers/vcd.h" + +#include "drivers/osd.h" #ifndef WHITEBRIGHTNESS #define WHITEBRIGHTNESS 0x01 @@ -41,14 +42,10 @@ enum VIDEO_TYPES { AUTO = 0, PAL, NTSC }; #define MAX7456_MODE_BLINK (1 << 4) #define MAX7456_MODE_SOLID_BG (1 << 5) -typedef struct max7456Character_s { - uint8_t data[54]; -} max7456Character_t; - void max7456Init(const videoSystem_e videoSystem); void max7456Update(void); -void max7456ReadNvm(uint16_t char_address, max7456Character_t *chr); -void max7456WriteNvm(uint16_t char_address, const max7456Character_t *chr); +void max7456ReadNvm(uint16_t char_address, osdCharacter_t *chr); +void max7456WriteNvm(uint16_t char_address, const osdCharacter_t *chr); uint16_t max7456GetScreenSize(void); uint8_t max7456GetRowsCount(void); void max7456Write(uint8_t x, uint8_t y, const char *buff, uint8_t mode); diff --git a/src/main/drivers/osd.h b/src/main/drivers/osd.h new file mode 100644 index 00000000000..895ddfc552a --- /dev/null +++ b/src/main/drivers/osd.h @@ -0,0 +1,59 @@ +/* + * This file is part of Cleanflight, Betaflight and INAV + * + * Cleanflight, Betaflight and INAV are free software. You can + * redistribute this software and/or modify this software 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, Betaflight and INAV are distributed in the hope that + * they 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 software. + * + * If not, see . + */ + +#pragma once + +#include + +#define OSD_CHAR_WIDTH 12 +#define OSD_CHAR_HEIGHT 18 +#define OSD_CHAR_BITS_PER_PIXEL 2 +#define OSD_CHAR_VISIBLE_BYTES (OSD_CHAR_WIDTH * OSD_CHAR_HEIGHT * OSD_CHAR_BITS_PER_PIXEL / 8) +// Only the first 54 bytes of a character represent visible data. However, some OSD drivers +// accept 64 bytes and use the extra 10 bytes for metadata. +#define OSD_CHAR_BYTES 64 + +#define OSD_CHARACTER_COLOR_BLACK 0 +#define OSD_CHARACTER_COLOR_TRANSPARENT 1 +#define OSD_CHARACTER_COLOR_WHITE 2 + +// 3 is unused but it's interpreted as transparent by all drivers + + +// Video Character Display parameters + +typedef enum { + VIDEO_SYSTEM_AUTO = 0, + VIDEO_SYSTEM_PAL, + VIDEO_SYSTEM_NTSC +} videoSystem_e; + +typedef enum { + OSD_DRIVER_NONE = 0, + OSD_DRIVER_MAX7456 = 1, +} osdDriver_e; + +// osdCharacter_t represents the binary data for an OSD +// character. All OSD drivers use the same character format +// as defined by OSD_CHARACTER_WIDTH, OSD_CHARACTER_HEIGHT +// and OSD_CHARACTER_BITS_PER_PIXEL. +typedef struct osdCharacter_s { + uint8_t data[OSD_CHAR_BYTES]; +} osdCharacter_t; diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/osd_symbols.h similarity index 76% rename from src/main/drivers/max7456_symbols.h rename to src/main/drivers/osd_symbols.h index 56fd7115d0d..51384fa9b1b 100644 --- a/src/main/drivers/max7456_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -1,5 +1,5 @@ -/* @file max7456_symbols.h - * @brief max7456 symbols for the mwosd font set +/* @file osd_symbols.h + * @brief Based on max7456 symbols for the mwosd font set * * @author Nathan Tsoi nathan@vertile.com * @@ -21,11 +21,11 @@ #pragma once -#ifdef USE_MAX7456 +#ifdef USE_OSD #define SYM_RSSI 0x01 // 001 Icon RSSI -#define SYM_AH_RIGHT 0x02 // 002 Arrow left -#define SYM_AH_LEFT 0x03 // 003 Arrow right +#define SYM_AH_LEFT 0x02 // 002 Arrow left +#define SYM_AH_RIGHT 0x03 // 003 Arrow right #define SYM_THR 0x04 // 004 Throttle #define SYM_AH_DECORATION_UP 0x05 // 005 Arrow up AHI #define SYM_VOLT 0x06 // 006 V @@ -53,8 +53,8 @@ #define SYM_HEADING_S 0x19 // 025 Heading Graphic south #define SYM_HEADING_E 0x1A // 026 Heading Graphic east #define SYM_HEADING_W 0x1B // 027 Heading Graphic west -#define SYM_HEADING_DIVIDED_LINE 0x1C // 028 Heading Graphic -#define SYM_HEADING_LINE 0x1D // 029 Heading Graphic +#define SYM_HEADING_DIVIDED_LINE 0x1C // 028 Heading Graphic +#define SYM_HEADING_LINE 0x1D // 029 Heading Graphic #define SYM_SAT_L 0x1E // 030 Sats left #define SYM_SAT_R 0x1F // 031 Sats right @@ -63,7 +63,8 @@ // 0x21 // 033 ASCII ! -#define SYM_TRIP_DIST 0x22 // 034 Icon total distance +#define SYM_TRIP_DIST 0x22 // 034 Trip distance +#define SYM_TOTAL 0x22 // 034 Total // 0x23 // 035 ASCII # @@ -124,13 +125,13 @@ #define SYM_BATT_FULL 0x90 // 144 Battery full #define SYM_BATT_5 0x91 // 145 Battery #define SYM_BATT_4 0x92 // 146 Battery -#define SYM_BATT_3 0x93 // 147 Battery +#define SYM_BATT_3 0x93 // 147 Battery #define SYM_BATT_2 0x94 // 148 Battery #define SYM_BATT_1 0x95 // 149 Battery #define SYM_BATT_EMPTY 0x96 // 150 Battery empty #define SYM_AIR 0x97 // 151 Air speed -// 0x98 // 152 Home point map +// 0x98 // 152 Home point map #define SYM_FTS 0x99 // 153 FT/S #define SYM_AMP 0x9A // 154 A #define SYM_ON_M 0x9B // 155 On MN @@ -187,27 +188,59 @@ #define SYM_ZERO_HALF_LEADING_DOT 0xD0 // 208 to 217 Numbers with leading dot #define SYM_AH_CH_AIRCRAFT0 0xDA // 218 Crossair aircraft left -#define SYM_AH_CH_AIRCRAFT1 0xDB // 219 Crossair aircraft +#define SYM_AH_CH_AIRCRAFT1 0xDB // 219 Crossair aircraft #define SYM_AH_CH_AIRCRAFT2 0xDC // 220 Crossair aircraft center -#define SYM_AH_CH_AIRCRAFT3 0xDD // 221 Crossair aircraft +#define SYM_AH_CH_AIRCRAFT3 0xDD // 221 Crossair aircraft #define SYM_AH_CH_AIRCRAFT4 0xDE // 222 Crossair aircraft right #define SYM_PITCH_DOWN 0xDF // 223 Pitch down #define SYM_AH_V_START 0xE0 // 224 to 229 Vertical AHI -#define SYM_BARO_TEMP 0xF0 -#define SYM_IMU_TEMP 0xF1 -#define SYM_TEMP 0xF2 +#define SYM_GFORCE 0xE6 // 230 Gforce (all axis) +#define SYM_GFORCE_X 0xE7 // 231 Gforce X +#define SYM_GFORCE_Y 0xE8 // 232 Gforce Y +#define SYM_GFORCE_Z 0xE9 // 233 Gforce Z -#define SYM_TEMP_SENSOR_FIRST 0xF2 -#define SYM_TEMP_SENSOR_LAST 0xF7 +#define SYM_BARO_TEMP 0xF0 // 240 +#define SYM_IMU_TEMP 0xF1 // 241 +#define SYM_TEMP 0xF2 // 242 + +#define SYM_TEMP_SENSOR_FIRST 0xF2 // 242 +#define SYM_TEMP_SENSOR_LAST 0xF7 // 247 #define TEMP_SENSOR_SYM_COUNT (SYM_TEMP_SENSOR_LAST - SYM_TEMP_SENSOR_FIRST + 1) +#define SYM_HUD_SIGNAL_0 0xF8 // 248 Hud signal icon Lost +#define SYM_HUD_SIGNAL_1 0xF9 // 249 Hud signal icon 25% +#define SYM_HUD_SIGNAL_2 0xFA // 250 Hud signal icon 50% +#define SYM_HUD_SIGNAL_3 0xFB // 251 Hud signal icon 75% +#define SYM_HUD_SIGNAL_4 0xFC // 252 Hud signal icon 100% + #define SYM_LOGO_START 0x101 // 257 to 280, INAV logo #define SYM_LOGO_WIDTH 6 #define SYM_LOGO_HEIGHT 4 -#define SYM_CURSOR SYM_AH_LEFT // Menu cursor - -#endif // USE_MAX7456 +#define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3 +#define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4 +#define SYM_AH_CH_TYPE5 0x196 // 406 to 408, crosshair 5 +#define SYM_AH_CH_TYPE6 0x199 // 409 to 411, crosshair 6 +#define SYM_AH_CH_TYPE7 0x19C // 412 to 414, crosshair 7 + +#define SYM_HUD_ARROWS_L1 0x1A2 // 418 1 arrow left +#define SYM_HUD_ARROWS_L2 0x1A3 // 419 2 arrows left +#define SYM_HUD_ARROWS_L3 0x1A4 // 420 3 arrows left +#define SYM_HUD_ARROWS_R1 0x1A5 // 421 1 arrow right +#define SYM_HUD_ARROWS_R2 0x1A6 // 422 2 arrows right +#define SYM_HUD_ARROWS_R3 0x1A7 // 423 3 arrows right +#define SYM_HUD_ARROWS_U1 0x1A8 // 424 1 arrow up +#define SYM_HUD_ARROWS_U2 0x1A9 // 425 2 arrows up +#define SYM_HUD_ARROWS_U3 0x1AA // 426 3 arrows up +#define SYM_HUD_ARROWS_D1 0x1AB // 427 1 arrow down +#define SYM_HUD_ARROWS_D2 0x1AC // 428 2 arrows down +#define SYM_HUD_ARROWS_D3 0x1AD // 429 3 arrows down + +#else + +#define TEMP_SENSOR_SYM_COUNT 0 + +#endif // USE_OSD diff --git a/src/main/drivers/pitotmeter_virtual.c b/src/main/drivers/pitotmeter_virtual.c new file mode 100644 index 00000000000..4ef87375e8d --- /dev/null +++ b/src/main/drivers/pitotmeter_virtual.c @@ -0,0 +1,91 @@ +/* + * 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 . + */ + +#include +#include + +#include + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/axis.h" + +#include "config/feature.h" + +#include "common/maths.h" +#include "common/utils.h" + +#include "fc/config.h" + +#include "flight/pid.h" +#include "flight/wind_estimator.h" + +#include "io/gps.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#include "sensors/pitotmeter.h" + +#include "pitotmeter.h" +#include "pitotmeter_virtual.h" + +#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) + +static bool virtualPitotStart(pitotDev_t *pitot) +{ + UNUSED(pitot); + return true; +} + +static bool virtualPitotRead(pitotDev_t *pitot) +{ + UNUSED(pitot); + return true; +} + +static void virtualPitotCalculate(pitotDev_t *pitot, float *pressure, float *temperature) +{ + UNUSED(pitot); + float airSpeed = 0.0f; + if (pitotIsCalibrationComplete()) { + if (isEstimatedWindSpeedValid()) { + uint16_t windHeading; //centidegrees + float windSpeed = getEstimatedHorizontalWindSpeed(&windHeading); //cm/s + float horizontalWindSpeed = windSpeed * cos_approx(CENTIDEGREES_TO_RADIANS(windHeading - posControl.actualState.yaw)); //yaw int32_t centidegrees + airSpeed = posControl.actualState.velXY - horizontalWindSpeed; //float cm/s or gpsSol.groundSpeed int16_t cm/s + } else { + airSpeed = pidProfile()->fixedWingReferenceAirspeed; //float cm/s + } + } + if (pressure) + //*pressure = sq(airSpeed / 100) * AIR_DENSITY_SEA_LEVEL_15C / 2 + P0; + *pressure = sq(airSpeed) * AIR_DENSITY_SEA_LEVEL_15C / 20000.0f + P0; + if (temperature) + *temperature = 288.15f; // Temperature at standard sea level (288.15 K) +} + +bool virtualPitotDetect(pitotDev_t *pitot) +{ + pitot->delay = 10000; + pitot->start = virtualPitotStart; + pitot->get = virtualPitotRead; + pitot->calculate = virtualPitotCalculate; + return feature(FEATURE_GPS); +} +#endif diff --git a/src/main/drivers/pitotmeter_virtual.h b/src/main/drivers/pitotmeter_virtual.h new file mode 100644 index 00000000000..f59162dd028 --- /dev/null +++ b/src/main/drivers/pitotmeter_virtual.h @@ -0,0 +1,20 @@ +/* + * 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 . + */ + +#pragma once + +bool virtualPitotDetect(pitotDev_t *pitot); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 2bf6fbe290e..00e1f434898 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -21,235 +21,351 @@ #include "platform.h" +#include "build/debug.h" +#include "common/log.h" #include "common/memory.h" + +#include "config/feature.h" + +#include "fc/config.h" + #include "drivers/io.h" #include "drivers/io_impl.h" #include "drivers/timer.h" -#include "drivers/logging.h" - #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" -#include "drivers/rx_pwm.h" +#include "drivers/serial.h" +#include "drivers/serial_uart.h" +//#include "drivers/rx_pwm.h" + +#include "sensors/rangefinder.h" + +#include "io/serial.h" enum { MAP_TO_NONE, - MAP_TO_PPM_INPUT, - MAP_TO_PWM_INPUT, MAP_TO_MOTOR_OUTPUT, MAP_TO_SERVO_OUTPUT, }; -static pwmIOConfiguration_t pwmIOConfiguration; +typedef struct { + int maxTimMotorCount; + int maxTimServoCount; + const timerHardware_t * timMotors[MAX_PWM_OUTPUT_PORTS]; + const timerHardware_t * timServos[MAX_PWM_OUTPUT_PORTS]; +} timMotorServoHardware_t; + +static pwmInitError_e pwmInitError = PWM_INIT_ERROR_NONE; + +static const char * pwmInitErrorMsg[] = { + /* PWM_INIT_ERROR_NONE */ "No error", + /* PWM_INIT_ERROR_TOO_MANY_MOTORS */ "Mixer defines too many motors", + /* PWM_INIT_ERROR_TOO_MANY_SERVOS */ "Mixer defines too many servos", + /* PWM_INIT_ERROR_NOT_ENOUGH_MOTOR_OUTPUTS */ "Not enough motor outputs/timers", + /* PWM_INIT_ERROR_NOT_ENOUGH_SERVO_OUTPUTS */ "Not enough servo outputs/timers", + /* PWM_INIT_ERROR_TIMER_INIT_FAILED */ "Output timer init failed" +}; -pwmIOConfiguration_t *pwmGetOutputConfiguration(void) +static const motorProtocolProperties_t motorProtocolProperties[] = { + [PWM_TYPE_STANDARD] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false }, + [PWM_TYPE_ONESHOT125] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false }, + [PWM_TYPE_ONESHOT42] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false }, + [PWM_TYPE_MULTISHOT] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false }, + [PWM_TYPE_BRUSHED] = { .usesHwTimer = true, .isDSHOT = false, .isSerialShot = false }, + [PWM_TYPE_DSHOT150] = { .usesHwTimer = true, .isDSHOT = true, .isSerialShot = false }, + [PWM_TYPE_DSHOT300] = { .usesHwTimer = true, .isDSHOT = true, .isSerialShot = false }, + [PWM_TYPE_DSHOT600] = { .usesHwTimer = true, .isDSHOT = true, .isSerialShot = false }, + [PWM_TYPE_DSHOT1200] = { .usesHwTimer = true, .isDSHOT = true, .isSerialShot = false }, + [PWM_TYPE_SERIALSHOT] = { .usesHwTimer = false, .isDSHOT = false, .isSerialShot = true }, +}; + +pwmInitError_e getPwmInitError(void) { - return &pwmIOConfiguration; + return pwmInitError; } -bool CheckGPIOPin(ioTag_t tag, GPIO_TypeDef *gpio, uint16_t pin) +const char * getPwmInitErrorMessage(void) { - return IO_GPIOBYTAG(tag) == gpio && IO_PINBYTAG(tag) == pin; + return pwmInitErrorMsg[pwmInitError]; } -bool CheckGPIOPinSource(ioTag_t tag, GPIO_TypeDef *gpio, uint16_t pin) +const motorProtocolProperties_t * getMotorProtocolProperties(motorPwmProtocolTypes_e proto) { - return IO_GPIOBYTAG(tag) == gpio && IO_GPIO_PinSource(IOGetByTag(tag)) == pin; + return &motorProtocolProperties[proto]; } -pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) +static bool checkPwmTimerConflicts(const timerHardware_t *timHw) { - memset(&pwmIOConfiguration, 0, sizeof(pwmIOConfiguration)); - pwmIOConfiguration.ioConfigurations = memAllocate(sizeof(pwmPortConfiguration_t) * timerHardwareCount, OWNER_PWMIO); + serialPortPins_t uartPins; -#if defined(USE_RX_PWM) || defined(USE_RX_PPM) - int channelIndex = 0; +#if defined(USE_UART2) + uartGetPortPins(UARTDEV_2, &uartPins); + if (doesConfigurationUsePort(SERIAL_PORT_USART2) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) { + return true; + } #endif - for (int timerIndex = 0; timerIndex < timerHardwareCount; timerIndex++) { - const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; - int type = MAP_TO_NONE; +#if defined(USE_UART3) + uartGetPortPins(UARTDEV_3, &uartPins); + if (doesConfigurationUsePort(SERIAL_PORT_USART3) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) { + return true; + } +#endif -#if defined(STM32F3) && defined(USE_UART3) - // skip UART3 ports (PB10/PB11) - if (init->useUART3 && (timerHardwarePtr->tag == IO_TAG(UART3_TX_PIN) || timerHardwarePtr->tag == IO_TAG(UART3_RX_PIN))) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; - } +#if defined(USE_UART4) + uartGetPortPins(UARTDEV_4, &uartPins); + if (doesConfigurationUsePort(SERIAL_PORT_USART4) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) { + return true; + } #endif -#if defined(UART6_TX_PIN) || defined(UART6_RX_PIN) - if (init->useUART6 && (timerHardwarePtr->tag == IO_TAG(UART6_TX_PIN) || timerHardwarePtr->tag == IO_TAG(UART6_RX_PIN))) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; - } +#if defined(USE_UART5) + uartGetPortPins(UARTDEV_5, &uartPins); + if (doesConfigurationUsePort(SERIAL_PORT_USART5) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) { + return true; + } +#endif + +#if defined(USE_UART6) + uartGetPortPins(UARTDEV_6, &uartPins); + if (doesConfigurationUsePort(SERIAL_PORT_USART6) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) { + return true; + } +#endif + +#if defined(USE_UART7) + uartGetPortPins(UARTDEV_7, &uartPins); + if (doesConfigurationUsePort(SERIAL_PORT_USART7) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) { + return true; + } +#endif + +#if defined(USE_UART8) + uartGetPortPins(UARTDEV_8, &uartPins); + if (doesConfigurationUsePort(SERIAL_PORT_USART8) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) { + return true; + } #endif #if defined(USE_SOFTSERIAL1) - if (init->useSoftSerial) { - const timerHardware_t *ss1TimerHardware = timerGetByTag(IO_TAG(SOFTSERIAL_1_RX_PIN), TIM_USE_ANY); - if (ss1TimerHardware != NULL && ss1TimerHardware->tim == timerHardwarePtr->tim) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; - } + if (feature(FEATURE_SOFTSERIAL)) { + const timerHardware_t *ssrx = timerGetByTag(IO_TAG(SOFTSERIAL_1_RX_PIN), TIM_USE_ANY); + const timerHardware_t *sstx = timerGetByTag(IO_TAG(SOFTSERIAL_1_TX_PIN), TIM_USE_ANY); + if ((ssrx != NULL && ssrx->tim == timHw->tim) || (sstx != NULL && sstx->tim == timHw->tim)) { + return true; } + } #endif #if defined(USE_SOFTSERIAL2) - if (init->useSoftSerial) { - const timerHardware_t *ss2TimerHardware = timerGetByTag(IO_TAG(SOFTSERIAL_2_RX_PIN), TIM_USE_ANY); - if (ss2TimerHardware != NULL && ss2TimerHardware->tim == timerHardwarePtr->tim) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; - } + if (feature(FEATURE_SOFTSERIAL)) { + const timerHardware_t *ssrx = timerGetByTag(IO_TAG(SOFTSERIAL_2_RX_PIN), TIM_USE_ANY); + const timerHardware_t *sstx = timerGetByTag(IO_TAG(SOFTSERIAL_2_TX_PIN), TIM_USE_ANY); + if ((ssrx != NULL && ssrx->tim == timHw->tim) || (sstx != NULL && sstx->tim == timHw->tim)) { + return true; } + } #endif #if defined(USE_LED_STRIP) - // skip LED Strip output - if (init->useLEDStrip) { - const timerHardware_t * ledTimHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); - if (ledTimHw != NULL && timerHardwarePtr->tim == ledTimHw->tim) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; - } + if (feature(FEATURE_LED_STRIP)) { + const timerHardware_t * ledTimHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); + if (ledTimHw != NULL && timHw->tim == ledTimHw->tim) { + return true; } + } #endif -#ifdef VBAT_ADC_PIN - if (init->useVbat && timerHardwarePtr->tag == IO_TAG(VBAT_ADC_PIN)) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; - } +#if defined(USE_ADC) +#if defined(ADC_CHANNEL_1_PIN) + if (timHw->tag == IO_TAG(ADC_CHANNEL_1_PIN)) { + return true; + } #endif - -#ifdef RSSI_ADC_PIN - if (init->useRSSIADC && timerHardwarePtr->tag == IO_TAG(RSSI_ADC_PIN)) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; - } +#if defined(ADC_CHANNEL_2_PIN) + if (timHw->tag == IO_TAG(ADC_CHANNEL_2_PIN)) { + return true; + } #endif - -#ifdef CURRENT_METER_ADC_PIN - if (init->useCurrentMeterADC && timerHardwarePtr->tag == IO_TAG(CURRENT_METER_ADC_PIN)) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; - } +#if defined(ADC_CHANNEL_3_PIN) + if (timHw->tag == IO_TAG(ADC_CHANNEL_3_PIN)) { + return true; + } #endif +#if defined(ADC_CHANNEL_4_PIN) + if (timHw->tag == IO_TAG(ADC_CHANNEL_4_PIN)) { + return true; + } +#endif +#endif + #ifdef USE_RANGEFINDER_HCSR04 - if (init->useTriggerRangefinder && - ( - timerHardwarePtr->tag == init->rangefinderIOConfig.triggerTag || - timerHardwarePtr->tag == init->rangefinderIOConfig.echoTag - )) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; + // HC-SR04 has a dedicated connection to FC and require two pins + if (rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) { + const rangefinderHardwarePins_t *rangefinderHardwarePins = rangefinderGetHardwarePins(); + if (rangefinderHardwarePins && (timHw->tag == rangefinderHardwarePins->triggerTag || timHw->tag == rangefinderHardwarePins->echoTag)) { + return true; } + } #endif - // Handle timer mapping to PWM/PPM inputs - if (init->useSerialRx) { - type = MAP_TO_NONE; - } - else if (init->useParallelPWM && (timerHardwarePtr->usageFlags & TIM_USE_PWM)) { - type = MAP_TO_PWM_INPUT; - } - else if (init->usePPM && (timerHardwarePtr->usageFlags & TIM_USE_PPM)) { - type = MAP_TO_PPM_INPUT; + return false; +} + +void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos) +{ + timOutputs->maxTimMotorCount = 0; + timOutputs->maxTimServoCount = 0; + + for (int idx = 0; idx < timerHardwareCount; idx++) { + const timerHardware_t *timHw = &timerHardware[idx]; + int type = MAP_TO_NONE; + + // Check for known conflicts (i.e. UART, LEDSTRIP, Rangefinder and ADC) + if (checkPwmTimerConflicts(timHw)) { + LOG_W(PWM, "Timer output %d skipped", idx); + continue; } - // Handle outputs - may override the PWM/PPM inputs - if (init->flyingPlatformType == PLATFORM_MULTIROTOR || init->flyingPlatformType == PLATFORM_TRICOPTER) { + // Determine if timer belongs to motor/servo + if (mixerConfig()->platformType == PLATFORM_MULTIROTOR || mixerConfig()->platformType == PLATFORM_TRICOPTER) { // Multicopter - if (init->useServoOutputs && (timerHardwarePtr->usageFlags & TIM_USE_MC_SERVO)) { + // We enable mapping to servos if mixer is actually using them + if (isMixerUsingServos && timHw->usageFlags & TIM_USE_MC_SERVO) { type = MAP_TO_SERVO_OUTPUT; } - else if (timerHardwarePtr->usageFlags & TIM_USE_MC_MOTOR) { + else if (timHw->usageFlags & TIM_USE_MC_MOTOR) { type = MAP_TO_MOTOR_OUTPUT; } } else { // Fixed wing or HELI (one/two motors and a lot of servos - if (timerHardwarePtr->usageFlags & TIM_USE_FW_SERVO) { + if (timHw->usageFlags & TIM_USE_FW_SERVO) { type = MAP_TO_SERVO_OUTPUT; } - else if (timerHardwarePtr->usageFlags & TIM_USE_FW_MOTOR) { + else if (timHw->usageFlags & TIM_USE_FW_MOTOR) { type = MAP_TO_MOTOR_OUTPUT; } } - // If timer not mapped - skip - if (type == MAP_TO_NONE) - continue; + switch(type) { + case MAP_TO_MOTOR_OUTPUT: + timOutputs->timMotors[timOutputs->maxTimMotorCount++] = timHw; + break; + case MAP_TO_SERVO_OUTPUT: + timOutputs->timServos[timOutputs->maxTimServoCount++] = timHw; + break; + default: + break; + } + } +} - if (type == MAP_TO_PPM_INPUT) { -#if defined(USE_RX_PPM) - ppmInConfig(timerHardwarePtr); - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_PPM; - pwmIOConfiguration.ppmInputCount++; +static bool motorsUseHardwareTimers(void) +{ + return getMotorProtocolProperties(motorConfig()->motorPwmProtocol)->usesHwTimer; +} - addBootlogEvent6(BOOT_EVENT_TIMER_CH_MAPPED, BOOT_EVENT_FLAGS_NONE, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 0); -#endif - } else if (type == MAP_TO_PWM_INPUT) { -#if defined(USE_RX_PWM) - pwmInConfig(timerHardwarePtr, channelIndex); - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_PWM; - pwmIOConfiguration.pwmInputCount++; - channelIndex++; +static bool servosUseHardwareTimers(void) +{ + return !feature(FEATURE_PWM_SERVO_DRIVER); +} - addBootlogEvent6(BOOT_EVENT_TIMER_CH_MAPPED, BOOT_EVENT_FLAGS_NONE, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 1); -#endif - } else if (type == MAP_TO_MOTOR_OUTPUT) { - /* Check if we already configured maximum supported number of motors and skip the rest */ - if (pwmIOConfiguration.motorCount >= MAX_MOTORS) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 2); - continue; - } +static void pwmInitMotors(timMotorServoHardware_t * timOutputs) +{ + const int motorCount = getMotorCount(); - if (pwmMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->pwmProtocolType, init->enablePWMOutput)) { - if (init->useFastPwm) { - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_FASTPWM | PWM_PF_OUTPUT_PROTOCOL_PWM; - } else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) { - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM; - } else { - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM ; - } + // Check if too many motors + if (motorCount > MAX_MOTORS) { + pwmInitError = PWM_INIT_ERROR_TOO_MANY_MOTORS; + LOG_E(PWM, "Too many motors. Mixer requested %d, max %d", motorCount, MAX_MOTORS); + return; + } - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].index = pwmIOConfiguration.motorCount; - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].timerHardware = timerHardwarePtr; + // Do the pre-configuration. For motors w/o hardware timers this should be sufficient + pwmMotorPreconfigure(); - pwmIOConfiguration.motorCount++; + // Now if we need to configure individual motor outputs - do that + if (!motorsUseHardwareTimers()) { + LOG_I(PWM, "Skipped timer init for motors"); + return; + } - addBootlogEvent6(BOOT_EVENT_TIMER_CH_MAPPED, BOOT_EVENT_FLAGS_NONE, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 2); - } - else { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 2); - continue; - } - } else if (type == MAP_TO_SERVO_OUTPUT) { - if (pwmIOConfiguration.servoCount >= MAX_SERVOS) { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; - } + // If mixer requests more motors than we have timer outputs - throw an error + if (motorCount > timOutputs->maxTimMotorCount) { + pwmInitError = PWM_INIT_ERROR_NOT_ENOUGH_MOTOR_OUTPUTS; + LOG_E(PWM, "Not enough motor outputs. Mixer requested %d, outputs %d", motorCount, timOutputs->maxTimMotorCount); + return; + } - if (pwmServoConfig(timerHardwarePtr, pwmIOConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse, init->enablePWMOutput)) { - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_SERVO | PWM_PF_OUTPUT_PROTOCOL_PWM; - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].index = pwmIOConfiguration.servoCount; - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].timerHardware = timerHardwarePtr; + // Finally initialize individual motor outputs + for (int idx = 0; idx < motorCount; idx++) { + const timerHardware_t *timHw = timOutputs->timMotors[idx]; + if (!pwmMotorConfig(timHw, idx, feature(FEATURE_PWM_OUTPUT_ENABLE))) { + pwmInitError = PWM_INIT_ERROR_TIMER_INIT_FAILED; + LOG_E(PWM, "Timer allocation failed for motor %d", idx); + return; + } + } +} - pwmIOConfiguration.servoCount++; +static void pwmInitServos(timMotorServoHardware_t * timOutputs) +{ + const int servoCount = getServoCount(); - addBootlogEvent6(BOOT_EVENT_TIMER_CH_MAPPED, BOOT_EVENT_FLAGS_NONE, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - } - else { - addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, timerIndex, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); - continue; - } - } else { - continue; - } + if (!isMixerUsingServos()) { + LOG_I(PWM, "Mixer does not use servos"); + return; + } + + // Check if too many servos + if (servoCount > MAX_SERVOS) { + pwmInitError = PWM_INIT_ERROR_TOO_MANY_SERVOS; + LOG_E(PWM, "Too many servos. Mixer requested %d, max %d", servoCount, MAX_SERVOS); + return; + } + + // Do the pre-configuration. This should configure non-timer PWM drivers + pwmServoPreconfigure(); - pwmIOConfiguration.ioCount++; + // Check if we need to init timer output for servos + if (!servosUseHardwareTimers()) { + // External PWM servo driver + LOG_I(PWM, "Skipped timer init for servos - using external servo driver"); + return; } - return &pwmIOConfiguration; + // If mixer requests more servos than we have timer outputs - throw an error + if (servoCount > timOutputs->maxTimServoCount) { + pwmInitError = PWM_INIT_ERROR_NOT_ENOUGH_SERVO_OUTPUTS; + LOG_E(PWM, "Too many servos. Mixer requested %d, timer outputs %d", servoCount, timOutputs->maxTimServoCount); + return; + } + + // Configure individual servo outputs + for (int idx = 0; idx < servoCount; idx++) { + const timerHardware_t *timHw = timOutputs->timServos[idx]; + + if (!pwmServoConfig(timHw, idx, servoConfig()->servoPwmRate, servoConfig()->servoCenterPulse, feature(FEATURE_PWM_OUTPUT_ENABLE))) { + pwmInitError = PWM_INIT_ERROR_TIMER_INIT_FAILED; + LOG_E(PWM, "Timer allocation failed for servo %d", idx); + return; + } + } +} + + +bool pwmMotorAndServoInit(void) +{ + timMotorServoHardware_t timOutputs; + + // Build temporary timer mappings for motor and servo + pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos()); + + // At this point we have built tables of timers suitable for motor and servo mappings + // Now we can actually initialize them according to motor/servo count from mixer + pwmInitMotors(&timOutputs); + pwmInitServos(&timOutputs); + + return (pwmInitError == PWM_INIT_ERROR_NONE); } diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 11929d12954..73f4342de0d 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -19,6 +19,7 @@ #include "drivers/io_types.h" #include "flight/mixer.h" +#include "flight/servos.h" #if defined(TARGET_MOTOR_COUNT) #define MAX_PWM_MOTORS TARGET_MOTOR_COUNT @@ -39,93 +40,40 @@ #define MAX_INPUTS 8 +typedef enum { + PWM_TYPE_STANDARD = 0, + PWM_TYPE_ONESHOT125, + PWM_TYPE_ONESHOT42, + PWM_TYPE_MULTISHOT, + PWM_TYPE_BRUSHED, + PWM_TYPE_DSHOT150, + PWM_TYPE_DSHOT300, + PWM_TYPE_DSHOT600, + PWM_TYPE_DSHOT1200, + PWM_TYPE_SERIALSHOT, +} motorPwmProtocolTypes_e; + +typedef enum { + PWM_INIT_ERROR_NONE = 0, + PWM_INIT_ERROR_TOO_MANY_MOTORS, + PWM_INIT_ERROR_TOO_MANY_SERVOS, + PWM_INIT_ERROR_NOT_ENOUGH_MOTOR_OUTPUTS, + PWM_INIT_ERROR_NOT_ENOUGH_SERVO_OUTPUTS, + PWM_INIT_ERROR_TIMER_INIT_FAILED, +} pwmInitError_e; + typedef struct rangefinderIOConfig_s { ioTag_t triggerTag; ioTag_t echoTag; } rangefinderIOConfig_t; -typedef struct drv_pwm_config_s { - int flyingPlatformType; - - bool enablePWMOutput; - bool useParallelPWM; - bool usePPM; - bool useSerialRx; - bool useRSSIADC; - bool useCurrentMeterADC; - bool useUART2; - bool useUART3; - bool useUART6; - bool useVbat; - bool useFastPwm; - bool useSoftSerial; - bool useLEDStrip; -#ifdef USE_RANGEFINDER - bool useTriggerRangefinder; -#endif - bool useServoOutputs; - uint16_t servoPwmRate; - uint16_t servoCenterPulse; - uint8_t pwmProtocolType; - uint16_t motorPwmRate; - rangefinderIOConfig_t rangefinderIOConfig; -} drv_pwm_config_t; - -typedef enum { - PWM_PF_NONE = 0, - PWM_PF_MOTOR = (1 << 0), - PWM_PF_SERVO = (1 << 1), - PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2), - PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3), - PWM_PF_OUTPUT_PROTOCOL_FASTPWM = (1 << 4), - PWM_PF_PPM = (1 << 5), - PWM_PF_PWM = (1 << 6) -} pwmPortFlags_e; - -struct timerHardware_s; -typedef struct pwmPortConfiguration_s { - uint8_t index; - pwmPortFlags_e flags; - const struct timerHardware_s *timerHardware; -} pwmPortConfiguration_t; - -typedef struct pwmIOConfiguration_s { - uint8_t servoCount; - uint8_t motorCount; - uint8_t ioCount; - uint8_t pwmInputCount; - uint8_t ppmInputCount; - pwmPortConfiguration_t * ioConfigurations; -} pwmIOConfiguration_t; - -// This indexes into the read-only hardware definition structure, timerHardware_t -enum { - PWM1 = 0, - PWM2, - PWM3, - PWM4, - PWM5, - PWM6, - PWM7, - PWM8, - PWM9, - PWM10, - PWM11, - PWM12, - PWM13, - PWM14, - PWM15, - PWM16, - PWM17, - PWM18, - PWM19, - PWM20 -}; - -extern const uint16_t multiPPM[]; -extern const uint16_t multiPWM[]; -extern const uint16_t airPPM[]; -extern const uint16_t airPWM[]; +typedef struct { + bool usesHwTimer; + bool isDSHOT; + bool isSerialShot; +} motorProtocolProperties_t; -pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init); -pwmIOConfiguration_t *pwmGetOutputConfiguration(void); +bool pwmMotorAndServoInit(void); +const motorProtocolProperties_t * getMotorProtocolProperties(motorPwmProtocolTypes_e proto); +pwmInitError_e getPwmInitError(void); +const char * getPwmInitErrorMessage(void); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 94925f941f1..99dfe9ac208 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -21,8 +21,10 @@ #include #include "platform.h" + #include "build/debug.h" +#include "common/log.h" #include "common/maths.h" #include "drivers/io.h" @@ -32,6 +34,7 @@ #include "drivers/io_pca9685.h" #include "io/pwmdriver_i2c.h" +#include "io/esc_serialshot.h" #include "config/feature.h" @@ -50,7 +53,7 @@ #define DSHOT_MOTOR_BIT_0 7 #define DSHOT_MOTOR_BIT_1 14 -#define DSHOT_MOTOR_BITLENGTH 19 +#define DSHOT_MOTOR_BITLENGTH 20 #define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ #endif @@ -59,9 +62,8 @@ typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function poi typedef struct { TCH_t * tch; - pwmWriteFuncPtr pwmWritePtr; bool configured; - uint16_t value; // Used to keep track of last motor value + uint16_t value; // PWM parameters volatile timCCR_t *ccr; // Shortcut for timer CCR register @@ -70,20 +72,27 @@ typedef struct { #ifdef USE_DSHOT // DSHOT parameters - uint32_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE] __attribute__ ((aligned (4))); + timerDMASafeType_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE]; #endif } pwmOutputPort_t; +typedef struct { + pwmOutputPort_t * pwmPort; // May be NULL if motor doesn't use the PWM port + uint16_t value; // Used to keep track of last motor value +} pwmOutputMotor_t; + static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; -static pwmOutputPort_t *motors[MAX_PWM_MOTORS]; -static pwmOutputPort_t *servos[MAX_PWM_SERVOS]; +static pwmOutputMotor_t motors[MAX_PWM_MOTORS]; +static motorPwmProtocolTypes_e initMotorProtocol; +static pwmWriteFuncPtr motorWritePtr = NULL; // Function to write value to motors -#ifdef USE_DSHOT +static pwmOutputPort_t * servos[MAX_PWM_SERVOS]; +static pwmWriteFuncPtr servoWritePtr = NULL; // Function to write value to motors -static bool isProtocolDshot = false; -static timeUs_t dshotMotorUpdateIntervalUs = 0; -static timeUs_t dshotMotorLastUpdateUs; +#if defined(USE_DSHOT) || defined(USE_SERIALSHOT) +static timeUs_t digitalMotorUpdateIntervalUs = 0; +static timeUs_t digitalMotorLastUpdateUs; #endif #ifdef BEEPER_PWM @@ -110,20 +119,34 @@ static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uin *p->ccr = 0; } -static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput) +static pwmOutputPort_t *pwmOutAllocatePort(void) { if (allocatedOutputPortCount >= MAX_PWM_OUTPUT_PORTS) { - DEBUG_TRACE("Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS"); + LOG_E(PWM, "Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS"); return NULL; } + pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++]; + + p->tch = NULL; + p->configured = false; + + return p; +} + +static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput) +{ // Attempt to allocate TCH TCH_t * tch = timerGetTCH(timHw); if (tch == NULL) { return NULL; } - pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++]; + // Allocate motor output port + pwmOutputPort_t *p = pwmOutAllocatePort(); + if (p == NULL) { + return NULL; + } const IO_t io = IOGetByTag(timHw->tag); IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount); @@ -141,15 +164,23 @@ static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t return p; } +static void pwmWriteNull(uint8_t index, uint16_t value) +{ + (void)index; + (void)value; +} + static void pwmWriteStandard(uint8_t index, uint16_t value) { - *motors[index]->ccr = lrintf((value * motors[index]->pulseScale) + motors[index]->pulseOffset); + if (motors[index].pwmPort) { + *(motors[index].pwmPort->ccr) = lrintf((value * motors[index].pwmPort->pulseScale) + motors[index].pwmPort->pulseOffset); + } } void pwmWriteMotor(uint8_t index, uint16_t value) { - if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) { - motors[index]->pwmWritePtr(index, value); + if (motorWritePtr && index < MAX_MOTORS && pwmMotorsEnabled) { + motorWritePtr(index, value); } } @@ -157,7 +188,9 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { for (int index = 0; index < motorCount; index++) { // Set the compare register to 0, which stops the output pulsing if the timer overflows - *motors[index]->ccr = 0; + if (motors[index].pwmPort) { + *(motors[index].pwmPort->ccr) = 0; + } } } @@ -210,21 +243,17 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) } } -static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, motorPwmProtocolTypes_e proto, uint16_t motorPwmRateHz, bool enableOutput) +static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput) { // Try allocating new port - pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, getDshotHz(proto), DSHOT_MOTOR_BITLENGTH, 0, enableOutput); + pwmOutputPort_t * port = pwmOutConfigMotor(timerHardware, dshotHz, DSHOT_MOTOR_BITLENGTH, 0, enableOutput); if (!port) { return NULL; } - // Keep track of motor update interval - const timeUs_t motorIntervalUs = 1000000 / motorPwmRateHz; - dshotMotorUpdateIntervalUs = MAX(dshotMotorUpdateIntervalUs, motorIntervalUs); - // Configure timer DMA - if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, DSHOT_DMA_BUFFER_SIZE)) { + if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) { // Only mark as DSHOT channel if DMA was set successfully memset(port->dmaBuffer, 0, sizeof(port->dmaBuffer)); port->configured = true; @@ -233,13 +262,7 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, return port; } -static void pwmWriteDshot(uint8_t index, uint16_t value) -{ - // DMA operation might still be running. Cache value for future use - motors[index]->value = value; -} - -static void loadDmaBufferDshot(uint32_t * dmaBuffer, uint16_t packet) +static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) { for (int i = 0; i < 16; i++) { dmaBuffer[i] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0; // MSB first @@ -265,71 +288,149 @@ static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) return packet; } +#endif + +#if defined(USE_DSHOT) || defined(USE_SERIALSHOT) +static void motorConfigDigitalUpdateInterval(uint16_t motorPwmRateHz) +{ + digitalMotorUpdateIntervalUs = 1000000 / motorPwmRateHz; + digitalMotorLastUpdateUs = 0; +} + +static void pwmWriteDigital(uint8_t index, uint16_t value) +{ + // Just keep track of motor value, actual update happens in pwmCompleteMotorUpdate() + // DSHOT and some other digital protocols use 11-bit throttle range [0;2047] + motors[index].value = constrain(value, 0, 2047); +} + +bool FAST_CODE NOINLINE isMotorProtocolDshot(void) +{ + // We look at cached `initMotorProtocol` to make sure we are consistent with the initialized config + // motorConfig()->motorPwmProtocol may change at run time which will cause uninitialized structures to be used + return getMotorProtocolProperties(initMotorProtocol)->isDSHOT; +} + +bool FAST_CODE NOINLINE isMotorProtocolSerialShot(void) +{ + return getMotorProtocolProperties(initMotorProtocol)->isSerialShot; +} + +bool FAST_CODE NOINLINE isMotorProtocolDigital(void) +{ + return isMotorProtocolDshot() || isMotorProtocolSerialShot(); +} -void pwmCompleteDshotUpdate(uint8_t motorCount) +void pwmCompleteMotorUpdate(void) { + // Get motor count from mixer + int motorCount = getMotorCount(); + // Get latest REAL time timeUs_t currentTimeUs = micros(); // Enforce motor update rate - if (!isProtocolDshot || (dshotMotorUpdateIntervalUs == 0) || ((currentTimeUs - dshotMotorLastUpdateUs) <= dshotMotorUpdateIntervalUs)) { + if (!isMotorProtocolDigital() || (digitalMotorUpdateIntervalUs == 0) || ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) { return; } - dshotMotorLastUpdateUs = currentTimeUs; + digitalMotorLastUpdateUs = currentTimeUs; - // Generate DMA buffers - for (int index = 0; index < motorCount; index++) { - if (motors[index] && motors[index]->configured) { - // TODO: ESC telemetry - uint16_t packet = prepareDshotPacket(motors[index]->value, false); +#ifdef USE_DSHOT + if (isMotorProtocolDshot()) { + // Generate DMA buffers + for (int index = 0; index < motorCount; index++) { + if (motors[index].pwmPort && motors[index].pwmPort->configured) { + // TODO: ESC telemetry + uint16_t packet = prepareDshotPacket(motors[index].value, false); + + loadDmaBufferDshot(motors[index].pwmPort->dmaBuffer, packet); + timerPWMPrepareDMA(motors[index].pwmPort->tch, DSHOT_DMA_BUFFER_SIZE); + } + } - loadDmaBufferDshot(motors[index]->dmaBuffer, packet); - timerPWMPrepareDMA(motors[index]->tch, DSHOT_DMA_BUFFER_SIZE); + // Start DMA on all timers + for (int index = 0; index < motorCount; index++) { + if (motors[index].pwmPort && motors[index].pwmPort->configured) { + timerPWMStartDMA(motors[index].pwmPort->tch); + } } } +#endif - // Start DMA on all timers - for (int index = 0; index < motorCount; index++) { - if (motors[index] && motors[index]->configured) { - timerPWMStartDMA(motors[index]->tch); +#ifdef USE_SERIALSHOT + if (isMotorProtocolSerialShot()) { + for (int index = 0; index < motorCount; index++) { + serialshotUpdateMotor(index, motors[index].value); } - } -} -bool isMotorProtocolDshot(void) -{ - return isProtocolDshot; + serialshotSendUpdate(); + } +#endif } #endif -bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRateHz, motorPwmProtocolTypes_e proto, bool enableOutput) +void pwmMotorPreconfigure(void) { - pwmOutputPort_t * port = NULL; - pwmWriteFuncPtr pwmWritePtr; + // Keep track of initial motor protocol + initMotorProtocol = motorConfig()->motorPwmProtocol; #ifdef BRUSHED_MOTORS - proto = PWM_TYPE_BRUSHED; // Override proto + initMotorProtocol = PWM_TYPE_BRUSHED; // Override proto #endif - switch (proto) { + // Protocol-specific configuration + switch (initMotorProtocol) { + default: + motorWritePtr = pwmWriteNull; + break; + + case PWM_TYPE_STANDARD: + case PWM_TYPE_BRUSHED: + case PWM_TYPE_ONESHOT125: + case PWM_TYPE_ONESHOT42: + case PWM_TYPE_MULTISHOT: + motorWritePtr = pwmWriteStandard; + break; + +#ifdef USE_DSHOT + case PWM_TYPE_DSHOT1200: + case PWM_TYPE_DSHOT600: + case PWM_TYPE_DSHOT300: + case PWM_TYPE_DSHOT150: + motorConfigDigitalUpdateInterval(motorConfig()->motorPwmRate); + motorWritePtr = pwmWriteDigital; + break; +#endif + +#ifdef USE_SERIALSHOT + case PWM_TYPE_SERIALSHOT: + // Kick off SerialShot driver initalization + serialshotInitialize(); + motorConfigDigitalUpdateInterval(motorConfig()->motorPwmRate); + motorWritePtr = pwmWriteDigital; + break; +#endif + } +} + +bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, bool enableOutput) +{ + switch (initMotorProtocol) { case PWM_TYPE_BRUSHED: - port = motorConfigPwm(timerHardware, 0.0f, 0.0f, motorPwmRateHz, enableOutput); - pwmWritePtr = pwmWriteStandard; + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 0.0f, 0.0f, motorConfig()->motorPwmRate, enableOutput); break; + case PWM_TYPE_ONESHOT125: - port = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorPwmRateHz, enableOutput); - pwmWritePtr = pwmWriteStandard; + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorConfig()->motorPwmRate, enableOutput); break; case PWM_TYPE_ONESHOT42: - port = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorPwmRateHz, enableOutput); - pwmWritePtr = pwmWriteStandard; + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorConfig()->motorPwmRate, enableOutput); break; case PWM_TYPE_MULTISHOT: - port = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorPwmRateHz, enableOutput); - pwmWritePtr = pwmWriteStandard; + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorConfig()->motorPwmRate, enableOutput); break; #ifdef USE_DSHOT @@ -337,28 +438,60 @@ bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, ui case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: - port = motorConfigDshot(timerHardware, proto, motorPwmRateHz, enableOutput); - if (port) { - isProtocolDshot = true; - pwmWritePtr = pwmWriteDshot; - } + motors[motorIndex].pwmPort = motorConfigDshot(timerHardware, getDshotHz(initMotorProtocol), enableOutput); break; #endif - default: case PWM_TYPE_STANDARD: - port = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, motorPwmRateHz, enableOutput); - pwmWritePtr = pwmWriteStandard; + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, motorConfig()->motorPwmRate, enableOutput); + break; + + default: + motors[motorIndex].pwmPort = NULL; break; } - if (port) { - port->pwmWritePtr = pwmWritePtr; - motors[motorIndex] = port; - return true; + return (motors[motorIndex].pwmPort != NULL); +} + +// Helper function for ESC passthrough +ioTag_t pwmGetMotorPinTag(int motorIndex) +{ + if (motors[motorIndex].pwmPort) { + return motors[motorIndex].pwmPort->tch->timHw->tag; + } + else { + return IOTAG_NONE; } +} - return false; +static void pwmServoWriteStandard(uint8_t index, uint16_t value) +{ + if (servos[index]) { + *servos[index]->ccr = value; + } +} + +#ifdef USE_PWM_SERVO_DRIVER +static void pwmServoWriteExternalDriver(uint8_t index, uint16_t value) +{ + // If PCA9685 is not detected, we do not want to write servo output anywhere + if (STATE(PWM_DRIVER_AVAILABLE)) { + pwmDriverSetPulse(index, value); + } +} +#endif + +void pwmServoPreconfigure(void) +{ + servoWritePtr = pwmServoWriteStandard; + +#ifdef USE_PWM_SERVO_DRIVER + // If PCA9685 is enabled - switch the servo write function to external + if (feature(FEATURE_PWM_SERVO_DRIVER)) { + servoWritePtr = pwmServoWriteExternalDriver; + } +#endif } bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput) @@ -375,23 +508,9 @@ bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, ui void pwmWriteServo(uint8_t index, uint16_t value) { -#ifdef USE_PWM_SERVO_DRIVER - - /* - * If PCA9685 is enabled and but not detected, we do not want to write servo - * output anywhere - */ - if (feature(FEATURE_PWM_SERVO_DRIVER) && STATE(PWM_DRIVER_AVAILABLE)) { - pwmDriverSetPulse(index, value); - } else if (!feature(FEATURE_PWM_SERVO_DRIVER) && servos[index] && index < MAX_SERVOS) { - *servos[index]->ccr = value; - } - -#else - if (servos[index] && index < MAX_SERVOS) { - *servos[index]->ccr = value; + if (servoWritePtr && index < MAX_SERVOS) { + servoWritePtr(index, value); } -#endif } #ifdef BEEPER_PWM diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index a7b6adfbf47..78d4351a872 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -20,29 +20,23 @@ #include "drivers/io_types.h" #include "drivers/time.h" -typedef enum { - PWM_TYPE_STANDARD = 0, - PWM_TYPE_ONESHOT125, - PWM_TYPE_ONESHOT42, - PWM_TYPE_MULTISHOT, - PWM_TYPE_BRUSHED, - PWM_TYPE_DSHOT150, - PWM_TYPE_DSHOT300, - PWM_TYPE_DSHOT600, - PWM_TYPE_DSHOT1200, -} motorPwmProtocolTypes_e; +ioTag_t pwmGetMotorPinTag(int motorIndex); void pwmWriteMotor(uint8_t index, uint16_t value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); -void pwmCompleteDshotUpdate(uint8_t motorCount); -bool isMotorProtocolDshot(void); +void pwmCompleteMotorUpdate(void); +bool isMotorProtocolDigital(void); void pwmWriteServo(uint8_t index, uint16_t value); void pwmDisableMotors(void); void pwmEnableMotors(void); struct timerHardware_s; -bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, motorPwmProtocolTypes_e proto, bool enableOutput); + +void pwmMotorPreconfigure(void); +bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, bool enableOutput); + +void pwmServoPreconfigure(void); bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput); void pwmWriteBeeper(bool onoffBeep); void beeperPwmInit(ioTag_t tag, uint16_t frequency); \ No newline at end of file diff --git a/src/main/drivers/rangefinder/rangefinder_hcsr04.c b/src/main/drivers/rangefinder/rangefinder_hcsr04.c index 8e3a269b4f3..fe935473bfc 100644 --- a/src/main/drivers/rangefinder/rangefinder_hcsr04.c +++ b/src/main/drivers/rangefinder/rangefinder_hcsr04.c @@ -32,8 +32,6 @@ #include "drivers/nvic.h" #include "drivers/rcc.h" -#include "drivers/logging.h" - #include "drivers/rangefinder/rangefinder.h" #include "drivers/rangefinder/rangefinder_hcsr04.h" @@ -161,12 +159,10 @@ bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * range echoIO = IOGetByTag(rangefinderHardwarePins->echoTag); if (IOGetOwner(triggerIO) != OWNER_FREE) { - addBootlogEvent4(BOOT_EVENT_HARDWARE_IO_CONFLICT, BOOT_EVENT_FLAGS_WARNING, IOGetOwner(triggerIO), OWNER_RANGEFINDER); return false; } if (IOGetOwner(echoIO) != OWNER_FREE) { - addBootlogEvent4(BOOT_EVENT_HARDWARE_IO_CONFLICT, BOOT_EVENT_FLAGS_WARNING, IOGetOwner(echoIO), OWNER_RANGEFINDER); return false; } diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index dafe60fd3de..1169f152900 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -21,7 +21,8 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "FREE", "PWM/IO", "PWM", "PPM", "MOTOR", "SERVO", "SOFTSERIAL", "ADC", "SERIAL", "DEBUG", "TIMER", "RANGEFINDER", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD", "BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER", - "NRF24", "VTX", "SPI_PREINIT", "COMPASS", "AIRSPEED", "OLED DISPLAY", "PINIO" + "NRF24", "VTX", "SPI_PREINIT", "COMPASS", "TEMPERATURE", "1-WIRE", "AIRSPEED", "OLED DISPLAY", + "PINIO" }; const char * const resourceNames[RESOURCE_TOTAL_COUNT] = { diff --git a/src/main/drivers/rx_pwm.c b/src/main/drivers/rx_pwm.c index 1a9b86bfb8e..c915e58d61a 100644 --- a/src/main/drivers/rx_pwm.c +++ b/src/main/drivers/rx_pwm.c @@ -20,7 +20,7 @@ #include -#if defined(USE_RX_PWM) || defined(USE_RX_PPM) +#if defined(USE_RX_PPM) #include "build/build_config.h" #include "build/debug.h" @@ -38,49 +38,13 @@ #include "rx_pwm.h" -#define DEBUG_PPM_ISR - -#define PPM_CAPTURE_COUNT 16 -#define PWM_INPUT_PORT_COUNT 8 - -#if PPM_CAPTURE_COUNT > MAX_PWM_INPUT_PORTS -#define PWM_PORTS_OR_PPM_CAPTURE_COUNT PPM_CAPTURE_COUNT -#else -#define PWM_PORTS_OR_PPM_CAPTURE_COUNT PWM_INPUT_PORT_COUNT -#endif - -#define INPUT_FILTER_TICKS 10 - -static inputFilteringMode_e inputFilteringMode; +#define PPM_CAPTURE_COUNT 16 +#define INPUT_FILTER_TICKS 10 +#define PPM_TIMER_PERIOD 0x10000 void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); -typedef enum { - INPUT_MODE_PPM, - INPUT_MODE_PWM, -} pwmInputMode_t; - -typedef struct { - pwmInputMode_t mode; - uint8_t channel; // only used for pwm, ignored by ppm - - uint8_t state; - captureCompare_t rise; - captureCompare_t fall; - captureCompare_t capture; - - uint8_t missedEvents; - - timerCallbacks_t cb; - const TCH_t * tch; -} pwmInputPort_t; - -static pwmInputPort_t pwmInputPorts[PWM_INPUT_PORT_COUNT]; - -static uint16_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT]; - -#define PPM_TIMER_PERIOD 0x10000 -#define PWM_TIMER_PERIOD 0x10000 +static uint16_t captures[PPM_CAPTURE_COUNT]; static uint8_t ppmFrameCount = 0; static uint8_t lastPPMFrameCount = 0; @@ -88,11 +52,10 @@ static uint8_t ppmCountDivisor = 1; typedef struct ppmDevice_s { uint8_t pulseIndex; - //uint32_t previousTime; uint32_t currentCapture; uint32_t currentTime; uint32_t deltaTime; - uint32_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT]; + uint32_t captures[PPM_CAPTURE_COUNT]; uint32_t largeCounter; int8_t numChannels; int8_t numChannelsPrevFrame; @@ -104,14 +67,12 @@ typedef struct ppmDevice_s { ppmDevice_t ppmDev; - -#define PPM_IN_MIN_SYNC_PULSE_US 2700 // microseconds -#define PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds -#define PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds +#define PPM_IN_MIN_SYNC_PULSE_US 2700 // microseconds +#define PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds +#define PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds #define PPM_STABLE_FRAMES_REQUIRED_COUNT 25 -#define PPM_IN_MIN_NUM_CHANNELS 4 -#define PPM_IN_MAX_NUM_CHANNELS PWM_PORTS_OR_PPM_CAPTURE_COUNT - +#define PPM_IN_MIN_NUM_CHANNELS 4 +#define PPM_IN_MAX_NUM_CHANNELS PPM_CAPTURE_COUNT bool isPPMDataBeingReceived(void) { @@ -125,36 +86,6 @@ void resetPPMDataReceivedState(void) #define MIN_CHANNELS_BEFORE_PPM_FRAME_CONSIDERED_VALID 4 -void pwmRxInit(inputFilteringMode_e inputFilteringModeToUse) -{ - inputFilteringMode = inputFilteringModeToUse; -} - -#ifdef DEBUG_PPM_ISR -typedef enum { - SOURCE_OVERFLOW = 0, - SOURCE_EDGE = 1 -} eventSource_e; - -typedef struct ppmISREvent_s { - eventSource_e source; - uint32_t capture; -} ppmISREvent_t; - -static ppmISREvent_t ppmEvents[20]; -static uint8_t ppmEventIndex = 0; - -void ppmISREvent(eventSource_e source, uint32_t capture) -{ - ppmEventIndex = (ppmEventIndex + 1) % (sizeof(ppmEvents) / sizeof(ppmEvents[0])); - - ppmEvents[ppmEventIndex].source = source; - ppmEvents[ppmEventIndex].capture = capture; -} -#else -void ppmISREvent(eventSource_e source, uint32_t capture) {} -#endif - static void ppmInit(void) { ppmDev.pulseIndex = 0; @@ -172,7 +103,6 @@ static void ppmInit(void) static void ppmOverflowCallback(struct TCH_s * tch, uint32_t capture) { UNUSED(tch); - ppmISREvent(SOURCE_OVERFLOW, capture); ppmDev.largeCounter += capture + 1; if (capture == PPM_TIMER_PERIOD - 1) { @@ -183,7 +113,6 @@ static void ppmOverflowCallback(struct TCH_s * tch, uint32_t capture) static void ppmEdgeCallback(struct TCH_s * tch, uint32_t capture) { UNUSED(tch); - ppmISREvent(SOURCE_EDGE, capture); int32_t i; @@ -219,7 +148,7 @@ static void ppmEdgeCallback(struct TCH_s * tch, uint32_t capture) ppmDev.currentTime = currentTime; ppmDev.currentCapture = capture; -#if 1 +#if 0 static uint32_t deltaTimes[20]; static uint8_t deltaIndex = 0; @@ -229,7 +158,7 @@ static void ppmEdgeCallback(struct TCH_s * tch, uint32_t capture) #endif -#if 1 +#if 0 static uint32_t captureTimes[20]; static uint8_t captureIndex = 0; @@ -282,122 +211,37 @@ static void ppmEdgeCallback(struct TCH_s * tch, uint32_t capture) } else { /* Not a valid pulse duration */ ppmDev.tracking = false; - for (i = 0; i < PWM_PORTS_OR_PPM_CAPTURE_COUNT; i++) { + for (i = 0; i < PPM_CAPTURE_COUNT; i++) { ppmDev.captures[i] = PPM_RCVR_TIMEOUT; } } } } -#define MAX_MISSED_PWM_EVENTS 10 - -bool isPWMDataBeingReceived(void) -{ - int channel; - for (channel = 0; channel < PWM_PORTS_OR_PPM_CAPTURE_COUNT; channel++) { - if (captures[channel] != PPM_RCVR_TIMEOUT) { - return true; - } - } - return false; -} - -static void pwmOverflowCallback(struct TCH_s * tch, uint32_t capture) -{ - UNUSED(capture); - pwmInputPort_t *pwmInputPort = (pwmInputPort_t *)tch->cb->callbackParam; - - if (++pwmInputPort->missedEvents > MAX_MISSED_PWM_EVENTS) { - captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT; - pwmInputPort->missedEvents = 0; - } -} - -static void pwmEdgeCallback(struct TCH_s * tch, uint32_t capture) -{ - pwmInputPort_t *pwmInputPort = (pwmInputPort_t *)tch->cb->callbackParam; - - if (pwmInputPort->state == 0) { - pwmInputPort->rise = capture; - pwmInputPort->state = 1; - timerChConfigIC(tch, false, INPUT_FILTER_TICKS); - } else { - pwmInputPort->fall = capture; - - // compute and store capture and handle overflow correctly - timer may be configured for PWM output in such case overflow value is not 0xFFFF - if (pwmInputPort->fall >= pwmInputPort->rise) { - pwmInputPort->capture = pwmInputPort->fall - pwmInputPort->rise; - } - else { - pwmInputPort->capture = (pwmInputPort->fall + timerGetPeriod(tch)) - pwmInputPort->rise; - } - captures[pwmInputPort->channel] = pwmInputPort->capture; - - // switch state - pwmInputPort->state = 0; - timerChConfigIC(tch, true, INPUT_FILTER_TICKS); - pwmInputPort->missedEvents = 0; - } -} - -void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel) -{ - pwmInputPort_t *self = &pwmInputPorts[channel]; - - TCH_t * tch = timerGetTCH(timerHardwarePtr); - if (tch == NULL) { - return; - } - - self->state = 0; - self->missedEvents = 0; - self->channel = channel; - self->mode = INPUT_MODE_PWM; - - IO_t io = IOGetByTag(timerHardwarePtr->tag); - IOInit(io, OWNER_PWMINPUT, RESOURCE_INPUT, RESOURCE_INDEX(channel)); - IOConfigGPIOAF(io, timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); - - timerConfigure(tch, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_HZ); - timerChInitCallbacks(&self->cb, (void*)self, &pwmEdgeCallback, &pwmOverflowCallback); - timerChConfigCallbacks(tch, &self->cb); - timerChConfigIC(tch, true, INPUT_FILTER_TICKS); - timerChCaptureEnable(tch); -} - -#define UNUSED_PPM_TIMER_REFERENCE 0 -#define FIRST_PWM_PORT 0 - -void ppmInConfig(const timerHardware_t *timerHardwarePtr) +bool ppmInConfig(const timerHardware_t *timerHardwarePtr) { + static timerCallbacks_t callbacks; TCH_t * tch = timerGetTCH(timerHardwarePtr); if (tch == NULL) { - return; + return false; } ppmInit(); - pwmInputPort_t *self = &pwmInputPorts[FIRST_PWM_PORT]; - - self->mode = INPUT_MODE_PPM; - IO_t io = IOGetByTag(timerHardwarePtr->tag); IOInit(io, OWNER_PPMINPUT, RESOURCE_INPUT, 0); IOConfigGPIOAF(io, timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction); timerConfigure(tch, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_HZ); - timerChInitCallbacks(&self->cb, (void*)self, &ppmEdgeCallback, &ppmOverflowCallback); - timerChConfigCallbacks(tch, &self->cb); + timerChInitCallbacks(&callbacks, (void*)&ppmDev, &ppmEdgeCallback, &ppmOverflowCallback); + timerChConfigCallbacks(tch, &callbacks); timerChConfigIC(tch, true, INPUT_FILTER_TICKS); timerChCaptureEnable(tch); -} -uint16_t ppmRead(uint8_t channel) -{ - return captures[channel]; + return true; } -uint16_t pwmRead(uint8_t channel) +uint16_t ppmRead(uint8_t channel) { return captures[channel]; } diff --git a/src/main/drivers/rx_pwm.h b/src/main/drivers/rx_pwm.h index 8757cf6e1cc..a80de956fc0 100644 --- a/src/main/drivers/rx_pwm.h +++ b/src/main/drivers/rx_pwm.h @@ -17,15 +17,10 @@ #pragma once -typedef enum { - INPUT_FILTERING_DISABLED = 0, - INPUT_FILTERING_ENABLED -} inputFilteringMode_e; - #define PPM_RCVR_TIMEOUT 0 struct timerHardware_s; -void ppmInConfig(const struct timerHardware_s *timerHardwarePtr); +bool ppmInConfig(const struct timerHardware_s *timerHardwarePtr); void pwmInConfig(const struct timerHardware_s *timerHardwarePtr, uint8_t channel); uint16_t pwmRead(uint8_t channel); @@ -34,6 +29,4 @@ uint16_t ppmRead(uint8_t channel); bool isPPMDataBeingReceived(void); void resetPPMDataReceivedState(void); -void pwmRxInit(inputFilteringMode_e inputFilteringMode); - bool isPWMDataBeingReceived(void); diff --git a/src/main/drivers/rx_spi.h b/src/main/drivers/rx_spi.h index 55613b12bce..c459c4735cf 100755 --- a/src/main/drivers/rx_spi.h +++ b/src/main/drivers/rx_spi.h @@ -20,7 +20,7 @@ #define RX_SPI_MAX_PAYLOAD_SIZE 32 -void rxSpiDeviceInit(); +void rxSpiDeviceInit(void); uint8_t rxSpiTransferByte(uint8_t data); uint8_t rxSpiWriteByte(uint8_t data); uint8_t rxSpiWriteCommand(uint8_t command, uint8_t data); diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index ef982305323..91d4fcd7bd9 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -83,7 +83,7 @@ bool sdcard_isInserted(void) /** * Dispatch */ -sdcardVTable_t *sdcardVTable; +sdcardVTable_t *sdcardVTable = NULL; void sdcard_init(void) { @@ -100,17 +100,29 @@ void sdcard_init(void) bool sdcard_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) { - return sdcardVTable->readBlock(blockIndex, buffer, callback, callbackData); + if (sdcardVTable) { + return sdcardVTable->readBlock(blockIndex, buffer, callback, callbackData); + } else { + return false; + } } sdcardOperationStatus_e sdcard_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount) { - return sdcardVTable->beginWriteBlocks(blockIndex, blockCount); + if (sdcardVTable) { + return sdcardVTable->beginWriteBlocks(blockIndex, blockCount); + } else { + return false; + } } sdcardOperationStatus_e sdcard_writeBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_operationCompleteCallback_c callback, uint32_t callbackData) { - return sdcardVTable->writeBlock(blockIndex, buffer, callback, callbackData); + if (sdcardVTable) { + return sdcardVTable->writeBlock(blockIndex, buffer, callback, callbackData); + } else { + return false; + } } bool sdcard_poll(void) diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index 29364228bd4..b61c5877d59 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -16,6 +16,12 @@ */ #pragma once +#include "drivers/io.h" + +typedef struct { + ioTag_t rxPin; + ioTag_t txPin; +} serialPortPins_t; typedef enum portMode_t { MODE_RX = 1 << 0, diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index 0f30b7d4a7b..30ffecb5247 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -61,6 +61,7 @@ typedef struct { USART_TypeDef *USARTx; } uartPort_t; +void uartGetPortPins(UARTDevice_e device, serialPortPins_t * pins); serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_t mode, portOptions_t options); // serialPort API diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index b76cd2bf6bf..b8d79fd2149 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -98,6 +98,47 @@ static uartPort_t uartPort4; static uartPort_t uartPort5; #endif + +void uartGetPortPins(UARTDevice_e device, serialPortPins_t * pins) +{ + switch(device) { +#ifdef USE_UART1 + case UARTDEV_1: + pins->txPin = IO_TAG(UART1_TX_PIN); + pins->rxPin = IO_TAG(UART1_RX_PIN); + break; +#endif +#ifdef USE_UART2 + case UARTDEV_2: + pins->txPin = IO_TAG(UART2_TX_PIN); + pins->rxPin = IO_TAG(UART2_RX_PIN); + break; +#endif +#ifdef USE_UART3 + case UARTDEV_3: + pins->txPin = IO_TAG(UART3_TX_PIN); + pins->rxPin = IO_TAG(UART3_RX_PIN); + break; +#endif +#ifdef USE_UART4 + case UARTDEV_4: + pins->txPin = IO_TAG(UART4_TX_PIN); + pins->rxPin = IO_TAG(UART4_RX_PIN); + break; +#endif +#ifdef USE_UART5 + case UARTDEV_5: + pins->txPin = IO_TAG(UART5_TX_PIN); + pins->rxPin = IO_TAG(UART5_RX_PIN); + break; +#endif + default: + pins->txPin = IO_TAG(NONE); + pins->rxPin = IO_TAG(NONE); + break; + } +} + void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, uint8_t af, uint8_t index) { if (options & SERIAL_BIDIR) { diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index aaafc2b25a7..dc9f0ddc502 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -239,6 +239,20 @@ void uartIrqHandler(uartPort_t *s) } } +void uartGetPortPins(UARTDevice_e device, serialPortPins_t * pins) +{ + uartDevice_t *uart = uartHardwareMap[device]; + + if (uart) { + pins->txPin = uart->tx; + pins->rxPin = uart->rx; + } + else { + pins->txPin = IO_TAG(NONE); + pins->rxPin = IO_TAG(NONE); + } +} + uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s; diff --git a/src/main/drivers/serial_uart_stm32f7xx.c b/src/main/drivers/serial_uart_stm32f7xx.c index 3a4674a72ec..9b4dc1abd71 100644 --- a/src/main/drivers/serial_uart_stm32f7xx.c +++ b/src/main/drivers/serial_uart_stm32f7xx.c @@ -286,6 +286,20 @@ void uartIrqHandler(uartPort_t *s) } } +void uartGetPortPins(UARTDevice_e device, serialPortPins_t * pins) +{ + uartDevice_t *uart = uartHardwareMap[device]; + + if (uart) { + pins->txPin = uart->tx; + pins->rxPin = uart->rx; + } + else { + pins->txPin = IO_TAG(NONE); + pins->rxPin = IO_TAG(NONE); + } +} + uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s; diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index 8250f125ae9..a31cb08aa95 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -27,19 +27,36 @@ #define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000) void SetSysClock(uint8_t underclock); +inline static void NVIC_DisableAllIRQs(void) +{ + // We access CMSIS NVIC registers directly here + for (int x = 0; x < 8; x++) { + // Mask all IRQs controlled by a ICERx + NVIC->ICER[x] = 0xFFFFFFFF; + // Clear all pending IRQs controlled by a ICPRx + NVIC->ICPR[x] = 0xFFFFFFFF; + } +} + void systemReset(void) { + // Disable all NVIC interrupts + __disable_irq(); + NVIC_DisableAllIRQs(); + // Generate system reset SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } void systemResetToBootloader(void) { - // 1FFFF000 -> 20000200 -> SP - // 1FFFF004 -> 1FFFF021 -> PC + __disable_irq(); + NVIC_DisableAllIRQs(); *((uint32_t *)0x20009FFC) = 0xDEADBEEF; // 40KB SRAM STM32F30X - systemReset(); + + // Generate system reset + SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 65cbdcec407..7da03ec009e 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -32,16 +32,31 @@ #define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000) void SetSysClock(void); +inline static void NVIC_DisableAllIRQs(void) +{ + // We access CMSIS NVIC registers directly here + for (int x = 0; x < 8; x++) { + // Mask all IRQs controlled by a ICERx + NVIC->ICER[x] = 0xFFFFFFFF; + // Clear all pending IRQs controlled by a ICPRx + NVIC->ICPR[x] = 0xFFFFFFFF; + } +} + void systemReset(void) { __disable_irq(); + NVIC_DisableAllIRQs(); NVIC_SystemReset(); } void systemResetToBootloader(void) { - *((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX __disable_irq(); + NVIC_DisableAllIRQs(); + + *((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX + NVIC_SystemReset(); } diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c index 4a79c6aefef..960d67404bb 100644 --- a/src/main/drivers/system_stm32f7xx.c +++ b/src/main/drivers/system_stm32f7xx.c @@ -31,16 +31,31 @@ #define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000) void SystemClock_Config(void); +inline static void NVIC_DisableAllIRQs(void) +{ + // We access CMSIS NVIC registers directly here + for (int x = 0; x < 8; x++) { + // Mask all IRQs controlled by a ICERx + NVIC->ICER[x] = 0xFFFFFFFF; + // Clear all pending IRQs controlled by a ICPRx + NVIC->ICPR[x] = 0xFFFFFFFF; + } +} + void systemReset(void) { __disable_irq(); + NVIC_DisableAllIRQs(); NVIC_SystemReset(); } void systemResetToBootloader(void) { - (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot __disable_irq(); + NVIC_DisableAllIRQs(); + + (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot + NVIC_SystemReset(); } diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index fcb8c9430c8..4b7e15b5519 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -23,10 +23,10 @@ #include "platform.h" #include "build/atomic.h" -#include "build/debug.h" -#include "common/utils.h" +#include "common/log.h" #include "common/memory.h" +#include "common/utils.h" #include "drivers/io.h" #include "drivers/rcc.h" @@ -80,7 +80,7 @@ TCH_t * timerGetTCH(const timerHardware_t * timHw) const int timerIndex = lookupTimerIndex(timHw->tim); if (timerIndex >= HARDWARE_TIMER_DEFINITION_COUNT) { - DEBUG_TRACE("Can't find hardware timer definition"); + LOG_E(TIMER, "Can't find hardware timer definition"); return NULL; } @@ -90,7 +90,7 @@ TCH_t * timerGetTCH(const timerHardware_t * timHw) // Check for OOM if (timerCtx[timerIndex] == NULL) { - DEBUG_TRACE("Can't allocate TCH object"); + LOG_E(TIMER, "Can't allocate TCH object"); return NULL; } @@ -193,6 +193,16 @@ const timerHardware_t * timerGetByTag(ioTag_t tag, timerUsageFlag_e flag) return NULL; } +const timerHardware_t * timerGetByUsageFlag(timerUsageFlag_e flag) +{ + for (int i = 0; i < timerHardwareCount; i++) { + if (timerHardware[i].usageFlags & flag) { + return &timerHardware[i]; + } + } + return NULL; +} + void timerPWMConfigChannel(TCH_t * tch, uint16_t value) { impl_timerPWMConfigChannel(tch, value); @@ -233,14 +243,14 @@ uint32_t timerGetBaseClockHW(const timerHardware_t * timHw) return SystemCoreClock / timerClockDivisor(timHw->tim); } -bool timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBufferSize) +bool timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount) { - return impl_timerPWMConfigChannelDMA(tch, dmaBuffer, dmaBufferSize); + return impl_timerPWMConfigChannelDMA(tch, dmaBuffer, dmaBufferElementSize, dmaBufferElementCount); } -void timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferSize) +void timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount) { - impl_timerPWMPrepareDMA(tch, dmaBufferSize); + impl_timerPWMPrepareDMA(tch, dmaBufferElementCount); } void timerPWMStartDMA(TCH_t * tch) @@ -256,4 +266,4 @@ void timerPWMStopDMA(TCH_t * tch) bool timerPWMDMAInProgress(TCH_t * tch) { return tch->dmaState != TCH_DMA_IDLE; -} \ No newline at end of file +} diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 2fe3c39e3f8..f16a4eceb7e 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -168,6 +168,7 @@ typedef enum { uint8_t timerClockDivisor(TIM_TypeDef *tim); uint32_t timerGetBaseClockHW(const timerHardware_t * timHw); +const timerHardware_t * timerGetByUsageFlag(timerUsageFlag_e flag); const timerHardware_t * timerGetByTag(ioTag_t tag, timerUsageFlag_e flag); TCH_t * timerGetTCH(const timerHardware_t * timHw); @@ -190,8 +191,11 @@ void timerEnable(TCH_t * tch); void timerPWMConfigChannel(TCH_t * tch, uint16_t value); void timerPWMStart(TCH_t * tch); -bool timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBufferSize); -void timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferSize); +// dmaBufferElementSize is the size in bytes of each element in the memory +// buffer. 1, 2 or 4 are the only valid values. +// dmaBufferElementCount is the number of elements in the buffer +bool timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount); +void timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount); void timerPWMStartDMA(TCH_t * tch); void timerPWMStopDMA(TCH_t * tch); bool timerPWMDMAInProgress(TCH_t * tch); diff --git a/src/main/drivers/timer_def_stm32f3xx.h b/src/main/drivers/timer_def_stm32f3xx.h index 3c5e7469ad8..dc320fd92ae 100644 --- a/src/main/drivers/timer_def_stm32f3xx.h +++ b/src/main/drivers/timer_def_stm32f3xx.h @@ -17,6 +17,8 @@ #pragma once +#define timerDMASafeType_t uint16_t + #define DEF_TIM_DMAMAP__D(dma, channel) DMA_TAG(dma, 0, channel) #define DEF_TIM_DMAMAP__NONE DMA_NONE diff --git a/src/main/drivers/timer_def_stm32f4xx.h b/src/main/drivers/timer_def_stm32f4xx.h index cd51c1f49d8..b96246680a1 100644 --- a/src/main/drivers/timer_def_stm32f4xx.h +++ b/src/main/drivers/timer_def_stm32f4xx.h @@ -17,6 +17,8 @@ #pragma once +#define timerDMASafeType_t uint32_t + #define DEF_TIM_DMAMAP__D(dma, stream, channel) DMA_TAG(dma, stream, channel) #define DEF_TIM_DMAMAP__NONE DMA_NONE diff --git a/src/main/drivers/timer_def_stm32f7xx.h b/src/main/drivers/timer_def_stm32f7xx.h index 85d763a9043..56e002d5336 100644 --- a/src/main/drivers/timer_def_stm32f7xx.h +++ b/src/main/drivers/timer_def_stm32f7xx.h @@ -17,6 +17,8 @@ #pragma once +#define timerDMASafeType_t uint32_t + // Mappings for STDLIB defines // #define DEF_TIM_CHNL_CH1 TIM_CHANNEL_1 // #define DEF_TIM_CHNL_CH1N TIM_CHANNEL_1 diff --git a/src/main/drivers/timer_impl.h b/src/main/drivers/timer_impl.h index aa9b7fee271..bb9d55af981 100644 --- a/src/main/drivers/timer_impl.h +++ b/src/main/drivers/timer_impl.h @@ -55,7 +55,7 @@ void impl_timerChCaptureCompareEnable(TCH_t * tch, bool enable); void impl_timerPWMConfigChannel(TCH_t * tch, uint16_t value); void impl_timerPWMStart(TCH_t * tch); -bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBufferSize); -void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferSize); +bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount); +void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount); void impl_timerPWMStartDMA(TCH_t * tch); void impl_timerPWMStopDMA(TCH_t * tch); diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c index 1834d84c748..a4eb82941d9 100644 --- a/src/main/drivers/timer_impl_hal.c +++ b/src/main/drivers/timer_impl_hal.c @@ -18,6 +18,7 @@ #include #include #include +#include #include "platform.h" @@ -82,7 +83,7 @@ void impl_timerConfigBase(TCH_t * tch, uint16_t period, uint32_t hz) } timHandle->Instance = timer; - timHandle->Init.Prescaler = (timerGetBaseClock(tch) / hz) - 1; + timHandle->Init.Prescaler = lrintf((float)timerGetBaseClock(tch) / hz + 0.01f) - 1; timHandle->Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR timHandle->Init.RepetitionCounter = 0; timHandle->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; @@ -319,7 +320,7 @@ static void impl_timerDMA_IRQHandler(DMA_t descriptor) } } -bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBufferSize) +bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount) { tch->dma = dmaGetByTag(tch->timHw->dmaTag); tch->dmaBuffer = dmaBuffer; @@ -346,11 +347,30 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBu init.Channel = channelLL; init.PeriphOrM2MSrcAddress = (uint32_t)impl_timerCCR(tch); init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; - init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD; + + switch (dmaBufferElementSize) { + case 1: + init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE; + init.PeriphOrM2MSrcDataSize = LL_DMA_MDATAALIGN_BYTE; + break; + case 2: + init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_HALFWORD; + init.PeriphOrM2MSrcDataSize = LL_DMA_MDATAALIGN_HALFWORD; + break; + case 4: + init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD; + init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD; + break; + default: + // Programmer error + while(1) { + + } + } + init.MemoryOrM2MDstAddress = (uint32_t)dmaBuffer; init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; - init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD; - init.NbData = dmaBufferSize; + init.NbData = dmaBufferElementCount; init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; init.Mode = LL_DMA_MODE_NORMAL; init.Priority = LL_DMA_PRIORITY_HIGH; @@ -375,10 +395,10 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBu return true; } -void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferSize) +void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount) { const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]; - DMA_TypeDef * dmaBase = tch->dma->dma; + DMA_TypeDef *dmaBase = tch->dma->dma; // Make sure we terminate any DMA transaction currently in progress // Clear the flag as well, so even if DMA transfer finishes while within ATOMIC_BLOCK @@ -389,7 +409,7 @@ void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferSize) DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF); } - LL_DMA_SetDataLength(dmaBase, streamLL, dmaBufferSize); + LL_DMA_SetDataLength(dmaBase, streamLL, dmaBufferElementCount); LL_DMA_ConfigAddresses(dmaBase, streamLL, (uint32_t)tch->dmaBuffer, (uint32_t)impl_timerCCR(tch), LL_DMA_DIRECTION_MEMORY_TO_PERIPH); LL_DMA_EnableIT_TC(dmaBase, streamLL); LL_DMA_EnableStream(dmaBase, streamLL); diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c index 17ccae91c22..4dd428ebc5e 100644 --- a/src/main/drivers/timer_impl_stdperiph.c +++ b/src/main/drivers/timer_impl_stdperiph.c @@ -17,6 +17,7 @@ #include #include +#include #include "platform.h" @@ -67,7 +68,7 @@ void impl_timerConfigBase(TCH_t * tch, uint16_t period, uint32_t hz) TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Period = (period - 1) & 0xffff; // AKA TIMx_ARR - TIM_TimeBaseStructure.TIM_Prescaler = (timerGetBaseClock(tch) / hz) - 1; + TIM_TimeBaseStructure.TIM_Prescaler = lrintf((float)timerGetBaseClock(tch) / hz + 0.01f) - 1; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure); @@ -284,7 +285,7 @@ static void impl_timerDMA_IRQHandler(DMA_t descriptor) } } -bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBufferSize) +bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount) { DMA_InitTypeDef DMA_InitStructure; TIM_TypeDef * timer = tch->timHw->tim; @@ -319,22 +320,39 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBu DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)impl_timerCCR(tch); - DMA_InitStructure.DMA_BufferSize = dmaBufferSize; + DMA_InitStructure.DMA_BufferSize = dmaBufferElementCount; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; + switch (dmaBufferElementSize) { + case 1: + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + break; + case 2: + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; + break; + case 4: + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; + break; + default: + // Programmer error + while(1) { + + } + } + #ifdef STM32F4 DMA_InitStructure.DMA_Channel = dmaGetChannelByTag(tch->timHw->dmaTag); DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; DMA_InitStructure.DMA_Priority = DMA_Priority_High; #else // F3 DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dmaBuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; #endif @@ -345,7 +363,7 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint32_t dmaBu return true; } -void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferSize) +void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount) { // Make sure we terminate any DMA transaction currently in progress // Clear the flag as well, so even if DMA transfer finishes while within ATOMIC_BLOCK @@ -356,7 +374,7 @@ void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferSize) DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF); } - DMA_SetCurrDataCounter(tch->dma->ref, dmaBufferSize); + DMA_SetCurrDataCounter(tch->dma->ref, dmaBufferElementCount); DMA_Cmd(tch->dma->ref, ENABLE); tch->dmaState = TCH_DMA_READY; } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 202c2abfc66..b031fd57350 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -50,13 +50,13 @@ extern uint8_t __config_end; #include "config/parameter_group_ids.h" #include "drivers/accgyro/accgyro.h" +#include "drivers/pwm_mapping.h" #include "drivers/buf_writer.h" #include "drivers/bus_i2c.h" #include "drivers/compass/compass.h" #include "drivers/io.h" #include "drivers/io_impl.h" -#include "drivers/logging.h" -#include "drivers/max7456_symbols.h" +#include "drivers/osd_symbols.h" #include "drivers/rx_pwm.h" #include "drivers/sdcard.h" #include "drivers/sensor.h" @@ -91,6 +91,7 @@ extern uint8_t __config_end; #include "io/serial.h" #include "navigation/navigation.h" +#include "navigation/navigation_private.h" #include "rx/rx.h" #include "rx/spektrum.h" @@ -119,10 +120,6 @@ extern uint8_t __config_end; #define PLAY_SOUND #endif -#ifndef USE_MAX7456 -#define TEMP_SENSOR_SYM_COUNT 0 -#endif - extern timeDelta_t cycleTime; // FIXME dependency on mw.c extern uint8_t detectedSensors[SENSOR_INDEX_COUNT]; @@ -138,10 +135,6 @@ static uint32_t bufferIndex = 0; static void cliAssert(char *cmdline); #endif -#if defined(USE_BOOTLOG) -static void cliBootlog(char *cmdline); -#endif - // sync this with features_e static const char * const featureNames[] = { "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP", @@ -150,7 +143,7 @@ static const char * const featureNames[] = { "RX_MSP", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", "SUPEREXPO", "VTX", "RX_SPI", "", "PWM_SERVO_DRIVER", "PWM_OUTPUT_ENABLE", - "OSD", "FW_LAUNCH", "TRACE" , NULL + "OSD", "FW_LAUNCH", NULL }; /* Sensor names (used in lookup tables for *_hardware settings and in status command output) */ @@ -191,7 +184,7 @@ static const char * const *sensorHardwareNames[] = { #else NULL, #endif -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPFLOW table_opflow_hardware, #else NULL, @@ -583,46 +576,6 @@ static void cliAssert(char *cmdline) } #endif -#if defined(USE_BOOTLOG) -static void cliBootlog(char *cmdline) -{ - UNUSED(cmdline); - - int bootEventCount = getBootlogEventCount(); - -#if defined(BOOTLOG_DESCRIPTIONS) - cliPrintLine("Time Evt Description Parameters"); -#else - cliPrintLine("Time Evt Parameters"); -#endif - - for (int idx = 0; idx < bootEventCount; idx++) { - bootLogEntry_t * event = getBootlogEvent(idx); - -#if defined(BOOTLOG_DESCRIPTIONS) - const char * eventDescription = getBootlogEventDescription(event->eventCode); - if (!eventDescription) { - eventDescription = ""; - } - - cliPrintf("%4d: %2d %22s ", event->timestamp, event->eventCode, eventDescription); -#else - cliPrintf("%4d: %2d ", event->timestamp, event->eventCode); -#endif - - if (event->eventFlags & BOOT_EVENT_FLAGS_PARAM16) { - cliPrintLinef(" (%d, %d, %d, %d)", event->params.u16[0], event->params.u16[1], event->params.u16[2], event->params.u16[3]); - } - else if (event->eventFlags & BOOT_EVENT_FLAGS_PARAM32) { - cliPrintLinef(" (%d, %d)", event->params.u32[0], event->params.u32[1]); - } - else { - cliPrintLinefeed(); - } - } -} -#endif - static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActivationConditions, const modeActivationCondition_t *defaultModeActivationConditions) { const char *format = "aux %u %u %u %u %u"; @@ -736,7 +689,6 @@ static void cliSerial(char *cmdline) return; } serialPortConfig_t portConfig; - memset(&portConfig, 0 , sizeof(portConfig)); serialPortConfig_t *currentConfig; @@ -746,15 +698,35 @@ static void cliSerial(char *cmdline) int val = fastA2I(ptr++); currentConfig = serialFindPortConfiguration(val); - if (currentConfig) { - portConfig.identifier = val; - validArgumentCount++; + if (!currentConfig) { + // Invalid port ID + cliPrintLinef("Invalid port ID %d", val); + return; } + memcpy(&portConfig, currentConfig, sizeof(portConfig)); + validArgumentCount++; ptr = nextArg(ptr); if (ptr) { - val = fastA2I(ptr); - portConfig.functionMask = val & 0xFFFFFFFF; + switch (*ptr) { + case '+': + // Add function + ptr++; + val = fastA2I(ptr); + portConfig.functionMask |= (1 << val); + break; + case '-': + // Remove function + ptr++; + val = fastA2I(ptr); + portConfig.functionMask &= 0xFFFFFFFF ^ (1 << val); + break; + default: + // Set functions + val = fastA2I(ptr); + portConfig.functionMask = val & 0xFFFFFFFF; + break; + } validArgumentCount++; } @@ -801,7 +773,7 @@ static void cliSerial(char *cmdline) validArgumentCount++; } - if (validArgumentCount < 6) { + if (validArgumentCount < 2) { cliShowParseError(); return; } @@ -848,7 +820,7 @@ static void cliSerialPassthrough(char *cmdline) serialPortUsage_t *passThroughPortUsage = findSerialPortUsageByIdentifier(id); if (!passThroughPortUsage || passThroughPortUsage->serialPort == NULL) { if (!baud) { - printf("Port %d is closed, must specify baud.\r\n", id); + tfp_printf("Port %d is closed, must specify baud.\r\n", id); return; } if (!mode) @@ -858,29 +830,29 @@ static void cliSerialPassthrough(char *cmdline) baud, mode, SERIAL_NOT_INVERTED); if (!passThroughPort) { - printf("Port %d could not be opened.\r\n", id); + tfp_printf("Port %d could not be opened.\r\n", id); return; } - printf("Port %d opened, baud = %d.\r\n", id, baud); + tfp_printf("Port %d opened, baud = %u.\r\n", id, (unsigned)baud); } else { passThroughPort = passThroughPortUsage->serialPort; // If the user supplied a mode, override the port's mode, otherwise // leave the mode unchanged. serialPassthrough() handles one-way ports. - printf("Port %d already open.\r\n", id); + tfp_printf("Port %d already open.\r\n", id); if (mode && passThroughPort->mode != mode) { - printf("Adjusting mode from %d to %d.\r\n", + tfp_printf("Adjusting mode from %d to %d.\r\n", passThroughPort->mode, mode); serialSetMode(passThroughPort, mode); } // If this port has a rx callback associated we need to remove it now. // Otherwise no data will be pushed in the serial port buffer! if (passThroughPort->rxCallback) { - printf("Removing rxCallback\r\n"); + tfp_printf("Removing rxCallback\r\n"); passThroughPort->rxCallback = 0; } } - printf("Forwarding data to %d, power cycle to exit.\r\n", id); + tfp_printf("Forwarding data to %d, power cycle to exit.\r\n", id); serialPassthrough(cliPort, passThroughPort, NULL, NULL); } @@ -983,7 +955,7 @@ static void cliAdjustmentRange(char *cmdline) } } -static void printMotorMix(uint8_t dumpMask, const motorMixer_t *customMotorMixer, const motorMixer_t *defaultCustomMotorMixer) +static void printMotorMix(uint8_t dumpMask, const motorMixer_t *primaryMotorMixer, const motorMixer_t *defaultprimaryMotorMixer) { const char *format = "mmix %d %s %s %s %s"; char buf0[FTOA_BUFFER_SIZE]; @@ -991,18 +963,18 @@ static void printMotorMix(uint8_t dumpMask, const motorMixer_t *customMotorMixer char buf2[FTOA_BUFFER_SIZE]; char buf3[FTOA_BUFFER_SIZE]; for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - if (customMotorMixer[i].throttle == 0.0f) + if (primaryMotorMixer[i].throttle == 0.0f) break; - const float thr = customMotorMixer[i].throttle; - const float roll = customMotorMixer[i].roll; - const float pitch = customMotorMixer[i].pitch; - const float yaw = customMotorMixer[i].yaw; + const float thr = primaryMotorMixer[i].throttle; + const float roll = primaryMotorMixer[i].roll; + const float pitch = primaryMotorMixer[i].pitch; + const float yaw = primaryMotorMixer[i].yaw; bool equalsDefault = false; - if (defaultCustomMotorMixer) { - const float thrDefault = defaultCustomMotorMixer[i].throttle; - const float rollDefault = defaultCustomMotorMixer[i].roll; - const float pitchDefault = defaultCustomMotorMixer[i].pitch; - const float yawDefault = defaultCustomMotorMixer[i].yaw; + if (defaultprimaryMotorMixer) { + const float thrDefault = defaultprimaryMotorMixer[i].throttle; + const float rollDefault = defaultprimaryMotorMixer[i].roll; + const float pitchDefault = defaultprimaryMotorMixer[i].pitch; + const float yawDefault = defaultprimaryMotorMixer[i].yaw; const bool equalsDefault = thr == thrDefault && roll == rollDefault && pitch == pitchDefault && yaw == yawDefault; cliDefaultPrintLinef(dumpMask, equalsDefault, format, @@ -1027,11 +999,11 @@ static void cliMotorMix(char *cmdline) const char *ptr; if (isEmpty(cmdline)) { - printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL); + printMotorMix(DUMP_MASTER, primaryMotorMixer(0), NULL); } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - customMotorMixerMutable(i)->throttle = 0.0f; + primaryMotorMixerMutable(i)->throttle = 0.0f; } } else { ptr = cmdline; @@ -1039,28 +1011,28 @@ static void cliMotorMix(char *cmdline) if (i < MAX_SUPPORTED_MOTORS) { ptr = nextArg(ptr); if (ptr) { - customMotorMixerMutable(i)->throttle = fastA2F(ptr); + primaryMotorMixerMutable(i)->throttle = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixerMutable(i)->roll = fastA2F(ptr); + primaryMotorMixerMutable(i)->roll = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixerMutable(i)->pitch = fastA2F(ptr); + primaryMotorMixerMutable(i)->pitch = fastA2F(ptr); check++; } ptr = nextArg(ptr); if (ptr) { - customMotorMixerMutable(i)->yaw = fastA2F(ptr); + primaryMotorMixerMutable(i)->yaw = fastA2F(ptr); check++; } if (check != 4) { cliShowParseError(); } else { - printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL); + printMotorMix(DUMP_MASTER, primaryMotorMixer(0), NULL); } } else { cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS - 1); @@ -1235,7 +1207,7 @@ static void cliTempSensor(char *cmdline) sensorConfig->alarm_min = alarm_min; sensorConfig->alarm_max = alarm_max; sensorConfig->osdSymbol = osdSymbol; - for (uint8_t index; index < TEMPERATURE_LABEL_LEN; ++index) { + for (uint8_t index = 0; index < TEMPERATURE_LABEL_LEN; ++index) { sensorConfig->label[index] = toupper(label[index]); if (label[index] == '\0') break; } @@ -1247,6 +1219,123 @@ static void cliTempSensor(char *cmdline) } #endif +#if defined(USE_NAV) && defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI) +static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint) +{ + cliPrintLinef("#wp %d %svalid", posControl.waypointCount, posControl.waypointListValid ? "" : "in"); //int8_t bool + const char *format = "wp %u %u %d %d %d %d %u"; //uint8_t action; int32_t lat; int32_t lon; int32_t alt; int16_t p1; uint8_t flag + for (uint8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) { + bool equalsDefault = false; + if (defaultNavWaypoint) { + equalsDefault = navWaypoint[i].action == defaultNavWaypoint[i].action + && navWaypoint[i].lat == defaultNavWaypoint[i].lat + && navWaypoint[i].lon == defaultNavWaypoint[i].lon + && navWaypoint[i].alt == defaultNavWaypoint[i].alt + && navWaypoint[i].p1 == defaultNavWaypoint[i].p1 + && navWaypoint[i].flag == defaultNavWaypoint[i].flag; + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + i, + defaultNavWaypoint[i].action, + defaultNavWaypoint[i].lat, + defaultNavWaypoint[i].lon, + defaultNavWaypoint[i].alt, + defaultNavWaypoint[i].p1, + defaultNavWaypoint[i].flag + ); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, + i, + navWaypoint[i].action, + navWaypoint[i].lat, + navWaypoint[i].lon, + navWaypoint[i].alt, + navWaypoint[i].p1, + navWaypoint[i].flag + ); + } +} + +static void cliWaypoints(char *cmdline) +{ + if (isEmpty(cmdline)) { + printWaypoints(DUMP_MASTER, posControl.waypointList, NULL); + } else if (sl_strcasecmp(cmdline, "reset") == 0) { + resetWaypointList(); + } else if (sl_strcasecmp(cmdline, "load") == 0) { + loadNonVolatileWaypointList(); + } else if (sl_strcasecmp(cmdline, "save") == 0) { + posControl.waypointListValid = false; + for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) { + if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_RTH)) break; + if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) { + posControl.waypointCount = i + 1; + posControl.waypointListValid = true; + break; + } + } + if (posControl.waypointListValid) { + saveNonVolatileWaypointList(); + } else { + cliShowParseError(); + } + } else { + int16_t i, p1; + uint8_t action, flag; + int32_t lat, lon, alt; + uint8_t validArgumentCount = 0; + const char *ptr = cmdline; + i = fastA2I(ptr); + if (i >= 0 && i < NAV_MAX_WAYPOINTS) { + ptr = nextArg(ptr); + if (ptr) { + action = fastA2I(ptr); + validArgumentCount++; + } + ptr = nextArg(ptr); + if (ptr) { + lat = fastA2I(ptr); + validArgumentCount++; + } + ptr = nextArg(ptr); + if (ptr) { + lon = fastA2I(ptr); + validArgumentCount++; + } + ptr = nextArg(ptr); + if (ptr) { + alt = fastA2I(ptr); + validArgumentCount++; + } + ptr = nextArg(ptr); + if (ptr) { + p1 = fastA2I(ptr); + validArgumentCount++; + } + ptr = nextArg(ptr); + if (ptr) { + flag = fastA2I(ptr); + validArgumentCount++; + } + if (validArgumentCount < 4) { + cliShowParseError(); + } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) { + cliShowParseError(); + } else { + posControl.waypointList[i].action = action; + posControl.waypointList[i].lat = lat; + posControl.waypointList[i].lon = lon; + posControl.waypointList[i].alt = alt; + posControl.waypointList[i].p1 = p1; + posControl.waypointList[i].flag = flag; + } + } else { + cliShowArgumentRangeError("wp index", 0, NAV_MAX_WAYPOINTS - 1); + } + } +} + +#endif + #ifdef USE_LED_STRIP static void printLed(uint8_t dumpMask, const ledConfig_t *ledConfigs, const ledConfig_t *defaultLedConfigs) { @@ -1488,7 +1577,7 @@ static void cliServo(char *cmdline) static void printServoMix(uint8_t dumpMask, const servoMixer_t *customServoMixers, const servoMixer_t *defaultCustomServoMixers) { - const char *format = "smix %d %d %d %d %d"; + const char *format = "smix %d %d %d %d %d %d"; for (uint32_t i = 0; i < MAX_SERVO_RULES; i++) { const servoMixer_t customServoMixer = customServoMixers[i]; if (customServoMixer.rate == 0) { @@ -1501,14 +1590,23 @@ static void printServoMix(uint8_t dumpMask, const servoMixer_t *customServoMixer equalsDefault = customServoMixer.targetChannel == customServoMixerDefault.targetChannel && customServoMixer.inputSource == customServoMixerDefault.inputSource && customServoMixer.rate == customServoMixerDefault.rate - && customServoMixer.speed == customServoMixerDefault.speed; + && customServoMixer.speed == customServoMixerDefault.speed + #ifdef USE_LOGIC_CONDITIONS + && customServoMixer.conditionId == customServoMixerDefault.conditionId + #endif + ; cliDefaultPrintLinef(dumpMask, equalsDefault, format, i, customServoMixerDefault.targetChannel, customServoMixerDefault.inputSource, customServoMixerDefault.rate, - customServoMixerDefault.speed + customServoMixerDefault.speed, + #ifdef USE_LOGIC_CONDITIONS + customServoMixer.conditionId + #else + 0 + #endif ); } cliDumpPrintLinef(dumpMask, equalsDefault, format, @@ -1516,7 +1614,12 @@ static void printServoMix(uint8_t dumpMask, const servoMixer_t *customServoMixer customServoMixer.targetChannel, customServoMixer.inputSource, customServoMixer.rate, - customServoMixer.speed + customServoMixer.speed, + #ifdef USE_LOGIC_CONDITIONS + customServoMixer.conditionId + #else + 0 + #endif ); } } @@ -1524,7 +1627,7 @@ static void printServoMix(uint8_t dumpMask, const servoMixer_t *customServoMixer static void cliServoMix(char *cmdline) { char * saveptr; - int args[8], check = 0; + int args[6], check = 0; uint8_t len = strlen(cmdline); if (len == 0) { @@ -1533,28 +1636,35 @@ static void cliServoMix(char *cmdline) // erase custom mixer pgResetCopy(customServoMixersMutable(0), PG_SERVO_MIXER); } else { - enum {RULE = 0, TARGET, INPUT, RATE, SPEED, ARGS_COUNT}; + enum {RULE = 0, TARGET, INPUT, RATE, SPEED, CONDITION, ARGS_COUNT}; char *ptr = strtok_r(cmdline, " ", &saveptr); + args[CONDITION] = -1; while (ptr != NULL && check < ARGS_COUNT) { args[check++] = fastA2I(ptr); ptr = strtok_r(NULL, " ", &saveptr); } - if (ptr != NULL || check != ARGS_COUNT) { + if (ptr != NULL || (check < ARGS_COUNT - 1)) { cliShowParseError(); return; } int32_t i = args[RULE]; - if (i >= 0 && i < MAX_SERVO_RULES && + 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] >= -1000 && args[RATE] <= 1000 && - args[SPEED] >= 0 && args[SPEED] <= MAX_SERVO_SPEED) { + args[SPEED] >= 0 && args[SPEED] <= MAX_SERVO_SPEED && + args[CONDITION] >= -1 && args[CONDITION] < MAX_LOGIC_CONDITIONS + ) { customServoMixersMutable(i)->targetChannel = args[TARGET]; customServoMixersMutable(i)->inputSource = args[INPUT]; customServoMixersMutable(i)->rate = args[RATE]; customServoMixersMutable(i)->speed = args[SPEED]; + #ifdef USE_LOGIC_CONDITIONS + customServoMixersMutable(i)->conditionId = args[CONDITION]; + #endif cliServoMix(""); } else { cliShowParseError(); @@ -1562,6 +1672,110 @@ static void cliServoMix(char *cmdline) } } +#ifdef USE_LOGIC_CONDITIONS + +static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions, const logicCondition_t *defaultLogicConditions) +{ + const char *format = "logic %d %d %d %d %d %d %d %d"; + for (uint32_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) { + const logicCondition_t logic = logicConditions[i]; + + bool equalsDefault = false; + if (defaultLogicConditions) { + logicCondition_t defaultValue = defaultLogicConditions[i]; + equalsDefault = + logic.enabled == defaultValue.enabled && + logic.operation == defaultValue.operation && + logic.operandA.type == defaultValue.operandA.type && + logic.operandA.value == defaultValue.operandA.value && + logic.operandB.type == defaultValue.operandB.type && + logic.operandB.value == defaultValue.operandB.value && + logic.flags == defaultValue.flags; + + cliDefaultPrintLinef(dumpMask, equalsDefault, format, + i, + logic.enabled, + logic.operation, + logic.operandA.type, + logic.operandA.value, + logic.operandB.type, + logic.operandB.value, + logic.flags + ); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, + i, + logic.enabled, + logic.operation, + logic.operandA.type, + logic.operandA.value, + logic.operandB.type, + logic.operandB.value, + logic.flags + ); + } +} + +static void cliLogic(char *cmdline) { + char * saveptr; + int args[8], check = 0; + uint8_t len = strlen(cmdline); + + if (len == 0) { + printLogic(DUMP_MASTER, logicConditions(0), NULL); + } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) { + pgResetCopy(logicConditionsMutable(0), PG_SERVO_MIXER); + } else { + enum { + INDEX = 0, + ENABLED, + OPERATION, + OPERAND_A_TYPE, + OPERAND_A_VALUE, + OPERAND_B_TYPE, + OPERAND_B_VALUE, + FLAGS, + ARGS_COUNT + }; + char *ptr = strtok_r(cmdline, " ", &saveptr); + while (ptr != NULL && check < ARGS_COUNT) { + args[check++] = fastA2I(ptr); + ptr = strtok_r(NULL, " ", &saveptr); + } + + if (ptr != NULL || check != ARGS_COUNT) { + cliShowParseError(); + return; + } + + int32_t i = args[INDEX]; + if ( + i >= 0 && i < MAX_LOGIC_CONDITIONS && + args[ENABLED] >= 0 && args[ENABLED] <= 1 && + args[OPERATION] >= 0 && args[OPERATION] < LOGIC_CONDITION_LAST && + args[OPERAND_A_TYPE] >= 0 && args[OPERAND_A_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST && + args[OPERAND_A_VALUE] >= -1000000 && args[OPERAND_A_VALUE] <= 1000000 && + args[OPERAND_B_TYPE] >= 0 && args[OPERAND_B_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST && + args[OPERAND_B_VALUE] >= -1000000 && args[OPERAND_B_VALUE] <= 1000000 && + args[FLAGS] >= 0 && args[FLAGS] <= 255 + + ) { + logicConditionsMutable(i)->enabled = args[ENABLED]; + logicConditionsMutable(i)->operation = args[OPERATION]; + logicConditionsMutable(i)->operandA.type = args[OPERAND_A_TYPE]; + logicConditionsMutable(i)->operandA.value = args[OPERAND_A_VALUE]; + logicConditionsMutable(i)->operandB.type = args[OPERAND_B_TYPE]; + logicConditionsMutable(i)->operandB.value = args[OPERAND_B_VALUE]; + logicConditionsMutable(i)->flags = args[FLAGS]; + + cliLogic(""); + } else { + cliShowParseError(); + } + } +} +#endif + #ifdef USE_SDCARD static void cliWriteBytes(const uint8_t *buffer, int count) @@ -2308,6 +2522,8 @@ static void cliGet(char *cmdline) int matchedCommands = 0; char name[SETTING_MAX_NAME_LENGTH]; + while(*cmdline == ' ') ++cmdline; // ignore spaces + for (uint32_t i = 0; i < SETTINGS_TABLE_COUNT; i++) { val = settingGet(i); if (settingNameContains(val, name, cmdline)) { @@ -2336,6 +2552,8 @@ static void cliSet(char *cmdline) char *eqptr = NULL; char name[SETTING_MAX_NAME_LENGTH]; + while(*cmdline == ' ') ++cmdline; // ignore spaces + len = strlen(cmdline); if (len == 0 || (len == 1 && cmdline[0] == '*')) { @@ -2555,6 +2773,11 @@ static void cliStatus(char *cmdline) #else cliPrintLinef("Arming disabled flags: 0x%lx", armingFlags & ARMING_DISABLED_ALL_FLAGS); #endif + + // If we are blocked by PWM init - provide more information + if (getPwmInitError() != PWM_INIT_ERROR_NONE) { + cliPrintLinef("PWM output init error: %s", getPwmInitErrorMessage()); + } } #ifndef SKIP_TASK_STATISTICS @@ -2712,9 +2935,9 @@ static void printConfig(const char *cmdline, bool doDiff) //printResource(dumpMask, &defaultConfig); cliPrintHashLine("mixer"); - cliDumpPrintLinef(dumpMask, customMotorMixer(0)->throttle == 0.0f, "\r\nmmix reset\r\n"); + cliDumpPrintLinef(dumpMask, primaryMotorMixer(0)->throttle == 0.0f, "\r\nmmix reset\r\n"); - printMotorMix(dumpMask, customMotorMixer_CopyArray, customMotorMixer(0)); + printMotorMix(dumpMask, primaryMotorMixer_CopyArray, primaryMotorMixer(0)); // print custom servo mixer if exists cliPrintHashLine("servo mix"); @@ -2725,6 +2948,11 @@ static void printConfig(const char *cmdline, bool doDiff) cliPrintHashLine("servo"); printServo(dumpMask, servoParams_CopyArray, servoParams(0)); +#ifdef USE_LOGIC_CONDITIONS + cliPrintHashLine("logic"); + printLogic(dumpMask, logicConditions_CopyArray, logicConditions(0)); +#endif + cliPrintHashLine("feature"); printFeature(dumpMask, &featureConfig_Copy, featureConfig()); @@ -2764,6 +2992,11 @@ static void printConfig(const char *cmdline, bool doDiff) printTempSensor(dumpMask, tempSensorConfig_CopyArray, tempSensorConfig(0)); #endif +#if defined(USE_NAV) && defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI) + cliPrintHashLine("wp"); + printWaypoints(dumpMask, posControl.waypointList, nonVolatileWaypointList(0)); +#endif + #ifdef USE_OSD cliPrintHashLine("osd_layout"); printOsdLayout(dumpMask, &osdConfig_Copy, osdConfig(), -1, -1); @@ -2913,9 +3146,14 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("serialpassthrough", "passthrough serial data to port", " [baud] [mode] : passthrough to serial", cliSerialPassthrough), #endif CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo), +#ifdef USE_LOGIC_CONDITIONS + CLI_COMMAND_DEF("logic", "configure logic conditions", + " \r\n" + "\treset\r\n", cliLogic), +#endif CLI_COMMAND_DEF("set", "change setting", "[=]", cliSet), CLI_COMMAND_DEF("smix", "servo mixer", - " \r\n" + " \r\n" "\treset\r\n", cliServoMix), #ifdef USE_SDCARD CLI_COMMAND_DEF("sd_info", "sdcard info", NULL, cliSdInfo), @@ -2928,6 +3166,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("temp_sensor", "change temp sensor settings", NULL, cliTempSensor), #endif CLI_COMMAND_DEF("version", "show version", NULL, cliVersion), +#if defined(USE_NAV) && defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI) + CLI_COMMAND_DEF("wp", "waypoint list", NULL, cliWaypoints), +#endif #ifdef USE_OSD CLI_COMMAND_DEF("osd_layout", "get or set the layout of OSD items", "[ [ [ []]]]", cliOsdLayout), #endif diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 2a5905f8bb6..e005fb1d94a 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -38,6 +38,7 @@ #include "drivers/system.h" #include "drivers/rx_spi.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "drivers/serial.h" #include "drivers/timer.h" @@ -106,7 +107,7 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = DEFAULT_FEATURES | COMMON_DEFAULT_FEATURES ); -PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 3); PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, @@ -115,7 +116,6 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .i2c_speed = I2C_SPEED_400KHZ, .cpuUnderclock = 0, .throttle_tilt_compensation_strength = 0, // 0-100, 0 - disabled - .pwmRxInputFilteringMode = INPUT_FILTERING_DISABLED, .name = { 0 } ); @@ -297,6 +297,11 @@ void validateAndFixConfig(void) case PWM_TYPE_DSHOT1200: motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 32000); break; +#endif +#ifdef USE_SERIALSHOT + case PWM_TYPE_SERIALSHOT: // 2-4 kHz + motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 4000); + break; #endif } #endif diff --git a/src/main/fc/config.h b/src/main/fc/config.h index e3832cffe3f..d79dd49562c 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -67,7 +67,6 @@ typedef enum { FEATURE_PWM_OUTPUT_ENABLE = 1 << 28, FEATURE_OSD = 1 << 29, FEATURE_FW_LAUNCH = 1 << 30, - FEATURE_DEBUG_TRACE = 1 << 31, } features_e; typedef struct systemConfig_s { @@ -77,7 +76,6 @@ typedef struct systemConfig_s { uint8_t i2c_speed; uint8_t cpuUnderclock; uint8_t throttle_tilt_compensation_strength; // the correction that will be applied at throttle_correction_angle. - inputFilteringMode_e pwmRxInputFilteringMode; char name[MAX_NAME_LENGTH + 1]; } systemConfig_t; diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index a9ce120e2d6..156819a3cf1 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -97,6 +97,21 @@ enum { #define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro +#define EMERGENCY_ARMING_TIME_WINDOW_MS 10000 +#define EMERGENCY_ARMING_COUNTER_STEP_MS 100 + +typedef struct emergencyArmingState_s { + bool armingSwitchWasOn; + // Each entry in the queue is an offset from start, + // in 0.1s increments. This lets us represent up to 25.5s + // so it will work fine as long as the window for the triggers + // is smaller (see EMERGENCY_ARMING_TIME_WINDOW_MS). First + // entry of the queue is implicit. + timeMs_t start; + uint8_t queue[9]; + uint8_t queueCount; +} emergencyArmingState_t; + timeDelta_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop static timeUs_t flightTime = 0; @@ -106,10 +121,9 @@ int16_t headFreeModeHold; uint8_t motorControlEnable = false; -static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero - static bool isRXDataNew; static disarmReason_t lastDisarmReason = DISARM_NONE; +static emergencyArmingState_t emergencyArming; bool isCalibrating(void) { @@ -217,7 +231,7 @@ static void updateArmingStatus(void) #if defined(USE_NAV) /* CHECK: Navigation safety */ - if (navigationBlockArming()) { + if (navigationIsBlockingArming(NULL) != NAV_ARMING_BLOCKER_NONE) { ENABLE_ARMING_FLAG(ARMING_DISABLED_NAVIGATION_UNSAFE); } else { @@ -251,16 +265,6 @@ static void updateArmingStatus(void) DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); } - /* CHECK: Arming switch */ - if (!isUsingSticksForArming()) { - // If arming is disabled and the ARM switch is on - if (isArmingDisabled() && IS_RC_MODE_ACTIVE(BOXARM)) { - ENABLE_ARMING_FLAG(ARMING_DISABLED_ARM_SWITCH); - } else if (!IS_RC_MODE_ACTIVE(BOXARM)) { - DISABLE_ARMING_FLAG(ARMING_DISABLED_ARM_SWITCH); - } - } - /* CHECK: BOXFAILSAFE */ if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) { ENABLE_ARMING_FLAG(ARMING_DISABLED_BOXFAILSAFE); @@ -269,21 +273,32 @@ static void updateArmingStatus(void) DISABLE_ARMING_FLAG(ARMING_DISABLED_BOXFAILSAFE); } - /* CHECK: BOXFAILSAFE */ + /* CHECK: BOXKILLSWITCH */ if (IS_RC_MODE_ACTIVE(BOXKILLSWITCH)) { ENABLE_ARMING_FLAG(ARMING_DISABLED_BOXKILLSWITCH); } else { DISABLE_ARMING_FLAG(ARMING_DISABLED_BOXKILLSWITCH); } + /* CHECK: Do not allow arming if Servo AutoTrim is enabled */ if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { - ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); + ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); } else { - DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); + DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); } + /* CHECK: Arming switch */ + // If arming is disabled and the ARM switch is on + // Note that this should be last check so all other blockers could be cleared correctly + // if blocking modes are linked to the same RC channel + if (isArmingDisabled() && IS_RC_MODE_ACTIVE(BOXARM)) { + ENABLE_ARMING_FLAG(ARMING_DISABLED_ARM_SWITCH); + } else if (!IS_RC_MODE_ACTIVE(BOXARM)) { + DISABLE_ARMING_FLAG(ARMING_DISABLED_ARM_SWITCH); + } + if (isArmingDisabled()) { warningLedFlash(); } else { @@ -294,6 +309,26 @@ static void updateArmingStatus(void) } } +static bool emergencyArmingIsTriggered(void) +{ + int threshold = (EMERGENCY_ARMING_TIME_WINDOW_MS / EMERGENCY_ARMING_COUNTER_STEP_MS); + return emergencyArming.queueCount == ARRAYLEN(emergencyArming.queue) + 1 && + emergencyArming.queue[ARRAYLEN(emergencyArming.queue) - 1] < threshold && + emergencyArming.start >= millis() - EMERGENCY_ARMING_TIME_WINDOW_MS; +} + +static bool emergencyArmingCanOverrideArmingDisabled(void) +{ + uint32_t armingPrevention = armingFlags & ARMING_DISABLED_ALL_FLAGS; + armingPrevention &= ~ARMING_DISABLED_EMERGENCY_OVERRIDE; + return armingPrevention == 0; +} + +static bool emergencyArmingIsEnabled(void) +{ + return emergencyArmingIsTriggered() && emergencyArmingCanOverrideArmingDisabled(); +} + void annexCode(void) { int32_t throttleValue; @@ -304,9 +339,9 @@ void annexCode(void) } else { // Compute ROLL PITCH and YAW command - rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); - rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); - rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcYawExpo8 : currentControlRateProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband); + rcCommand[ROLL] = getAxisRcCommand(rxGetChannelValue(ROLL), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); + rcCommand[PITCH] = getAxisRcCommand(rxGetChannelValue(PITCH), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); + rcCommand[YAW] = -getAxisRcCommand(rxGetChannelValue(YAW), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcYawExpo8 : currentControlRateProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband); // Apply manual control rates if (FLIGHT_MODE(MANUAL_MODE)) { @@ -316,7 +351,7 @@ void annexCode(void) } //Compute THROTTLE command - throttleValue = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); + throttleValue = constrain(rxGetChannelValue(THROTTLE), rxConfig()->mincheck, PWM_RANGE_MAX); throttleValue = (uint32_t)(throttleValue - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000] rcCommand[THROTTLE] = rcLookupThrottle(throttleValue); @@ -359,6 +394,39 @@ disarmReason_t getDisarmReason(void) return lastDisarmReason; } +void emergencyArmingUpdate(bool armingSwitchIsOn) +{ + if (armingSwitchIsOn == emergencyArming.armingSwitchWasOn) { + return; + } + if (armingSwitchIsOn) { + timeMs_t now = millis(); + if (emergencyArming.queueCount == 0) { + emergencyArming.queueCount = 1; + emergencyArming.start = now; + } else { + while (emergencyArming.start < now - EMERGENCY_ARMING_TIME_WINDOW_MS || emergencyArmingIsTriggered()) { + if (emergencyArming.queueCount > 1) { + uint8_t delta = emergencyArming.queue[0]; + emergencyArming.start += delta * EMERGENCY_ARMING_COUNTER_STEP_MS; + for (int ii = 0; ii < emergencyArming.queueCount - 2; ii++) { + emergencyArming.queue[ii] = emergencyArming.queue[ii + 1] - delta; + } + emergencyArming.queueCount--; + } else { + emergencyArming.start = now; + } + } + uint8_t delta = (now - emergencyArming.start) / EMERGENCY_ARMING_COUNTER_STEP_MS; + if (delta > 0) { + emergencyArming.queue[emergencyArming.queueCount - 1] = delta; + emergencyArming.queueCount++; + } + } + } + emergencyArming.armingSwitchWasOn = !emergencyArming.armingSwitchWasOn; +} + #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) void releaseSharedTelemetryPorts(void) { @@ -373,11 +441,23 @@ void tryArm(void) { updateArmingStatus(); - if (!isArmingDisabled()) { + if (!isArmingDisabled() || emergencyArmingIsEnabled()) { if (ARMING_FLAG(ARMED)) { return; } +#if defined(USE_NAV) + // If nav_extra_arming_safety was bypassed we always + // allow bypassing it even without the sticks set + // in the correct position to allow re-arming quickly + // in case of a mid-air accidental disarm. + bool usedBypass = false; + navigationIsBlockingArming(&usedBypass); + if (usedBypass) { + ENABLE_STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED); + } +#endif + lastDisarmReason = DISARM_NONE; ENABLE_ARMING_FLAG(ARMED); @@ -395,7 +475,6 @@ void tryArm(void) blackboxStart(); } #endif - disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero //beep to indicate arming #ifdef USE_NAV @@ -418,8 +497,6 @@ void tryArm(void) void processRx(timeUs_t currentTimeUs) { - static bool armedBeeperOn = false; - // Calculate RPY channel data calculateRxChannelsAndUpdateFailsafe(currentTimeUs); @@ -440,52 +517,21 @@ void processRx(timeUs_t currentTimeUs) const throttleStatus_e throttleStatus = calculateThrottleStatus(); - // When armed and motors aren't spinning, do beeps and then disarm - // board after delay so users without buzzer won't lose fingers. - // mixTable constrains motor commands, so checking throttleStatus is enough - if (ARMING_FLAG(ARMED) - && feature(FEATURE_MOTOR_STOP) - && !STATE(FIXED_WING) - ) { - if (isUsingSticksForArming()) { - if (throttleStatus == THROTTLE_LOW) { - if (armingConfig()->auto_disarm_delay != 0 - && (int32_t)(disarmAt - millis()) < 0 - ) { - // auto-disarm configured and delay is over - disarm(DISARM_TIMEOUT); - armedBeeperOn = false; - } else { - // still armed; do warning beeps while armed - beeper(BEEPER_ARMED); - armedBeeperOn = true; - } - } else { - // throttle is not low - if (armingConfig()->auto_disarm_delay != 0) { - // extend disarm time - disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; - } + // When armed and motors aren't spinning, do beeps periodically + if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING)) { + static bool armedBeeperOn = false; - if (armedBeeperOn) { - beeperSilence(); - armedBeeperOn = false; - } - } - } else { - // arming is via AUX switch; beep while throttle low - if (throttleStatus == THROTTLE_LOW) { - beeper(BEEPER_ARMED); - armedBeeperOn = true; - } else if (armedBeeperOn) { - beeperSilence(); - armedBeeperOn = false; - } + if (throttleStatus == THROTTLE_LOW) { + beeper(BEEPER_ARMED); + armedBeeperOn = true; + } else if (armedBeeperOn) { + beeperSilence(); + armedBeeperOn = false; } } processRcStickPositions(throttleStatus); - + processAirmode(); updateActivatedModes(); #ifdef USE_PINIOBOX @@ -589,9 +635,9 @@ void processRx(timeUs_t currentTimeUs) /* In MANUAL mode we reset integrators prevent I-term wind-up (PID output is not used in MANUAL) */ pidResetErrorAccumulators(); } - else { + else if (STATE(FIXED_WING) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { if (throttleStatus == THROTTLE_LOW) { - if (isAirmodeActive() && !failsafeIsActive() && ARMING_FLAG(ARMED)) { + if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) { rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(); // ANTI_WINDUP at centred stick with MOTOR_STOP is needed on MRs and not needed on FWs @@ -610,6 +656,12 @@ void processRx(timeUs_t currentTimeUs) else { DISABLE_STATE(ANTI_WINDUP); } + } else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) { + DISABLE_STATE(ANTI_WINDUP); + //This case applies only to MR when Airmode management is throttle threshold activated + if (throttleStatus == THROTTLE_LOW && !STATE(AIRMODE_ACTIVE)) { + pidResetErrorAccumulators(); + } } if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { @@ -637,7 +689,7 @@ void processRx(timeUs_t currentTimeUs) } // Function for loop trigger -void taskGyro(timeUs_t currentTimeUs) { +void FAST_CODE NOINLINE taskGyro(timeUs_t currentTimeUs) { // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point. // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF); @@ -655,7 +707,7 @@ void taskGyro(timeUs_t currentTimeUs) { /* Update actual hardware readings */ gyroUpdate(); -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPFLOW if (sensors(SENSOR_OPFLOW)) { opflowGyroUpdateCallback((timeUs_t)currentDeltaTime + (gyroUpdateUs - currentTimeUs)); } @@ -679,6 +731,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) { flightTime += cycleTime; + updateAccExtremes(); } taskGyro(currentTimeUs); @@ -704,17 +757,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs) applyWaypointNavigationAndAltitudeHold(); #endif - // If we're armed, at minimum throttle, and we do arming via the - // sticks, do not process yaw input from the rx. We do this so the - // motors do not spin up while we are trying to arm or disarm. - // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. - if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck - && !((mixerConfig()->platformType == PLATFORM_TRICOPTER) && servoConfig()->tri_unarmed_servo) - && mixerConfig()->platformType != PLATFORM_AIRPLANE - ) { - rcCommand[YAW] = 0; - } - // Apply throttle tilt compensation if (!STATE(FIXED_WING)) { int16_t thrTiltCompStrength = 0; @@ -784,7 +826,7 @@ void taskRunRealtimeCallbacks(timeUs_t currentTimeUs) #endif #ifdef USE_DSHOT - pwmCompleteDshotUpdate(getMotorCount()); + pwmCompleteMotorUpdate(); #endif } diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 3d7d0c9a360..a7317e48d1d 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -17,6 +17,8 @@ #pragma once +#include + #include "common/time.h" typedef enum disarmReason_e { @@ -38,7 +40,9 @@ void disarm(disarmReason_t disarmReason); void tryArm(void); disarmReason_t getDisarmReason(void); +void emergencyArmingUpdate(bool armingSwitchIsOn); + bool isCalibrating(void); -float getFlightTime(); +float getFlightTime(void); -void fcReboot(bool bootLoader); \ No newline at end of file +void fcReboot(bool bootLoader); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 3aab52166c4..85276b2d121 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -22,6 +22,7 @@ #include "platform.h" #include "blackbox/blackbox.h" +#include "blackbox/blackbox_io.h" #include "build/assert.h" #include "build/atomic.h" @@ -30,9 +31,10 @@ #include "common/axis.h" #include "common/color.h" +#include "common/log.h" #include "common/maths.h" -#include "common/printf.h" #include "common/memory.h" +#include "common/printf.h" #include "config/config_eeprom.h" #include "config/feature.h" @@ -52,8 +54,8 @@ #include "drivers/io.h" #include "drivers/io_pca9685.h" #include "drivers/light_led.h" -#include "drivers/logging.h" #include "drivers/nvic.h" +#include "drivers/osd.h" #include "drivers/pwm_esc_detect.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" @@ -70,7 +72,6 @@ #include "drivers/time.h" #include "drivers/timer.h" #include "drivers/uart_inverter.h" -#include "drivers/vcd.h" #include "drivers/io.h" #include "drivers/exti.h" #include "drivers/io_pca9685.h" @@ -178,8 +179,6 @@ void init(void) #endif systemState = SYSTEM_STATE_INITIALISING; - initBootlog(); - printfSupportInit(); // Initialize system and CPU clocks to their initial values @@ -209,7 +208,6 @@ void init(void) initialisePreBootHardware(); #endif - addBootlogEvent2(BOOT_EVENT_CONFIG_LOADED, BOOT_EVENT_FLAGS_NONE); systemState |= SYSTEM_STATE_CONFIG_LOADED; debugMode = systemConfig()->debug_mode; @@ -227,8 +225,6 @@ void init(void) EXTIInit(); #endif - addBootlogEvent2(BOOT_EVENT_SYSTEM_INIT_DONE, BOOT_EVENT_FLAGS_NONE); - #ifdef USE_SPEKTRUM_BIND if (rxConfig()->receiverType == RX_TYPE_SERIAL) { switch (rxConfig()->serialrx_provider) { @@ -260,54 +256,44 @@ void init(void) serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif - // Initialize MSP serial ports here so DEBUG_TRACE can share a port with MSP. + // Initialize MSP serial ports here so LOG can share a port with MSP. // XXX: Don't call mspFcInit() yet, since it initializes the boxes and needs // to run after the sensors have been detected. mspSerialInit(); -#if defined(USE_DEBUG_TRACE) - // Debug trace uses serial output, so we only can init it after serial port is ready - // From this point on we can use DEBUG_TRACE() to produce real-time debugging information - debugTraceInit(); +#if defined(USE_LOG) + // LOG might use serial output, so we only can init it after serial port is ready + // From this point on we can use LOG_*() to produce real-time debugging information + logInit(); #endif + // Initialize servo and motor mixers + // This needs to be called early to set up platform type correctly and count required motors & servos servosInit(); - mixerUpdateStateFlags(); // This needs to be called early to allow pwm mapper to use information about FIXED_WING state + mixerUpdateStateFlags(); + mixerInit(); - drv_pwm_config_t pwm_params; - memset(&pwm_params, 0, sizeof(pwm_params)); + // Some sanity checking + if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) { + featureClear(FEATURE_3D); + } -#ifdef USE_RANGEFINDER_HCSR04 - // HC-SR04 has a dedicated connection to FC and require two pins - if (rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) { - const rangefinderHardwarePins_t *rangefinderHardwarePins = rangefinderGetHardwarePins(); - if (rangefinderHardwarePins) { - pwm_params.useTriggerRangefinder = true; - pwm_params.rangefinderIOConfig.triggerTag = rangefinderHardwarePins->triggerTag; - pwm_params.rangefinderIOConfig.echoTag = rangefinderHardwarePins->echoTag; - } + // Initialize motor and servo outpus + if (pwmMotorAndServoInit()) { + DISABLE_ARMING_FLAG(ARMING_DISABLED_PWM_OUTPUT_ERROR); + } + else { + ENABLE_ARMING_FLAG(ARMING_DISABLED_PWM_OUTPUT_ERROR); } -#endif + + /* + drv_pwm_config_t pwm_params; + memset(&pwm_params, 0, sizeof(pwm_params)); // when using airplane/wing mixer, servo/motor outputs are remapped pwm_params.flyingPlatformType = mixerConfig()->platformType; -#ifdef STM32F303xC - pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3); -#endif -#if defined(USE_UART2) && defined(STM32F40_41xxx) - pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); -#endif -#if defined(USE_UART6) && defined(STM32F40_41xxx) - pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6); -#endif - pwm_params.useVbat = feature(FEATURE_VBAT); - pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = (rxConfig()->receiverType == RX_TYPE_PWM); - pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); - pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) - && batteryMetersConfig()->current.type == CURRENT_SENSOR_ADC; - pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = (rxConfig()->receiverType == RX_TYPE_PPM); pwm_params.useSerialRx = (rxConfig()->receiverType == RX_TYPE_SERIAL); @@ -315,18 +301,6 @@ void init(void) pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse; pwm_params.servoPwmRate = servoConfig()->servoPwmRate; - pwm_params.pwmProtocolType = motorConfig()->motorPwmProtocol; -#ifndef BRUSHED_MOTORS - pwm_params.useFastPwm = (motorConfig()->motorPwmProtocol == PWM_TYPE_ONESHOT125) || - (motorConfig()->motorPwmProtocol == PWM_TYPE_ONESHOT42) || - (motorConfig()->motorPwmProtocol == PWM_TYPE_MULTISHOT); -#endif - pwm_params.motorPwmRate = motorConfig()->motorPwmRate; - - if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) { - pwm_params.useFastPwm = false; - featureClear(FEATURE_3D); - } pwm_params.enablePWMOutput = feature(FEATURE_PWM_OUTPUT_ENABLE); @@ -335,24 +309,14 @@ void init(void) #endif #ifdef USE_PWM_SERVO_DRIVER - /* - If external PWM driver is enabled, for example PCA9685, disable internal - servo handling mechanism, since external device will do that - */ + // If external PWM driver is enabled, for example PCA9685, disable internal + // servo handling mechanism, since external device will do that if (feature(FEATURE_PWM_SERVO_DRIVER)) { pwm_params.useServoOutputs = false; } #endif + */ - // pwmInit() needs to be called as soon as possible for ESC compatibility reasons - pwmInit(&pwm_params); - - mixerUsePWMIOConfiguration(); - - if (!pwm_params.useFastPwm) - motorControlEnable = true; - - addBootlogEvent2(BOOT_EVENT_PWM_INIT_DONE, BOOT_EVENT_FLAGS_NONE); systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER @@ -380,27 +344,6 @@ void init(void) // Initialize buses busInit(); -#ifdef USE_SPI -#ifdef USE_SPI_DEVICE_1 - spiInit(SPIDEV_1); -#endif -#ifdef USE_SPI_DEVICE_2 - spiInit(SPIDEV_2); -#endif -#ifdef USE_SPI_DEVICE_3 -#ifdef ALIENFLIGHTF3 - if (hardwareRevision == AFF3_REV_2) { - spiInit(SPIDEV_3); - } -#else - spiInit(SPIDEV_3); -#endif -#endif -#ifdef USE_SPI_DEVICE_4 - spiInit(SPIDEV_4); -#endif -#endif - #ifdef USE_HARDWARE_REVISION_DETECTION updateHardwareRevision(); #endif @@ -498,8 +441,6 @@ void init(void) /* Extra 500ms delay prior to initialising hardware if board is cold-booting */ if (!isMPUSoftReset()) { - addBootlogEvent2(BOOT_EVENT_EXTRA_BOOT_DELAY, BOOT_EVENT_FLAGS_NONE); - LED1_ON; LED0_OFF; @@ -542,7 +483,6 @@ void init(void) failureMode(FAILURE_MISSING_ACC); } - addBootlogEvent2(BOOT_EVENT_SENSOR_INIT_DONE, BOOT_EVENT_FLAGS_NONE); systemState |= SYSTEM_STATE_SENSORS_READY; flashLedsAndBeep(); @@ -592,7 +532,6 @@ void init(void) #ifdef USE_GPS if (feature(FEATURE_GPS)) { gpsInit(); - addBootlogEvent2(BOOT_EVENT_GPS_INIT_DONE, BOOT_EVENT_FLAGS_NONE); } #endif @@ -606,32 +545,39 @@ void init(void) if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); - addBootlogEvent2(BOOT_EVENT_LEDSTRIP_INIT_DONE, BOOT_EVENT_FLAGS_NONE); } #endif #ifdef USE_TELEMETRY if (feature(FEATURE_TELEMETRY)) { telemetryInit(); - addBootlogEvent2(BOOT_EVENT_TELEMETRY_INIT_DONE, BOOT_EVENT_FLAGS_NONE); } #endif +#ifdef USE_BLACKBOX + // SDCARD and FLASHFS are used only for blackbox + // Make sure we only init what's necessary for blackbox + switch (blackboxConfig()->device) { #ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: #ifdef USE_FLASH_M25P16 - m25p16_init(0); + m25p16_init(0); #endif - - flashfsInit(); + flashfsInit(); + break; #endif #ifdef USE_SDCARD - sdcardInsertionDetectInit(); - sdcard_init(); - afatfs_init(); -#endif + case BLACKBOX_DEVICE_SDCARD: + sdcardInsertionDetectInit(); + sdcard_init(); + afatfs_init(); + break; +#endif + default: + break; + } -#ifdef USE_BLACKBOX blackboxInit(); #endif @@ -683,6 +629,5 @@ void init(void) motorControlEnable = true; fcTasksInit(); - addBootlogEvent2(BOOT_EVENT_SYSTEM_READY, BOOT_EVENT_FLAGS_NONE); systemState |= SYSTEM_STATE_READY; } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ccdf23cab4f..d72fab4d0aa 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -41,7 +41,9 @@ #include "drivers/accgyro/accgyro.h" #include "drivers/bus_i2c.h" #include "drivers/compass/compass.h" -#include "drivers/max7456.h" +#include "drivers/display.h" +#include "drivers/osd.h" +#include "drivers/osd_symbols.h" #include "drivers/pwm_mapping.h" #include "drivers/sdcard.h" #include "drivers/serial.h" @@ -314,7 +316,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF // 0 = no OSD // 1 = OSD slave (not supported in INAV) // 2 = OSD chip on board -#if defined(USE_OSD) && defined(USE_MAX7456) +#if defined(USE_OSD) sbufWriteU8(dst, 2); #else sbufWriteU8(dst, 0); @@ -463,12 +465,38 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, 0); } break; + case MSP2_INAV_SERVO_MIXER: + for (int i = 0; i < MAX_SERVO_RULES; i++) { + sbufWriteU8(dst, customServoMixers(i)->targetChannel); + sbufWriteU8(dst, customServoMixers(i)->inputSource); + sbufWriteU16(dst, customServoMixers(i)->rate); + sbufWriteU8(dst, customServoMixers(i)->speed); + #ifdef USE_LOGIC_CONDITIONS + sbufWriteU8(dst, customServoMixers(i)->conditionId); + #else + sbufWriteU8(dst, -1); + #endif + } + break; +#ifdef USE_LOGIC_CONDITIONS + case MSP2_INAV_LOGIC_CONDITIONS: + for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) { + sbufWriteU8(dst, logicConditions(i)->enabled); + sbufWriteU8(dst, logicConditions(i)->operation); + sbufWriteU8(dst, logicConditions(i)->operandA.type); + sbufWriteU32(dst, logicConditions(i)->operandA.value); + sbufWriteU8(dst, logicConditions(i)->operandB.type); + sbufWriteU32(dst, logicConditions(i)->operandB.value); + sbufWriteU8(dst, logicConditions(i)->flags); + } + break; +#endif case MSP2_COMMON_MOTOR_MIXER: for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - sbufWriteU16(dst, customMotorMixer(i)->throttle * 1000); - sbufWriteU16(dst, constrainf(customMotorMixer(i)->roll + 2.0f, 0.0f, 4.0f) * 1000); - sbufWriteU16(dst, constrainf(customMotorMixer(i)->pitch + 2.0f, 0.0f, 4.0f) * 1000); - sbufWriteU16(dst, constrainf(customMotorMixer(i)->yaw + 2.0f, 0.0f, 4.0f) * 1000); + sbufWriteU16(dst, primaryMotorMixer(i)->throttle * 1000); + sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->roll + 2.0f, 0.0f, 4.0f) * 1000); + sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->pitch + 2.0f, 0.0f, 4.0f) * 1000); + sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->yaw + 2.0f, 0.0f, 4.0f) * 1000); } break; @@ -480,7 +508,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_RC: for (int i = 0; i < rxRuntimeConfig.channelCount; i++) { - sbufWriteU16(dst, rcRaw[i]); + sbufWriteU16(dst, rxGetRawChannelValue(i)); } break; @@ -514,7 +542,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_OPTICAL_FLOW: -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPFLOW sbufWriteU8(dst, opflow.rawQuality); sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[X])); sbufWriteU16(dst, RADIANS_TO_DEGREES(opflow.flowRate[Y])); @@ -550,7 +578,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP_ARMING_CONFIG: - sbufWriteU8(dst, armingConfig()->auto_disarm_delay); + sbufWriteU8(dst, 0); sbufWriteU8(dst, armingConfig()->disarm_kill_switch); break; @@ -601,6 +629,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; + case MSP2_PID: + for (int i = 0; i < PID_ITEM_COUNT; i++) { + sbufWriteU8(dst, pidBank()->pid[i].P); + sbufWriteU8(dst, pidBank()->pid[i].I); + sbufWriteU8(dst, pidBank()->pid[i].D); + sbufWriteU8(dst, pidBank()->pid[i].FF); + } + break; + case MSP_PIDNAMES: for (const char *c = pidnames; *c; c++) { sbufWriteU8(dst, *c); @@ -984,13 +1021,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_OSD_CONFIG: #ifdef USE_OSD - sbufWriteU8(dst, 1); // OSD supported + sbufWriteU8(dst, OSD_DRIVER_MAX7456); // OSD supported // send video system (AUTO/PAL/NTSC) -#ifdef USE_MAX7456 sbufWriteU8(dst, osdConfig()->video_system); -#else - sbufWriteU8(dst, 0); -#endif sbufWriteU8(dst, osdConfig()->units); sbufWriteU8(dst, osdConfig()->rssi_alarm); sbufWriteU16(dst, currentBatteryProfile->capacity.warning); @@ -1002,7 +1035,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, osdConfig()->item_pos[0][i]); } #else - sbufWriteU8(dst, 0); // OSD not supported + sbufWriteU8(dst, OSD_DRIVER_NONE); // OSD not supported #endif break; @@ -1029,7 +1062,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, gyroConfig()->gyro_align); sbufWriteU8(dst, accelerometerConfig()->acc_align); sbufWriteU8(dst, compassConfig()->mag_align); -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPFLOW sbufWriteU8(dst, opticalFlowConfig()->opflow_align); #else sbufWriteU8(dst, 0); @@ -1145,7 +1178,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #else sbufWriteU8(dst, 0); #endif -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPFLOW sbufWriteU8(dst, opticalFlowConfig()->opflow_hardware); #else sbufWriteU8(dst, 0); @@ -1219,6 +1252,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, 0); sbufWriteU16(dst, 0); #endif + + #ifdef USE_OPFLOW + sbufWriteU16(dst, opticalFlowConfig()->opflow_scale * 256); + #else + sbufWriteU16(dst, 0); + #endif break; case MSP_POSITION_ESTIMATION_CONFIG: @@ -1356,6 +1395,10 @@ 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()->gforce_alarm * 1000); + sbufWriteU16(dst, (int16_t)(osdConfig()->gforce_axis_alarm_min * 1000)); + sbufWriteU16(dst, (int16_t)(osdConfig()->gforce_axis_alarm_max * 1000)); + sbufWriteU8(dst, osdConfig()->current_alarm); sbufWriteU16(dst, osdConfig()->imu_temp_alarm_min); sbufWriteU16(dst, osdConfig()->imu_temp_alarm_max); #ifdef USE_BARO @@ -1405,12 +1448,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF 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) + for (uint8_t addrIndex = 0; 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); + sbufWriteU8(dst, sensorConfig->osdSymbol); + for (uint8_t labelIndex = 0; labelIndex < TEMPERATURE_LABEL_LEN; ++labelIndex) + sbufWriteU8(dst, sensorConfig->label[labelIndex]); } break; #endif @@ -1513,7 +1557,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_ARMING_CONFIG: if (dataSize >= 2) { - armingConfigMutable()->auto_disarm_delay = constrain(sbufReadU8(src), AUTO_DISARM_DELAY_MIN, AUTO_DISARM_DELAY_MAX); + sbufReadU8(src); //Swallow the first byte, used to be auto_disarm_delay armingConfigMutable()->disarm_kill_switch = !!sbufReadU8(src); } else return MSP_RESULT_ERROR; @@ -1545,6 +1589,22 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; break; + case MSP2_SET_PID: + if (dataSize >= PID_ITEM_COUNT * 4) { + for (int i = 0; i < PID_ITEM_COUNT; i++) { + pidBankMutable()->pid[i].P = sbufReadU8(src); + pidBankMutable()->pid[i].I = sbufReadU8(src); + pidBankMutable()->pid[i].D = sbufReadU8(src); + pidBankMutable()->pid[i].FF = sbufReadU8(src); + } + schedulePidGainsUpdate(); +#if defined(USE_NAV) + navigationUsePIDs(); +#endif + } else + return MSP_RESULT_ERROR; + break; + case MSP_SET_MODE_RANGE: sbufReadU8Safe(&tmp_u8, src); if ((dataSize >= 5) && (tmp_u8 < MAX_MODE_ACTIVATION_CONDITION_COUNT)) { @@ -1824,13 +1884,44 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; break; + case MSP2_INAV_SET_SERVO_MIXER: + sbufReadU8Safe(&tmp_u8, src); + if ((dataSize == 7) && (tmp_u8 < MAX_SERVO_RULES)) { + customServoMixersMutable(tmp_u8)->targetChannel = sbufReadU8(src); + customServoMixersMutable(tmp_u8)->inputSource = sbufReadU8(src); + customServoMixersMutable(tmp_u8)->rate = sbufReadU16(src); + customServoMixersMutable(tmp_u8)->speed = sbufReadU8(src); + #ifdef USE_LOGIC_CONDITIONS + customServoMixersMutable(tmp_u8)->conditionId = sbufReadU8(src); + #else + sbufReadU8(src); + #endif + loadCustomServoMixer(); + } else + return MSP_RESULT_ERROR; + break; +#ifdef USE_LOGIC_CONDITIONS + case MSP2_INAV_SET_LOGIC_CONDITIONS: + sbufReadU8Safe(&tmp_u8, src); + if ((dataSize == 14) && (tmp_u8 < MAX_LOGIC_CONDITIONS)) { + logicConditionsMutable(tmp_u8)->enabled = sbufReadU8(src); + logicConditionsMutable(tmp_u8)->operation = sbufReadU8(src); + logicConditionsMutable(tmp_u8)->operandA.type = sbufReadU8(src); + logicConditionsMutable(tmp_u8)->operandA.value = sbufReadU32(src); + logicConditionsMutable(tmp_u8)->operandB.type = sbufReadU8(src); + logicConditionsMutable(tmp_u8)->operandB.value = sbufReadU32(src); + logicConditionsMutable(tmp_u8)->flags = sbufReadU8(src); + } else + return MSP_RESULT_ERROR; + break; +#endif case MSP2_COMMON_SET_MOTOR_MIXER: sbufReadU8Safe(&tmp_u8, src); if ((dataSize == 9) && (tmp_u8 < MAX_SUPPORTED_MOTORS)) { - customMotorMixerMutable(tmp_u8)->throttle = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 1.0f); - customMotorMixerMutable(tmp_u8)->roll = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f; - customMotorMixerMutable(tmp_u8)->pitch = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f; - customMotorMixerMutable(tmp_u8)->yaw = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f; + primaryMotorMixerMutable(tmp_u8)->throttle = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 1.0f); + primaryMotorMixerMutable(tmp_u8)->roll = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f; + primaryMotorMixerMutable(tmp_u8)->pitch = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f; + primaryMotorMixerMutable(tmp_u8)->yaw = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f; } else return MSP_RESULT_ERROR; break; @@ -1867,7 +1958,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #else sbufReadU8(src); #endif -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPFLOW opticalFlowConfigMutable()->opflow_align = sbufReadU8(src); #else sbufReadU8(src); @@ -1999,7 +2090,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #else sbufReadU8(src); // rangefinder hardware #endif -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPFLOW opticalFlowConfigMutable()->opflow_hardware = sbufReadU8(src); #else sbufReadU8(src); // optical flow hardware @@ -2082,6 +2173,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU16(src); sbufReadU16(src); sbufReadU16(src); +#endif +#ifdef USE_OPFLOW + if (dataSize >= 20) { + opticalFlowConfigMutable()->opflow_scale = sbufReadU16(src) / 256.0f; + } #endif } else return MSP_RESULT_ERROR; @@ -2124,6 +2220,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; break; +#ifdef USE_OPFLOW + case MSP2_INAV_OPFLOW_CALIBRATION: + if (!ARMING_FLAG(ARMED)) + opflowStartCalibration(); + else + return MSP_RESULT_ERROR; + break; +#endif + case MSP_EEPROM_WRITE: if (!ARMING_FLAG(ARMED)) { writeEEPROM(); @@ -2150,11 +2255,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) // set all the other settings if ((int8_t)tmp_u8 == -1) { if (dataSize >= 10) { -#ifdef USE_MAX7456 osdConfigMutable()->video_system = sbufReadU8(src); -#else - sbufReadU8(src); // Skip video system -#endif osdConfigMutable()->units = sbufReadU8(src); osdConfigMutable()->rssi_alarm = sbufReadU8(src); currentBatteryProfileMutable->capacity.warning = sbufReadU16(src); @@ -2180,23 +2281,39 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP_OSD_CHAR_WRITE: -#ifdef USE_MAX7456 if (dataSize >= 55) { - max7456Character_t chr; + osdCharacter_t chr; + size_t osdCharacterBytes; uint16_t addr; - if (dataSize >= 56) { - addr = sbufReadU16(src); + if (dataSize >= OSD_CHAR_VISIBLE_BYTES + 2) { + if (dataSize >= OSD_CHAR_BYTES + 2) { + // 16 bit address, full char with metadata + addr = sbufReadU16(src); + osdCharacterBytes = OSD_CHAR_BYTES; + } else if (dataSize >= OSD_CHAR_BYTES + 1) { + // 8 bit address, full char with metadata + addr = sbufReadU8(src); + osdCharacterBytes = OSD_CHAR_BYTES; + } else { + // 16 bit character address, only visible char bytes + addr = sbufReadU16(src); + osdCharacterBytes = OSD_CHAR_VISIBLE_BYTES; + } } else { + // 8 bit character address, only visible char bytes addr = sbufReadU8(src); + osdCharacterBytes = OSD_CHAR_VISIBLE_BYTES; } - for (unsigned ii = 0; ii < sizeof(chr.data); ii++) { + for (unsigned ii = 0; ii < MIN(osdCharacterBytes, sizeof(chr.data)); ii++) { chr.data[ii] = sbufReadU8(src); } - // !!TODO - replace this with a device independent implementation - max7456WriteNvm(addr, &chr); - } else + displayPort_t *osdDisplayPort = osdGetDisplayPort(); + if (osdDisplayPort) { + displayWriteFontCharacter(osdDisplayPort, addr, &chr); + } + } else { return MSP_RESULT_ERROR; -#endif // USE_MAX7456 + } break; #endif // USE_OSD @@ -2291,6 +2408,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } else return MSP_RESULT_ERROR; break; + case MSP2_COMMON_SET_RADAR_POS: + if (dataSize >= 19) { + const uint8_t msp_radar_no = MIN(sbufReadU8(src), RADAR_MAX_POIS - 1); // Radar poi number, 0 to 3 + radar_pois[msp_radar_no].state = sbufReadU8(src); // 0=undefined, 1=armed, 2=lost + radar_pois[msp_radar_no].gps.lat = sbufReadU32(src); // lat 10E7 + radar_pois[msp_radar_no].gps.lon = sbufReadU32(src); // lon 10E7 + radar_pois[msp_radar_no].gps.alt = sbufReadU32(src); // altitude (cm) + radar_pois[msp_radar_no].heading = sbufReadU16(src); // ° + radar_pois[msp_radar_no].speed = sbufReadU16(src); // cm/s + radar_pois[msp_radar_no].lq = sbufReadU8(src); // Link quality, from 0 to 4 + } else + return MSP_RESULT_ERROR; + break; #endif case MSP_SET_FEATURE: @@ -2610,12 +2740,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_INAV_OSD_SET_ALARMS: { - if (dataSize >= 17) { + if (dataSize == 24) { 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); + tmp_u16 = sbufReadU16(src); + osdConfigMutable()->gforce_alarm = tmp_u16 / 1000.0f; + tmp_u16 = sbufReadU16(src); + osdConfigMutable()->gforce_axis_alarm_min = (int16_t)tmp_u16 / 1000.0f; + tmp_u16 = sbufReadU16(src); + osdConfigMutable()->gforce_axis_alarm_max = (int16_t)tmp_u16 / 1000.0f; + osdConfigMutable()->current_alarm = sbufReadU8(src); osdConfigMutable()->imu_temp_alarm_min = sbufReadU16(src); osdConfigMutable()->imu_temp_alarm_max = sbufReadU16(src); #ifdef USE_BARO @@ -2677,12 +2814,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) 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) + for (uint8_t addrIndex = 0; addrIndex < 8; ++addrIndex) ((uint8_t *)&sensorConfig->address)[addrIndex] = sbufReadU8(src); - for (uint8_t labelIndex; labelIndex < 4; ++labelIndex) - sensorConfig->label[labelIndex] = toupper(sbufReadU8(src)); sensorConfig->alarm_min = sbufReadU16(src); sensorConfig->alarm_max = sbufReadU16(src); + tmp_u8 = sbufReadU8(src); + sensorConfig->osdSymbol = tmp_u8 > TEMP_SENSOR_SYM_COUNT ? 0 : tmp_u8; + for (uint8_t labelIndex = 0; labelIndex < TEMPERATURE_LABEL_LEN; ++labelIndex) + sensorConfig->label[labelIndex] = toupper(sbufReadU8(src)); } } else return MSP_RESULT_ERROR; diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index b8ff879a4c9..03e7bd29eb0 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -85,6 +85,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXUSER1, "USER1", 47 }, { BOXUSER2, "USER2", 48 }, { BOXLOITERDIRCHN, "LOITER CHANGE", 49 }, + { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -292,6 +293,10 @@ void initActiveBoxIds(void) #endif #endif #endif + +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + activeBoxIds[activeBoxIdCount++] = BOXMSPRCOVERRIDE; +#endif } #define IS_ENABLED(mask) (mask == 0 ? 0 : 1) @@ -305,49 +310,52 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES // Requires new Multiwii protocol version to fix // It would be preferable to setting the enabled bits based on BOXINDEX. - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)), BOXANGLE); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)), BOXHORIZON); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADING_MODE)), BOXHEADINGHOLD); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)), BOXHEADFREE); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)), BOXHEADADJ); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)), BOXCAMSTAB); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)), BOXFPVANGLEMIX); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(MANUAL_MODE)), BOXMANUAL); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)), BOXBEEPERON); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)), BOXLEDLOW); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLIGHTS)), BOXLIGHTS); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)), BOXOSD); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)), BOXTELEMETRY); - CHECK_ACTIVE_BOX(IS_ENABLED(ARMING_FLAG(ARMED)), BOXARM); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)), BOXBLACKBOX); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)), BOXFAILSAFE); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVALTHOLD); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)), BOXNAVPOSHOLD); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_CRUISE_MODE)), BOXNAVCRUISE); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)), BOXNAVRTH); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)), BOXGCSNAV); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)), BOXANGLE); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)), BOXHORIZON); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADING_MODE)), BOXHEADINGHOLD); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)), BOXHEADFREE); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)), BOXHEADADJ); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)), BOXCAMSTAB); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)), BOXFPVANGLEMIX); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(MANUAL_MODE)), BOXMANUAL); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)), BOXBEEPERON); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)), BOXLEDLOW); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLIGHTS)), BOXLIGHTS); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)), BOXOSD); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)), BOXTELEMETRY); + CHECK_ACTIVE_BOX(IS_ENABLED(ARMING_FLAG(ARMED)), BOXARM); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)), BOXBLACKBOX); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)), BOXFAILSAFE); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVALTHOLD); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)), BOXNAVPOSHOLD); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_CRUISE_MODE)), BOXNAVCRUISE); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)), BOXNAVRTH); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)), BOXGCSNAV); #ifdef USE_FLM_FLAPERON - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FLAPERON)), BOXFLAPERON); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FLAPERON)), BOXFLAPERON); #endif - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)), BOXTURNASSIST); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_LAUNCH_MODE)), BOXNAVLAUNCH); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(AUTO_TUNE)), BOXAUTOTUNE); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTRIM)), BOXAUTOTRIM); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXKILLSWITCH)), BOXKILLSWITCH); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)), BOXHOMERESET); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA1)), BOXCAMERA1); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA2)), BOXCAMERA2); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA3)), BOXCAMERA3); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT1)), BOXOSDALT1); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT2)), BOXOSDALT2); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT3)), BOXOSDALT3); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)), BOXTURNASSIST); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_LAUNCH_MODE)), BOXNAVLAUNCH); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(AUTO_TUNE)), BOXAUTOTUNE); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTRIM)), BOXAUTOTRIM); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXKILLSWITCH)), BOXKILLSWITCH); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)), BOXHOMERESET); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA1)), BOXCAMERA1); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA2)), BOXCAMERA2); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMERA3)), BOXCAMERA3); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT1)), BOXOSDALT1); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT2)), BOXOSDALT2); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT3)), BOXOSDALT3); CHECK_ACTIVE_BOX(IS_ENABLED(navigationTerrainFollowingEnabled()), BOXSURFACE); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBRAKING)), BOXBRAKING); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER1)), BOXUSER1); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER2)), BOXUSER2); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)),BOXLOITERDIRCHN); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBRAKING)), BOXBRAKING); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER1)), BOXUSER1); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER2)), BOXUSER2); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)), BOXLOITERDIRCHN); +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)), BOXMSPRCOVERRIDE); +#endif memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index f8bf2827f7a..a71bf2692ff 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -26,6 +26,7 @@ #include "common/axis.h" #include "common/color.h" #include "common/utils.h" +#include "common/logic_condition.h" #include "drivers/accgyro/accgyro.h" #include "drivers/compass/compass.h" @@ -152,8 +153,6 @@ void taskUpdateCompass(timeUs_t currentTimeUs) #ifdef USE_BARO void taskUpdateBaro(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); - if (!sensors(SENSOR_BARO)) { return; } @@ -202,7 +201,7 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs) } #endif -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPFLOW void taskUpdateOpticalFlow(timeUs_t currentTimeUs) { if (!sensors(SENSOR_OPFLOW)) @@ -323,7 +322,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD)); #endif #endif -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPFLOW setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW)); #endif #ifdef USE_VTX_CONTROL @@ -337,6 +336,9 @@ void fcTasksInit(void) #ifdef USE_RCDEVICE setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled()); #endif +#ifdef USE_LOGIC_CONDITIONS + setTaskEnabled(TASK_LOGIC_CONDITIONS, true); +#endif } cfTask_t cfTasks[TASK_COUNT] = { @@ -507,7 +509,7 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPFLOW [TASK_OPFLOW] = { .taskName = "OPFLOW", .taskFunc = taskUpdateOpticalFlow, @@ -542,4 +544,12 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_IDLE, }, #endif +#ifdef USE_LOGIC_CONDITIONS + [TASK_LOGIC_CONDITIONS] = { + .taskName = "LOGIC", + .taskFunc = logicConditionUpdateTask, + .desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz @100msec + .staticPriority = TASK_PRIORITY_IDLE, + }, +#endif }; diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 3f18bfacc5a..bf07c3d6f34 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -33,6 +33,7 @@ #include "config/parameter_group_ids.h" #include "drivers/time.h" +#include "drivers/vtx_common.h" #include "fc/config.h" #include "fc/controlrate_profile.h" @@ -45,6 +46,7 @@ #include "flight/pid.h" #include "io/beeper.h" +#include "io/vtx.h" #include "sensors/boardalignment.h" @@ -262,6 +264,10 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 5 }} + }, { + .adjustmentFunction = ADJUSTMENT_VTX_POWER_LEVEL, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT }, { .adjustmentFunction = ADJUSTMENT_PROFILE, @@ -542,6 +548,14 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, 0, FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX); break; + case ADJUSTMENT_VTX_POWER_LEVEL: + { + vtxDeviceCapability_t vtxDeviceCapability; + if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + applyAdjustmentU8(ADJUSTMENT_VTX_POWER_LEVEL, &vtxSettingsConfigMutable()->power, delta, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); + } + } + break; default: break; }; @@ -601,9 +615,9 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, bool canUseRxD if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) { int delta; - if (rcData[channelIndex] > PWM_RANGE_MIDDLE + 200) { + if (rxGetChannelValue(channelIndex) > PWM_RANGE_MIDDLE + 200) { delta = adjustmentState->config->data.stepConfig.step; - } else if (rcData[channelIndex] < PWM_RANGE_MIDDLE - 200) { + } else if (rxGetChannelValue(channelIndex) < PWM_RANGE_MIDDLE - 200) { delta = 0 - adjustmentState->config->data.stepConfig.step; } else { // returning the switch to the middle immediately resets the ready state @@ -620,7 +634,7 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, bool canUseRxD #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) { const uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); - const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; + const uint8_t position = (constrain(rxGetChannelValue(channelIndex), 900, 2100 - 1) - 900) / rangeWidth; applySelectAdjustment(adjustmentFunction, position); #endif diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index b6782e273ab..747f75a2816 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -74,8 +74,9 @@ typedef enum { ADJUSTMENT_VEL_Z_I = 46, ADJUSTMENT_VEL_Z_D = 47, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 48, + ADJUSTMENT_VTX_POWER_LEVEL = 49, #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT - ADJUSTMENT_PROFILE = 49, + ADJUSTMENT_PROFILE = 50, #endif ADJUSTMENT_FUNCTION_COUNT // must be last } adjustmentFunction_e; diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index be595437492..6cc3e129d8b 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -67,24 +67,25 @@ stickPositions_e rcStickPositions; -int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW +FASTRAM int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW -PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 1); PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, .deadband = 5, .yaw_deadband = 5, .pos_hold_deadband = 20, .alt_hold_deadband = 50, - .deadband3d_throttle = 50 + .deadband3d_throttle = 50, + .airmodeHandlingType = STICK_CENTER, + .airmodeThrottleThreshold = AIRMODE_THROTTLE_THRESHOLD, ); -PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 2); PG_RESET_TEMPLATE(armingConfig_t, armingConfig, .fixed_wing_auto_arm = 0, .disarm_kill_switch = 1, - .auto_disarm_delay = 5, .switchDisarmDelayMs = DEFAULT_RC_SWITCH_DISARM_DELAY_MS, ); @@ -101,9 +102,9 @@ bool areSticksDeflectedMoreThanPosHoldDeadband(void) throttleStatus_e calculateThrottleStatus(void) { const uint16_t deadband3d_throttle = rcControlsConfig()->deadband3d_throttle; - if (feature(FEATURE_3D) && (rcData[THROTTLE] > (PWM_RANGE_MIDDLE - deadband3d_throttle) && rcData[THROTTLE] < (PWM_RANGE_MIDDLE + deadband3d_throttle))) + if (feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) > (PWM_RANGE_MIDDLE - deadband3d_throttle) && rxGetChannelValue(THROTTLE) < (PWM_RANGE_MIDDLE + deadband3d_throttle))) return THROTTLE_LOW; - else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->mincheck)) + else if (!feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck)) return THROTTLE_LOW; return THROTTLE_HIGH; @@ -111,8 +112,8 @@ throttleStatus_e calculateThrottleStatus(void) rollPitchStatus_e calculateRollPitchCenterStatus(void) { - if (((rcData[PITCH] < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rcData[PITCH] > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND))) - && ((rcData[ROLL] < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rcData[ROLL] > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND)))) + if (((rxGetChannelValue(PITCH) < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rxGetChannelValue(PITCH) > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND))) + && ((rxGetChannelValue(ROLL) < (PWM_RANGE_MIDDLE + AIRMODE_DEADBAND)) && (rxGetChannelValue(ROLL) > (PWM_RANGE_MIDDLE -AIRMODE_DEADBAND)))) return CENTERED; return NOT_CENTERED; @@ -139,17 +140,17 @@ static void updateRcStickPositions(void) { stickPositions_e tmp = 0; - tmp |= ((rcData[ROLL] > rxConfig()->mincheck) ? 0x02 : 0x00) << (ROLL * 2); - tmp |= ((rcData[ROLL] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (ROLL * 2); + tmp |= ((rxGetChannelValue(ROLL) > rxConfig()->mincheck) ? 0x02 : 0x00) << (ROLL * 2); + tmp |= ((rxGetChannelValue(ROLL) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (ROLL * 2); - tmp |= ((rcData[PITCH] > rxConfig()->mincheck) ? 0x02 : 0x00) << (PITCH * 2); - tmp |= ((rcData[PITCH] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (PITCH * 2); + tmp |= ((rxGetChannelValue(PITCH) > rxConfig()->mincheck) ? 0x02 : 0x00) << (PITCH * 2); + tmp |= ((rxGetChannelValue(PITCH) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (PITCH * 2); - tmp |= ((rcData[YAW] > rxConfig()->mincheck) ? 0x02 : 0x00) << (YAW * 2); - tmp |= ((rcData[YAW] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (YAW * 2); + tmp |= ((rxGetChannelValue(YAW) > rxConfig()->mincheck) ? 0x02 : 0x00) << (YAW * 2); + tmp |= ((rxGetChannelValue(YAW) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (YAW * 2); - tmp |= ((rcData[THROTTLE] > rxConfig()->mincheck) ? 0x02 : 0x00) << (THROTTLE * 2); - tmp |= ((rcData[THROTTLE] < rxConfig()->maxcheck) ? 0x01 : 0x00) << (THROTTLE * 2); + tmp |= ((rxGetChannelValue(THROTTLE) > rxConfig()->mincheck) ? 0x02 : 0x00) << (THROTTLE * 2); + tmp |= ((rxGetChannelValue(THROTTLE) < rxConfig()->maxcheck) ? 0x01 : 0x00) << (THROTTLE * 2); rcStickPositions = tmp; } @@ -178,25 +179,25 @@ void processRcStickPositions(throttleStatus_e throttleStatus) rcSticks = stTmp; // perform actions - if (!isUsingSticksForArming()) { - if (IS_RC_MODE_ACTIVE(BOXARM)) { - rcDisarmTimeMs = currentTimeMs; - tryArm(); - } else { - // Disarming via ARM BOX - // Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver - // and can't afford to risk disarming in the air - if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) { - const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs; - if (disarmDelay > armingConfig()->switchDisarmDelayMs) { - if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) { - disarm(DISARM_SWITCH); - } + bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM); + emergencyArmingUpdate(armingSwitchIsActive); + if (armingSwitchIsActive) { + rcDisarmTimeMs = currentTimeMs; + tryArm(); + } else { + // Disarming via ARM BOX + // Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver + // and can't afford to risk disarming in the air + if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) { + const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs; + if (disarmDelay > armingConfig()->switchDisarmDelayMs) { + if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) { + disarm(DISARM_SWITCH); } } - else { - rcDisarmTimeMs = currentTimeMs; - } + } + else { + rcDisarmTimeMs = currentTimeMs; } } @@ -209,23 +210,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus) return; } - if (isUsingSticksForArming()) { - // Disarm on throttle down + yaw - if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { - // Dont disarm if fixedwing and motorstop - if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) { - return; - } - else if (ARMING_FLAG(ARMED)) { - disarm(DISARM_STICKS); - } - else { - beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held - rcDelayCommand = 0; // reset so disarm tone will repeat - } - } - } - if (ARMING_FLAG(ARMED)) { // actions during armed return; @@ -295,40 +279,18 @@ void processRcStickPositions(throttleStatus_e throttleStatus) saveConfigAndNotify(); } - - // Arming by sticks - if (isUsingSticksForArming()) { - if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) { - // Auto arm on throttle when using fixedwing and motorstop - if (throttleStatus != THROTTLE_LOW) { - tryArm(); - return; - } - } - else { - if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { - // Arm via YAW - tryArm(); - return; - } - } - } - - // Calibrating Acc if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { accStartCalibration(); return; } - // Calibrating Mag if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) { ENABLE_STATE(CALIBRATE_MAG); return; } - // Accelerometer Trim if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) { applyAndSaveBoardAlignmentDelta(0, -2); @@ -350,5 +312,5 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } int32_t getRcStickDeflection(int32_t axis) { - return MIN(ABS(rcData[axis] - PWM_RANGE_MIDDLE), 500); + return MIN(ABS(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE), 500); } diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 51d49884e02..3a372a0ffb9 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -19,8 +19,7 @@ #include "config/parameter_group.h" -#define AUTO_DISARM_DELAY_MIN 0 -#define AUTO_DISARM_DELAY_MAX 60 +#define AIRMODE_THROTTLE_THRESHOLD 1300 typedef enum rc_alias { ROLL = 0, @@ -51,6 +50,11 @@ typedef enum { CENTERED } rollPitchStatus_e; +typedef enum { + STICK_CENTER = 0, + THROTTLE_THRESHOLD +} airmodeAndAntiWindupHandlingType_e; + typedef enum { ROL_LO = (1 << (2 * ROLL)), ROL_CE = (3 << (2 * ROLL)), @@ -77,6 +81,8 @@ typedef struct rcControlsConfig_s { uint8_t pos_hold_deadband; // Adds ability to adjust the Hold-position when moving the sticks (assisted mode) uint8_t alt_hold_deadband; // Defines the neutral zone of throttle stick during altitude hold uint16_t deadband3d_throttle; // default throttle deadband from MIDRC + uint8_t airmodeHandlingType; // Defaults to ANTI_WINDUP triggered at sticks centered + uint16_t airmodeThrottleThreshold; // Throttle threshold for airmode initial activation } rcControlsConfig_t; PG_DECLARE(rcControlsConfig_t, rcControlsConfig); @@ -84,7 +90,6 @@ PG_DECLARE(rcControlsConfig_t, rcControlsConfig); typedef struct armingConfig_s { uint8_t fixed_wing_auto_arm; // Auto-arm fixed wing aircraft on throttle up and never disarm uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value - uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0 uint16_t switchDisarmDelayMs; // additional delay between ARM box going off and actual disarm } armingConfig_t; diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index dfe2e5ec174..01f84af7f41 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -31,11 +31,11 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/runtime_config.h" #include "rx/rx.h" static uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT]; -static bool isUsingSticksToArm = true; #ifdef USE_NAV static bool isUsingNAVModes = false; #endif @@ -54,14 +54,62 @@ boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); PG_REGISTER(modeActivationOperatorConfig_t, modeActivationOperatorConfig, PG_MODE_ACTIVATION_OPERATOR_CONFIG, 0); -bool isUsingSticksForArming(void) -{ - return isUsingSticksToArm; +static void processAirmodeAirplane(void) { + if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + ENABLE_STATE(AIRMODE_ACTIVE); + } else { + DISABLE_STATE(AIRMODE_ACTIVE); + } } -bool isAirmodeActive(void) -{ - return feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE); +static void processAirmodeMultirotor(void) { + if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { + if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + ENABLE_STATE(AIRMODE_ACTIVE); + } else { + DISABLE_STATE(AIRMODE_ACTIVE); + } + } else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) { + + if (!ARMING_FLAG(ARMED)) { + /* + * Disarm disables airmode immediately + */ + DISABLE_STATE(AIRMODE_ACTIVE); + } else if ( + !STATE(AIRMODE_ACTIVE) && + rcCommand[THROTTLE] > rcControlsConfig()->airmodeThrottleThreshold && + (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) + ) { + /* + * Airmode is allowed to be active only after ARMED and then THROTTLE goes above + * activation threshold + */ + ENABLE_STATE(AIRMODE_ACTIVE); + } else if ( + STATE(AIRMODE_ACTIVE) && + !feature(FEATURE_AIRMODE) && + !IS_RC_MODE_ACTIVE(BOXAIRMODE) + ) { + /* + * When user disables BOXAIRMODE, turn airmode off as well + */ + DISABLE_STATE(AIRMODE_ACTIVE); + } + + } else { + DISABLE_STATE(AIRMODE_ACTIVE); + } +} + +void processAirmode(void) { + + if (STATE(FIXED_WING)) { + processAirmodeAirplane(); + } else { + processAirmodeMultirotor(); + } + } #if defined(USE_NAV) @@ -102,7 +150,7 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) // No need to constrain() here, since we're testing for a closed range defined // by the channelRange_t. If channelValue has an invalid value, the test will // be false anyway. - uint16_t channelValue = rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT]; + uint16_t channelValue = rxGetChannelValue(auxChannelIndex + NON_AUX_CHANNEL_COUNT); return (channelValue >= CHANNEL_RANGE_MIN + (range->startStep * CHANNEL_RANGE_STEP_WIDTH) && channelValue < CHANNEL_RANGE_MIN + (range->endStep * CHANNEL_RANGE_STEP_WIDTH)); } @@ -159,8 +207,6 @@ void updateUsedModeActivationConditionFlags(void) } } - isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM); - #ifdef USE_NAV isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) || isModeActivationConditionPresent(BOXNAVRTH) || diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 1a90da24577..2058c822d31 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -26,47 +26,48 @@ #define BOXID_NONE 255 typedef enum { - BOXARM = 0, - BOXANGLE = 1, - BOXHORIZON = 2, - BOXNAVALTHOLD = 3, // old BOXBARO - BOXHEADINGHOLD = 4, // old MAG - BOXHEADFREE = 5, - BOXHEADADJ = 6, - BOXCAMSTAB = 7, - BOXNAVRTH = 8, // old GPSHOME - BOXNAVPOSHOLD = 9, // old GPSHOLD - BOXMANUAL = 10, - BOXBEEPERON = 11, - BOXLEDLOW = 12, - BOXLIGHTS = 13, - BOXNAVLAUNCH = 14, - BOXOSD = 15, - BOXTELEMETRY = 16, - BOXBLACKBOX = 17, - BOXFAILSAFE = 18, - BOXNAVWP = 19, - BOXAIRMODE = 20, - BOXHOMERESET = 21, - BOXGCSNAV = 22, - BOXKILLSWITCH = 23, // old HEADING LOCK - BOXSURFACE = 24, - BOXFLAPERON = 25, - BOXTURNASSIST = 26, - BOXAUTOTRIM = 27, - BOXAUTOTUNE = 28, - BOXCAMERA1 = 29, - BOXCAMERA2 = 30, - BOXCAMERA3 = 31, - BOXOSDALT1 = 32, - BOXOSDALT2 = 33, - BOXOSDALT3 = 34, - BOXNAVCRUISE = 35, - BOXBRAKING = 36, - BOXUSER1 = 37, - BOXUSER2 = 38, - BOXFPVANGLEMIX = 39, - BOXLOITERDIRCHN = 40, + BOXARM = 0, + BOXANGLE = 1, + BOXHORIZON = 2, + BOXNAVALTHOLD = 3, // old BOXBARO + BOXHEADINGHOLD = 4, // old MAG + BOXHEADFREE = 5, + BOXHEADADJ = 6, + BOXCAMSTAB = 7, + BOXNAVRTH = 8, // old GPSHOME + BOXNAVPOSHOLD = 9, // old GPSHOLD + BOXMANUAL = 10, + BOXBEEPERON = 11, + BOXLEDLOW = 12, + BOXLIGHTS = 13, + BOXNAVLAUNCH = 14, + BOXOSD = 15, + BOXTELEMETRY = 16, + BOXBLACKBOX = 17, + BOXFAILSAFE = 18, + BOXNAVWP = 19, + BOXAIRMODE = 20, + BOXHOMERESET = 21, + BOXGCSNAV = 22, + BOXKILLSWITCH = 23, // old HEADING LOCK + BOXSURFACE = 24, + BOXFLAPERON = 25, + BOXTURNASSIST = 26, + BOXAUTOTRIM = 27, + BOXAUTOTUNE = 28, + BOXCAMERA1 = 29, + BOXCAMERA2 = 30, + BOXCAMERA3 = 31, + BOXOSDALT1 = 32, + BOXOSDALT2 = 33, + BOXOSDALT3 = 34, + BOXNAVCRUISE = 35, + BOXBRAKING = 36, + BOXUSER1 = 37, + BOXUSER2 = 38, + BOXFPVANGLEMIX = 39, + BOXLOITERDIRCHN = 40, + BOXMSPRCOVERRIDE = 41, CHECKBOX_ITEM_COUNT } boxId_e; @@ -120,8 +121,7 @@ void rcModeUpdate(boxBitmask_t *newState); bool isModeActivationConditionPresent(boxId_e modeId); -bool isUsingSticksForArming(void); -bool isAirmodeActive(void); +void processAirmode(void); bool isUsingNavigationModes(void); bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range); diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index cb1f4039e9e..8540e7b3175 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -24,18 +24,18 @@ #include "io/beeper.h" -uint32_t armingFlags = 0; -uint32_t stateFlags = 0; -uint32_t flightModeFlags = 0; +EXTENDED_FASTRAM uint32_t armingFlags = 0; +EXTENDED_FASTRAM uint32_t stateFlags = 0; +EXTENDED_FASTRAM uint32_t flightModeFlags = 0; -static uint32_t enabledSensors = 0; +static EXTENDED_FASTRAM uint32_t enabledSensors = 0; #if !defined(CLI_MINIMAL_VERBOSITY) const char *armingDisableFlagNames[]= { "FS", "ANGLE", "CAL", "OVRLD", "NAV", "COMPASS", "ACC", "ARMSW", "HWFAIL", "BOXFS", "KILLSW", "RX", "THR", "CLI", "CMS", "OSD", "ROLL/PITCH", "AUTOTRIM", "OOM", - "SETTINGFAIL", + "SETTINGFAIL", "PWMOUT" }; #endif @@ -80,7 +80,7 @@ uint32_t disableFlightMode(flightModeFlags_e mask) return flightModeFlags; } -bool sensors(uint32_t mask) +bool FAST_CODE NOINLINE sensors(uint32_t mask) { return enabledSensors & mask; } diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index b84fa7ec357..e62e22817bd 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -39,18 +39,35 @@ typedef enum { ARMING_DISABLED_CLI = (1 << 20), ARMING_DISABLED_CMS_MENU = (1 << 21), ARMING_DISABLED_OSD_MENU = (1 << 22), - ARMING_DISABLED_ROLLPITCH_NOT_CENTERED = (1 << 23), + ARMING_DISABLED_ROLLPITCH_NOT_CENTERED = (1 << 23), ARMING_DISABLED_SERVO_AUTOTRIM = (1 << 24), ARMING_DISABLED_OOM = (1 << 25), ARMING_DISABLED_INVALID_SETTING = (1 << 26), - - ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | - ARMING_DISABLED_NAVIGATION_UNSAFE | ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED | - ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE | ARMING_DISABLED_BOXKILLSWITCH | - ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI | ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | - ARMING_DISABLED_ROLLPITCH_NOT_CENTERED | ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING) + ARMING_DISABLED_PWM_OUTPUT_ERROR = (1 << 27), + + ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | + ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | + ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED | + ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE | + ARMING_DISABLED_BOXKILLSWITCH | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI | + ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED | + ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING | + ARMING_DISABLED_PWM_OUTPUT_ERROR), } armingFlag_e; +// Arming blockers that can be overriden by emergency arming. +// Keep in mind that this feature is intended to allow arming in +// situations where we might just need the motors to spin so the +// aircraft can move (even unpredictably) and get unstuck (e.g. +// crashed into a high tree). +#define ARMING_DISABLED_EMERGENCY_OVERRIDE (ARMING_DISABLED_NOT_LEVEL \ + | ARMING_DISABLED_NAVIGATION_UNSAFE \ + | ARMING_DISABLED_COMPASS_NOT_CALIBRATED \ + | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED \ + | ARMING_DISABLED_ARM_SWITCH \ + | ARMING_DISABLED_HARDWARE_FAILURE) + + extern uint32_t armingFlags; extern const char *armingDisableFlagNames[]; @@ -89,21 +106,22 @@ extern uint32_t flightModeFlags; #define FLIGHT_MODE(mask) (flightModeFlags & (mask)) typedef enum { - GPS_FIX_HOME = (1 << 0), - GPS_FIX = (1 << 1), - CALIBRATE_MAG = (1 << 2), - SMALL_ANGLE = (1 << 3), - FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code - ANTI_WINDUP = (1 << 5), - FLAPERON_AVAILABLE = (1 << 6), - NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available - COMPASS_CALIBRATED = (1 << 8), - ACCELEROMETER_CALIBRATED= (1 << 9), - PWM_DRIVER_AVAILABLE = (1 << 10), - HELICOPTER = (1 << 11), - NAV_CRUISE_BRAKING = (1 << 12), - NAV_CRUISE_BRAKING_BOOST = (1 << 13), - NAV_CRUISE_BRAKING_LOCKED = (1 << 14), + GPS_FIX_HOME = (1 << 0), + GPS_FIX = (1 << 1), + CALIBRATE_MAG = (1 << 2), + SMALL_ANGLE = (1 << 3), + FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code + ANTI_WINDUP = (1 << 5), + FLAPERON_AVAILABLE = (1 << 6), + NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available + COMPASS_CALIBRATED = (1 << 8), + ACCELEROMETER_CALIBRATED = (1 << 9), + PWM_DRIVER_AVAILABLE = (1 << 10), + NAV_CRUISE_BRAKING = (1 << 11), + NAV_CRUISE_BRAKING_BOOST = (1 << 12), + NAV_CRUISE_BRAKING_LOCKED = (1 << 13), + NAV_EXTRA_ARMING_SAFETY_BYPASSED = (1 << 14), // nav_extra_arming_safey was bypassed. Keep it until power cycle. + AIRMODE_ACTIVE = (1 << 15), } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index f426148aaa7..3205c331bf2 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -284,6 +284,18 @@ const char * settingLookupValueName(const setting_t *val, unsigned v) return NULL; } +size_t settingGetValueNameMaxSize(const setting_t *val) +{ + size_t maxSize = 0; + const lookupTableEntry_t *table = settingLookupTable(val); + if (table) { + for (unsigned ii = 0; ii < table->valueCount; ii++) { + maxSize = MAX(maxSize, strlen(table->values[ii])); + } + } + return maxSize; +} + const char * settingGetString(const setting_t *val) { if (SETTING_TYPE(val) == VAR_STRING) { diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index bbca4662c89..e73f7d65c12 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -112,6 +112,9 @@ const lookupTableEntry_t * settingLookupTable(const setting_t *val); // for the given setting. If the setting mode is not MODE_LOOKUP or // if the value is out of range, it returns NULL. const char * settingLookupValueName(const setting_t *val, unsigned v); +// Returns the length of the longest value name for the given setting, +// or 0 if the setting does not use value names. +size_t settingGetValueNameMaxSize(const setting_t *val); // Returns the setting value as a const char * iff the setting is of type // VAR_STRING. Otherwise it returns NULL. const char * settingGetString(const setting_t *val); @@ -125,4 +128,4 @@ setting_max_t settingGetStringMaxLength(const setting_t *val); // Retrieve the setting indexes for the given PG. If the PG is not // found, these function returns false. -bool settingsGetParameterGroupIndexes(pgn_t pg, uint16_t *start, uint16_t *end); \ No newline at end of file +bool settingsGetParameterGroupIndexes(pgn_t pg, uint16_t *start, uint16_t *end); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index bb718961cd4..12d6f9a367c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -32,7 +32,7 @@ tables: - name: blackbox_device values: ["SERIAL", "SPIFLASH", "SDCARD"] - name: motor_pwm_protocol - values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200"] + values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", "SERIALSHOT"] - name: failsafe_procedure values: ["SET-THR", "DROP", "RTH", "NONE"] - name: current_sensor @@ -54,7 +54,7 @@ tables: - name: nav_user_control_mode values: ["ATTI", "CRUISE"] - name: nav_rth_alt_mode - values: ["CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST"] + values: ["CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST", "AT_LEAST_LINEAR_DESCENT"] - name: osd_unit values: ["IMPERIAL", "METRIC", "UK"] enum: osd_unit_e @@ -76,15 +76,15 @@ tables: values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"] - name: debug_modes values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", - "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE", - "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC"] + "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE", + "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX", "D_BOOST"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e - name: osd_crosshairs_style - values: ["DEFAULT", "AIRCRAFT"] + values: ["DEFAULT", "AIRCRAFT", "TYPE3", "TYPE4", "TYPE5", "TYPE6", "TYPE7"] enum: osd_crosshairs_style_e - name: osd_sidebar_scroll values: ["NONE", "ALTITUDE", "GROUND_SPEED", "HOME_DISTANCE"] @@ -110,6 +110,21 @@ tables: - name: vtx_low_power_disarm values: ["OFF", "ON", "UNTIL_FIRST_ARM"] enum: vtxLowerPowerDisarm_e + - name: filter_type + values: ["PT1", "BIQUAD"] + - name: log_level + values: ["ERROR", "WARNING", "INFO", "VERBOSE", "DEBUG"] + - name: iterm_relax + values: ["OFF", "RP", "RPY"] + enum: itermRelax_e + - name: iterm_relax_type + values: ["GYRO", "SETPOINT"] + enum: itermRelaxType_e + - name: airmodeHandlingType + values: ["STICK_CENTER", "THROTTLE_THRESHOLD"] + - name: nav_extra_arming_safety + values: ["OFF", "ON", "ALLOW_BYPASS"] + enum: navExtraArmingSafety_e groups: - name: PG_GYRO_CONFIG @@ -131,6 +146,9 @@ groups: - name: gyro_lpf_hz field: gyro_soft_lpf_hz max: 200 + - name: gyro_lpf_type + field: gyro_soft_lpf_type + table: filter_type - name: moron_threshold field: gyroMovementCalibrationThreshold max: 128 @@ -203,7 +221,11 @@ groups: - name: acc_hardware table: acc_hardware - name: acc_lpf_hz + min: 0 max: 200 + - name: acc_lpf_type + field: acc_soft_lpf_type + table: filter_type - name: acczero_x field: accZero.raw[X] min: INT16_MIN @@ -243,7 +265,7 @@ groups: - name: PG_OPFLOW_CONFIG type: opticalFlowConfig_t headers: ["sensors/opflow.h"] - condition: USE_OPTICAL_FLOW + condition: USE_OPFLOW members: - name: opflow_hardware table: opflow_hardware @@ -394,6 +416,11 @@ groups: - name: serialrx_halfduplex field: halfDuplex type: bool + - name: msp_override_channels + field: mspOverrideChannels + condition: USE_MSP_RC_OVERRIDE + min: 0 + max: 65535 - name: PG_BLACKBOX_CONFIG type: blackboxConfig_t @@ -487,6 +514,8 @@ groups: max: 65000 - name: failsafe_min_distance_procedure table: failsafe_procedure + - name: failsafe_mission + type: bool - name: PG_LIGHTS_CONFIG type: lightsConfig_t @@ -780,9 +809,6 @@ groups: type: bool - name: disarm_kill_switch type: bool - - name: auto_disarm_delay - min: 0 - max: 60 - name: switch_disarm_delay field: switchDisarmDelayMs min: 0 @@ -838,6 +864,13 @@ groups: field: deadband3d_throttle min: 0 max: 200 + - name: mc_airmode_type + field: airmodeHandlingType + table: airmodeHandlingType + - name: mc_airmode_threshold + field: airmodeThrottleThreshold + min: 1000 + max: 2000 - name: PG_PID_PROFILE type: pidProfile_t @@ -901,7 +934,7 @@ groups: min: 0 max: 200 - name: fw_ff_pitch - field: bank_fw.pid[PID_PITCH].D + field: bank_fw.pid[PID_PITCH].FF min: 0 max: 200 - name: fw_p_roll @@ -913,7 +946,7 @@ groups: min: 0 max: 200 - name: fw_ff_roll - field: bank_fw.pid[PID_ROLL].D + field: bank_fw.pid[PID_ROLL].FF min: 0 max: 200 - name: fw_p_yaw @@ -925,7 +958,7 @@ groups: min: 0 max: 200 - name: fw_ff_yaw - field: bank_fw.pid[PID_YAW].D + field: bank_fw.pid[PID_YAW].FF min: 0 max: 200 - name: fw_p_level @@ -951,6 +984,9 @@ groups: - name: dterm_lpf_hz min: 0 max: 200 + - name: use_dterm_fir_filter + field: use_dterm_fir_filter + type: bool - name: yaw_lpf_hz min: 0 max: 200 @@ -1013,16 +1049,6 @@ groups: condition: USE_NAV min: 0 max: 255 - - name: nav_mc_pos_z_i - field: bank_mc.pid[PID_POS_Z].I - condition: USE_NAV - min: 0 - max: 255 - - name: nav_mc_pos_z_d - field: bank_mc.pid[PID_POS_Z].D - condition: USE_NAV - min: 0 - max: 255 - name: nav_mc_vel_z_p field: bank_mc.pid[PID_VEL_Z].P condition: USE_NAV @@ -1043,16 +1069,6 @@ groups: condition: USE_NAV min: 0 max: 255 - - name: nav_mc_pos_xy_i - field: bank_mc.pid[PID_POS_XY].I - condition: USE_NAV - min: 0 - max: 255 - - name: nav_mc_pos_xy_d - field: bank_mc.pid[PID_POS_XY].D - condition: USE_NAV - min: 0 - max: 255 - name: nav_mc_vel_xy_p field: bank_mc.pid[PID_VEL_XY].P condition: USE_NAV @@ -1068,11 +1084,20 @@ groups: condition: USE_NAV min: 0 max: 255 + - name: nav_mc_vel_xy_ff + field: bank_mc.pid[PID_VEL_XY].FF + condition: USE_NAV + min: 0 + max: 255 - name: nav_mc_heading_p field: bank_mc.pid[PID_HEADING].P condition: USE_NAV min: 0 max: 255 + - name: nav_mc_vel_xy_dterm_lpf_hz + field: navVelXyDTermLpfHz + min: 0 + max: 100 - name: nav_fw_pos_z_p field: bank_fw.pid[PID_POS_Z].P condition: USE_NAV @@ -1108,6 +1133,31 @@ groups: condition: USE_NAV min: 0 max: 255 + - name: mc_iterm_relax_type + field: iterm_relax_type + table: iterm_relax_type + - name: mc_iterm_relax + field: iterm_relax + table: iterm_relax + - name: mc_iterm_relax_cutoff + field: iterm_relax_cutoff + min: 1 + max: 100 + - name: d_boost_factor + field: dBoostFactor + condition: USE_D_BOOST + min: 1 + max: 3 + - name: d_boost_max_at_acceleration + field: dBoostMaxAtAlleceleration + condition: USE_D_BOOST + min: 1000 + max: 16000 + - name: d_boost_gyro_delta_lpf_hz + field: dBoostGyroDeltaLpfHz + condition: USE_D_BOOST + min: 10 + max: 250 - name: PG_PID_AUTOTUNE_CONFIG type: pidAutotuneConfig_t @@ -1140,7 +1190,6 @@ groups: members: - name: inav_auto_mag_decl field: automatic_mag_declination - condition: NAV_AUTO_MAG_DECLINATION type: bool - name: inav_gravity_cal_tolerance field: gravity_calibration_tolerance @@ -1165,19 +1214,19 @@ groups: - name: inav_w_z_surface_p field: w_z_surface_p min: 0 - max: 10 + max: 100 - name: inav_w_z_surface_v field: w_z_surface_v min: 0 - max: 10 + max: 100 - name: inav_w_xy_flow_p field: w_xy_flow_p min: 0 - max: 10 + max: 100 - name: inav_w_xy_flow_v field: w_xy_flow_v min: 0 - max: 10 + max: 100 - name: inav_w_z_baro_p field: w_z_baro_p min: 0 @@ -1206,6 +1255,10 @@ groups: field: w_xy_res_v min: 0 max: 10 + - name: inav_w_xyz_acc_p + field: w_xyz_acc_p + min: 0 + max: 1 - name: inav_w_acc_bias field: w_acc_bias min: 0 @@ -1232,7 +1285,7 @@ groups: type: bool - name: nav_extra_arming_safety field: general.flags.extra_arming_safety - type: bool + table: nav_extra_arming_safety - name: nav_user_control_mode field: general.flags.user_control_mode table: nav_user_control_mode @@ -1366,6 +1419,16 @@ groups: condition: USE_MR_BRAKING_MODE min: 15 max: 60 + - name: nav_mc_pos_deceleration_time + field: mc.posDecelerationTime + condition: USE_NAV + min: 0 + max: 255 + - name: nav_mc_pos_expo + field: mc.posResponseExpo + condition: USE_NAV + min: 0 + max: 255 - name: nav_fw_cruise_thr field: fw.cruise_throttle min: 1000 @@ -1466,7 +1529,7 @@ groups: - name: PG_TELEMETRY_CONFIG type: telemetryConfig_t - headers: ["io/serial.h", "telemetry/telemetry.h", "telemetry/frsky_d.h"] + headers: ["io/serial.h", "telemetry/telemetry.h", "telemetry/frsky_d.h", "telemetry/sim.h"] condition: USE_TELEMETRY members: - name: telemetry_switch @@ -1500,9 +1563,8 @@ groups: field: hottAlarmSoundInterval min: 0 max: 120 - - name: smartport_uart_unidir - field: smartportUartUnidirectional - condition: USE_TELEMETRY_SMARTPORT + - name: telemetry_uart_unidir + field: uartUnidirectional type: bool - name: smartport_fuel_unit field: smartportFuelUnit @@ -1517,7 +1579,71 @@ groups: field: ltmUpdateRate condition: USE_TELEMETRY_LTM table: ltm_rates - + - name: sim_ground_station_number + field: simGroundStationNumber + condition: USE_TELEMETRY_SIM + type: string + max: 16 + - name: sim_transmit_interval + field: simTransmitInterval + condition: USE_TELEMETRY_SIM + type: uint16_t + min: SIM_MIN_TRANSMIT_INTERVAL + max: 65535 + - name: sim_transmit_flags + field: simTransmitFlags + condition: USE_TELEMETRY_SIM + type: string + max: SIM_N_TX_FLAGS + - name: acc_event_threshold_high + field: accEventThresholdHigh + condition: USE_TELEMETRY_SIM + type: uint16_t + min: 0 + max: 65535 + - name: acc_event_threshold_low + field: accEventThresholdLow + condition: USE_TELEMETRY_SIM + type: uint16_t + min: 0 + max: 900 + - name: acc_event_threshold_neg_x + field: accEventThresholdNegX + condition: USE_TELEMETRY_SIM + type: uint16_t + min: 0 + max: 65535 + - name: sim_low_altitude + field: simLowAltitude + condition: USE_TELEMETRY_SIM + type: int16_t + min: INT16_MIN + max: INT16_MAX + - name: mavlink_ext_status_rate + field: mavlink.extended_status_rate + type: uint8_t + min: 0 + max: 255 + - name: mavlink_rc_chan_rate + field: mavlink.rc_channels_rate + type: uint8_t + min: 0 + max: 255 + - name: mavlink_pos_rate + field: mavlink.position_rate + type: uint8_t + min: 0 + max: 255 + - name: mavlink_extra1_rate + field: mavlink.extra1_rate + type: uint8_t + min: 0 + max: 255 + - name: mavlink_extra2_rate + field: mavlink.extra2_rate + type: uint8_t + min: 0 + max: 255 - name: PG_ELERES_CONFIG type: eleresConfig_t headers: ["rx/eleres.h"] @@ -1559,7 +1685,7 @@ groups: - name: PG_OSD_CONFIG type: osdConfig_t - headers: ["io/osd.h", "drivers/vcd.h"] + headers: ["io/osd.h", "drivers/osd.h"] condition: USE_OSD members: - name: osd_video_system @@ -1599,6 +1725,22 @@ groups: field: neg_alt_alarm min: 0 max: 10000 + - name: osd_current_alarm + field: current_alarm + min: 0 + max: 255 + - name: osd_gforce_alarm + field: gforce_alarm + min: 0 + max: 20 + - name: osd_gforce_axis_alarm_min + field: gforce_axis_alarm_min + min: -20 + max: 20 + - name: osd_gforce_axis_alarm_max + field: gforce_axis_alarm_max + min: -20 + max: 20 - name: osd_imu_temp_alarm_min field: imu_temp_alarm_min min: -550 @@ -1634,6 +1776,52 @@ groups: field: crosshairs_style table: osd_crosshairs_style type: uint8_t + - name: osd_horizon_offset + field: horizon_offset + min: -2 + max: 2 + - name: osd_camera_uptilt + field: camera_uptilt + min: -40 + max: 80 + - name: osd_camera_fov_h + field: camera_fov_h + min: 60 + max: 150 + - name: osd_camera_fov_v + field: camera_fov_v + min: 30 + max: 120 + - name: osd_hud_margin_h + field: hud_margin_h + min: 0 + max: 4 + - name: osd_hud_margin_v + field: hud_margin_v + min: 0 + max: 4 + - name: osd_hud_homing + field: hud_homing + type: bool + - name: osd_hud_homepoint + field: hud_homepoint + type: bool + - name: osd_hud_radar_disp + field: hud_radar_disp + min: 0 + max: 4 + - name: osd_hud_radar_range_min + field: hud_radar_range_min + min: 1 + max: 30 + - name: osd_hud_radar_range_max + field: hud_radar_range_max + min: 100 + max: 9990 + - name: osd_hud_radar_nearest + field: hud_radar_nearest + min: 0 + max: 4000 - name: osd_left_sidebar_scroll field: left_sidebar_scroll table: osd_sidebar_scroll @@ -1684,9 +1872,6 @@ groups: field: throttle_tilt_compensation_strength min: 0 max: 100 - - name: input_filtering_mode - field: pwmRxInputFilteringMode - type: bool - name: name - name: PG_MODE_ACTIVATION_OPERATOR_CONFIG @@ -1798,3 +1983,16 @@ groups: min: 0 max: 255 type: uint8_t + + - name: PG_LOG_CONFIG + type: logConfig_t + headers: ["common/log.h"] + condition: USE_LOG + members: + - name: log_level + field: level + table: log_level + - name: log_topics + field: topics + min: 0, + max: UINT32_MAX diff --git a/src/main/fc/stats.h b/src/main/fc/stats.h index 95bf8c2bca7..275460643c3 100644 --- a/src/main/fc/stats.h +++ b/src/main/fc/stats.h @@ -11,7 +11,7 @@ typedef struct statsConfig_s { uint8_t stats_enabled; } statsConfig_t; -uint32_t getFlyingEnergy(); +uint32_t getFlyingEnergy(void); void statsOnArm(void); void statsOnDisarm(void); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index fd45ddc6f81..2fcf1251c01 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -64,21 +64,22 @@ static failsafeState_t failsafeState; -PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 1); PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, - .failsafe_delay = 5, // 0.5 sec - .failsafe_recovery_delay = 5, // 0.5 seconds (plus 200ms explicit delay) - .failsafe_off_delay = 200, // 20sec - .failsafe_throttle = 1000, // default throttle off. - .failsafe_throttle_low_delay = 0, // default throttle low delay for "just disarm" on failsafe condition - .failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING, // default full failsafe procedure - .failsafe_fw_roll_angle = -200, // 20 deg left - .failsafe_fw_pitch_angle = 100, // 10 deg dive (yes, positive means dive) - .failsafe_fw_yaw_rate = -45, // 45 deg/s left yaw (left is negative, 8s for full turn) + .failsafe_delay = 5, // 0.5 sec + .failsafe_recovery_delay = 5, // 0.5 seconds (plus 200ms explicit delay) + .failsafe_off_delay = 200, // 20sec + .failsafe_throttle = 1000, // default throttle off. + .failsafe_throttle_low_delay = 0, // default throttle low delay for "just disarm" on failsafe condition + .failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING, // default full failsafe procedure + .failsafe_fw_roll_angle = -200, // 20 deg left + .failsafe_fw_pitch_angle = 100, // 10 deg dive (yes, positive means dive) + .failsafe_fw_yaw_rate = -45, // 45 deg/s left yaw (left is negative, 8s for full turn) .failsafe_stick_motion_threshold = 50, - .failsafe_min_distance = 0, // No minimum distance for failsafe by default - .failsafe_min_distance_procedure = FAILSAFE_PROCEDURE_DROP_IT // default minimum distance failsafe procedure + .failsafe_min_distance = 0, // No minimum distance for failsafe by default + .failsafe_min_distance_procedure = FAILSAFE_PROCEDURE_DROP_IT, // default minimum distance failsafe procedure + .failsafe_mission = true, // Enable failsafe in WP mode or not ); typedef enum { @@ -154,7 +155,7 @@ void failsafeReset(void) failsafeState.receivingRxDataPeriodPreset = 0; failsafeState.phase = FAILSAFE_IDLE; failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; - failsafeState.bypassNavigation = true; + failsafeState.activeProcedure = failsafeConfig()->failsafe_procedure; failsafeState.lastGoodRcCommand[ROLL] = 0; failsafeState.lastGoodRcCommand[PITCH] = 0; @@ -172,12 +173,15 @@ void failsafeInit(void) #ifdef USE_NAV bool failsafeBypassNavigation(void) { - return failsafeState.active && failsafeState.controlling && failsafeState.bypassNavigation; + return failsafeState.active && + failsafeState.controlling && + failsafeProcedureLogic[failsafeState.activeProcedure].bypassNavigation; } bool failsafeMayRequireNavigationMode(void) { - return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_RTH; + return (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_RTH) || + (failsafeConfig()->failsafe_min_distance_procedure == FAILSAFE_PROCEDURE_RTH); } #endif @@ -203,13 +207,15 @@ bool failsafeShouldApplyControlInput(void) bool failsafeRequiresAngleMode(void) { - return failsafeState.active && failsafeState.controlling && failsafeProcedureLogic[failsafeConfig()->failsafe_procedure].forceAngleMode; + return failsafeState.active && + failsafeState.controlling && + failsafeProcedureLogic[failsafeState.activeProcedure].forceAngleMode; } bool failsafeRequiresMotorStop(void) { return failsafeState.active && - failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_AUTO_LANDING && + failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING && failsafeConfig()->failsafe_throttle < motorConfig()->minthrottle; } @@ -223,12 +229,16 @@ static bool failsafeShouldHaveCausedLandingByNow(void) return failsafeConfig()->failsafe_off_delay && (millis() > failsafeState.landingShouldBeFinishedAt); } +static void failsafeSetActiveProcedure(failsafeProcedure_e procedure) +{ + failsafeState.activeProcedure = procedure; +} + static void failsafeActivate(failsafePhase_e newPhase) { failsafeState.active = true; failsafeState.controlling = true; failsafeState.phase = newPhase; - failsafeState.bypassNavigation = failsafeProcedureLogic[failsafeConfig()->failsafe_procedure].bypassNavigation; ENABLE_FLIGHT_MODE(FAILSAFE_MODE); failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND; @@ -263,7 +273,7 @@ void failsafeApplyControlInput(void) // Apply channel values for (int idx = 0; idx < 4; idx++) { - switch (failsafeProcedureLogic[failsafeConfig()->failsafe_procedure].channelBehavior[idx]) { + switch (failsafeProcedureLogic[failsafeState.activeProcedure].channelBehavior[idx]) { case FAILSAFE_CHANNEL_HOLD: rcCommand[idx] = failsafeState.lastGoodRcCommand[idx]; break; @@ -332,9 +342,9 @@ static bool failsafeCheckStickMotion(void) if (failsafeConfig()->failsafe_stick_motion_threshold > 0) { uint32_t totalRcDelta = 0; - totalRcDelta += ABS(rcData[ROLL] - PWM_RANGE_MIDDLE); - totalRcDelta += ABS(rcData[PITCH] - PWM_RANGE_MIDDLE); - totalRcDelta += ABS(rcData[YAW] - PWM_RANGE_MIDDLE); + totalRcDelta += ABS(rxGetChannelValue(ROLL) - PWM_RANGE_MIDDLE); + totalRcDelta += ABS(rxGetChannelValue(PITCH) - PWM_RANGE_MIDDLE); + totalRcDelta += ABS(rxGetChannelValue(YAW) - PWM_RANGE_MIDDLE); return totalRcDelta >= failsafeConfig()->failsafe_stick_motion_threshold; } @@ -343,6 +353,25 @@ static bool failsafeCheckStickMotion(void) } } +static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) +{ + if (FLIGHT_MODE(NAV_WP_MODE) && !failsafeConfig()->failsafe_mission) { + return FAILSAFE_PROCEDURE_NONE; + } + + // Craft is closer than minimum failsafe procedure distance (if set to non-zero) + // GPS must also be working, and home position set + if ((failsafeConfig()->failsafe_min_distance > 0) && + ((GPS_distanceToHome * 100) < failsafeConfig()->failsafe_min_distance) && + sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { + + // Use the alternate, minimum distance failsafe procedure instead + return failsafeConfig()->failsafe_min_distance_procedure; + } + + return failsafeConfig()->failsafe_procedure; +} + void failsafeUpdateState(void) { if (!failsafeIsMonitoring() || failsafeIsSuspended()) { @@ -375,6 +404,7 @@ void failsafeUpdateState(void) if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) { // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch // Don't disarm at all if `failsafe_throttle_low_delay` is set to zero + failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_DROP_IT); failsafeActivate(FAILSAFE_LANDED); // skip auto-landing procedure failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData } else { @@ -398,18 +428,10 @@ void failsafeUpdateState(void) if (receivingRxDataAndNotFailsafeMode) { failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; } else { - uint8_t failsafe_procedure_to_use = failsafeConfig()->failsafe_procedure; - - // Craft is closer than minimum failsafe procedure distance (if set to non-zero) - // GPS must also be working, and home position set - if ((failsafeConfig()->failsafe_min_distance > 0) && - ((GPS_distanceToHome * 100) < failsafeConfig()->failsafe_min_distance) && - sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { - // Use the alternate, minimum distance failsafe procedure instead - failsafe_procedure_to_use = failsafeConfig()->failsafe_min_distance_procedure; - } + // Set active failsafe procedure + failsafeSetActiveProcedure(failsafeChooseFailsafeProcedure()); - switch (failsafe_procedure_to_use) { + switch (failsafeState.activeProcedure) { case FAILSAFE_PROCEDURE_AUTO_LANDING: // Stabilize, and set Throttle to specified level failsafeActivate(FAILSAFE_LANDING); @@ -470,8 +492,8 @@ void failsafeUpdateState(void) default: // This shouldn't happen. If RTH was somehow aborted during failsafe - fallback to FAILSAFE_LANDING procedure abortForcedRTH(); + failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_AUTO_LANDING); failsafeActivate(FAILSAFE_LANDING); - failsafeState.bypassNavigation = true; // Force bypassing navigation reprocessState = true; break; } @@ -513,7 +535,7 @@ void failsafeUpdateState(void) if (receivingRxDataAndNotFailsafeMode) { if (millis() > failsafeState.receivingRxDataPeriod) { // rx link is good now, when arming via ARM switch, it must be OFF first - if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) { + if (!IS_RC_MODE_ACTIVE(BOXARM)) { // XXX: Requirements for removing the ARMING_DISABLED_FAILSAFE_SYSTEM flag // are tested by osd.c to show the user how to re-arm. If these // requirements change, update osdArmingDisabledReasonMessage(). diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 8de8a0062e6..042a52186fd 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -30,19 +30,20 @@ typedef struct failsafeConfig_s { - uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500. - uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure" (TENTH_SECOND) - uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10) - uint8_t failsafe_recovery_delay; // Time from RC link recovery to failsafe abort. 1 step = 0.1sec - 1sec in example (10) - uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200) - uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH) + uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500. + uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure" (TENTH_SECOND) + uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10) + uint8_t failsafe_recovery_delay; // Time from RC link recovery to failsafe abort. 1 step = 0.1sec - 1sec in example (10) + uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200) + uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH) - int16_t failsafe_fw_roll_angle; // Settings to be applies during "LAND" procedure on a fixed-wing + int16_t failsafe_fw_roll_angle; // Settings to be applies during "LAND" procedure on a fixed-wing int16_t failsafe_fw_pitch_angle; int16_t failsafe_fw_yaw_rate; uint16_t failsafe_stick_motion_threshold; - uint16_t failsafe_min_distance; // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default) - uint8_t failsafe_min_distance_procedure; // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH) + uint16_t failsafe_min_distance; // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default) + uint8_t failsafe_min_distance_procedure; // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH) + bool failsafe_mission; // Enable failsafe in WP mode or not } failsafeConfig_t; PG_DECLARE(failsafeConfig_t, failsafeConfig); @@ -134,7 +135,6 @@ typedef struct failsafeState_s { bool suspended; // Failsafe is temporary suspended. This happens when we temporary suspend RX system due to EEPROM write/read bool active; // Failsafe is active (on RC link loss) bool controlling; // Failsafe is driving the sticks instead of pilot - bool bypassNavigation; timeMs_t rxDataFailurePeriod; timeMs_t rxDataRecoveryPeriod; timeMs_t validRxDataReceivedAt; @@ -143,6 +143,7 @@ typedef struct failsafeState_s { timeMs_t landingShouldBeFinishedAt; timeMs_t receivingRxDataPeriod; // period for the required period of valid rxData timeMs_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData + failsafeProcedure_e activeProcedure; failsafePhase_e phase; failsafeRxLinkState_e rxLinkState; int16_t lastGoodRcCommand[4]; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 8f0bb9a7ebb..e99bdc601bf 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -30,6 +30,7 @@ #include "common/axis.h" #include "common/filter.h" +#include "common/log.h" #include "common/maths.h" #include "common/vector.h" #include "common/quaternion.h" @@ -265,13 +266,13 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c // Previous quaternion valid. Reset to it orientation = *quat; imuErrorEvent.errorCode = 1; - DEBUG_TRACE("AHRS orientation quaternion error. Reset to last known good value"); + LOG_E(IMU, "AHRS orientation quaternion error. Reset to last known good value"); } else { // No valid reference. Best guess from accelerometer imuResetOrientationQuaternion(accBF); imuErrorEvent.errorCode = 2; - DEBUG_TRACE("AHRS orientation quaternion error. Best guess from ACC"); + LOG_E(IMU, "AHRS orientation quaternion error. Best guess from ACC"); } #ifdef USE_BLACKBOX @@ -579,9 +580,10 @@ void imuCheckVibrationLevels(void) DEBUG_SET(DEBUG_VIBE, 1, accVibeLevels.y * 100); DEBUG_SET(DEBUG_VIBE, 2, accVibeLevels.z * 100); DEBUG_SET(DEBUG_VIBE, 3, accClipCount); + // DEBUG_VIBE values 4-7 are used by NAV estimator } -void imuUpdateAttitude(timeUs_t currentTimeUs) +void FAST_CODE NOINLINE imuUpdateAttitude(timeUs_t currentTimeUs) { /* Calculate dT */ static timeUs_t previousIMUUpdateTimeUs; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 5e9e484f4eb..79407b49377 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -53,14 +53,12 @@ #include "sensors/battery.h" - -//#define MIXER_DEBUG - -static uint8_t motorCount; - -int16_t motor[MAX_SUPPORTED_MOTORS]; -int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; +FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS]; +FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; static float motorMixRange; +static float mixerScale = 1.0f; +static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; +static EXTENDED_FASTRAM uint8_t motorCount = 0; PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0); @@ -104,12 +102,21 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .digitalIdleOffsetValue = 450 // Same scale as in Betaflight ); -static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; - -PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0); +PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); -uint8_t getMotorCount(void) +static void computeMotorCount(void) { + motorCount = 0; + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + // check if done + if (primaryMotorMixer(i)->throttle == 0.0f) { + break; + } + motorCount++; + } +} + +uint8_t FAST_CODE NOINLINE getMotorCount(void) { return motorCount; } @@ -128,13 +135,10 @@ void mixerUpdateStateFlags(void) // set flag that we're on something with wings if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { ENABLE_STATE(FIXED_WING); - DISABLE_STATE(HELICOPTER); } else if (mixerConfig()->platformType == PLATFORM_HELICOPTER) { DISABLE_STATE(FIXED_WING); - ENABLE_STATE(HELICOPTER); } else { DISABLE_STATE(FIXED_WING); - DISABLE_STATE(HELICOPTER); } if (mixerConfig()->hasFlaps) { @@ -144,28 +148,13 @@ void mixerUpdateStateFlags(void) } } -void mixerUsePWMIOConfiguration(void) +void mixerInit(void) { - motorCount = 0; - - // load custom mixer into currentMixer - for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - // check if done - if (customMotorMixer(i)->throttle == 0.0f) - break; - currentMixer[i] = *customMotorMixer(i); - motorCount++; - } - + computeMotorCount(); + loadPrimaryMotorMixer(); // in 3D mode, mixer gain has to be halved if (feature(FEATURE_3D)) { - if (motorCount > 1) { - for (int i = 0; i < motorCount; i++) { - currentMixer[i].pitch *= 0.5f; - currentMixer[i].roll *= 0.5f; - currentMixer[i].yaw *= 0.5f; - } - } + mixerScale = 0.5f; } mixerResetDisarmedMotors(); @@ -179,14 +168,14 @@ void mixerResetDisarmedMotors(void) } } -void writeMotors(void) +void FAST_CODE NOINLINE writeMotors(void) { for (int i = 0; i < motorCount; i++) { uint16_t motorValue; #ifdef USE_DSHOT // If we use DSHOT we need to convert motorValue to DSHOT ranges - if (isMotorProtocolDshot()) { + if (isMotorProtocolDigital()) { const float dshotMinThrottleOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 10000.0f * motorConfig()->digitalIdleOffsetValue; if (feature(FEATURE_3D)) { @@ -283,7 +272,7 @@ static void applyMotorRateLimiting(const float dT) } } -void mixTable(const float dT) +void FAST_CODE NOINLINE mixTable(const float dT) { int16_t input[3]; // RPY, range [-500:+500] // Allow direct stick input to motors in passthrough mode on airplanes @@ -312,9 +301,9 @@ void mixTable(const float dT) // motors for non-servo mixes for (int i = 0; i < motorCount; i++) { rpyMix[i] = - input[PITCH] * currentMixer[i].pitch + + (input[PITCH] * currentMixer[i].pitch + input[ROLL] * currentMixer[i].roll + - -mixerConfig()->yaw_motor_direction * input[YAW] * currentMixer[i].yaw; + -mixerConfig()->yaw_motor_direction * input[YAW] * currentMixer[i].yaw) * mixerScale; if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i]; if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i]; @@ -414,11 +403,17 @@ motorStatus_e getMotorStatus(void) return MOTOR_STOPPED_AUTO; } - if (rcData[THROTTLE] < rxConfig()->mincheck) { - if ((STATE(FIXED_WING) || !isAirmodeActive()) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) { + if (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck) { + if ((STATE(FIXED_WING) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) { return MOTOR_STOPPED_USER; } } return MOTOR_RUNNING; } + +void loadPrimaryMotorMixer(void) { + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + currentMixer[i] = *primaryMotorMixer(i); + } +} \ No newline at end of file diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 5d4df59e006..ea55a9ce386 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -60,7 +60,7 @@ typedef struct motorMixer_s { float yaw; } motorMixer_t; -PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer); +PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer); typedef struct mixerConfig_s { int8_t yaw_motor_direction; @@ -110,7 +110,7 @@ bool mixerIsOutputSaturated(void); motorStatus_e getMotorStatus(void); void writeAllMotors(int16_t mc); -void mixerUsePWMIOConfiguration(void); +void mixerInit(void); void mixerUpdateStateFlags(void); void mixerResetDisarmedMotors(void); void mixTable(const float dT); @@ -118,3 +118,5 @@ void writeMotors(void); void processServoAutotrim(void); void stopMotors(void); void stopPwmAllMotors(void); + +void loadPrimaryMotorMixer(void); \ No newline at end of file diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2be7449788f..3d91d0a815a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -27,6 +27,7 @@ #include "common/axis.h" #include "common/filter.h" #include "common/maths.h" +#include "common/utils.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -86,6 +87,13 @@ typedef struct { biquadFilter_t deltaNotchFilter; #endif float stickPosition; + +#ifdef USE_D_BOOST + float previousRateTarget; + float previousRateGyro; + pt1Filter_t dBoostLpf; + biquadFilter_t dBoostGyroLpf; +#endif } pidState_t; #ifdef USE_DTERM_NOTCH @@ -94,6 +102,7 @@ STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn; extern float dT; +STATIC_FASTRAM bool pidFiltersConfigured = false; FASTRAM float headingHoldCosZLimit; FASTRAM int16_t headingHoldTarget; STATIC_FASTRAM pt1Filter_t headingHoldRateFilter; @@ -109,63 +118,84 @@ 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, 6); +static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT]; +static EXTENDED_FASTRAM uint8_t itermRelax; +static EXTENDED_FASTRAM uint8_t itermRelaxType; +static EXTENDED_FASTRAM float itermRelaxSetpointThreshold; + +#define D_BOOST_GYRO_LPF_HZ 80 // Biquad lowpass input cutoff to peak D around propwash frequencies +#define D_BOOST_LPF_HZ 10 // PT1 lowpass cutoff to smooth the boost effect + +#ifdef USE_D_BOOST +static EXTENDED_FASTRAM float dBoostFactor; +static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration; +#endif + +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 9); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { .pid = { - [PID_ROLL] = { 40, 30, 23 }, - [PID_PITCH] = { 40, 30, 23 }, - [PID_YAW] = { 85, 45, 0 }, + [PID_ROLL] = { 40, 30, 23, 0 }, + [PID_PITCH] = { 40, 30, 23, 0 }, + [PID_YAW] = { 85, 45, 0, 0 }, [PID_LEVEL] = { .P = 20, // Self-level strength .I = 15, // Self-leveing low-pass frequency (0 - disabled) .D = 75, // 75% horizon strength + .FF = 0, }, - [PID_HEADING] = { 60, 0, 0 }, + [PID_HEADING] = { 60, 0, 0, 0 }, [PID_POS_XY] = { .P = 65, // NAV_POS_XY_P * 100 - .I = 120, // posDecelerationTime * 100 - .D = 10, // posResponseExpo * 100 + .I = 0, + .D = 0, + .FF = 0, }, [PID_VEL_XY] = { .P = 40, // NAV_VEL_XY_P * 20 .I = 15, // NAV_VEL_XY_I * 100 .D = 100, // NAV_VEL_XY_D * 100 + .FF = 40, // NAV_VEL_XY_D * 100 }, [PID_POS_Z] = { .P = 50, // NAV_POS_Z_P * 100 .I = 0, // not used .D = 0, // not used + .FF = 0, }, [PID_VEL_Z] = { .P = 100, // NAV_VEL_Z_P * 66.7 .I = 50, // NAV_VEL_Z_I * 20 .D = 10, // NAV_VEL_Z_D * 100 + .FF = 0, } } }, .bank_fw = { .pid = { - [PID_ROLL] = { 5, 7, 50 }, - [PID_PITCH] = { 5, 7, 50 }, - [PID_YAW] = { 6, 10, 60 }, + [PID_ROLL] = { 5, 7, 0, 50 }, + [PID_PITCH] = { 5, 7, 0, 50 }, + [PID_YAW] = { 6, 10, 0, 60 }, [PID_LEVEL] = { .P = 20, // Self-level strength .I = 5, // Self-leveing low-pass frequency (0 - disabled) .D = 75, // 75% horizon strength + .FF = 0, }, - [PID_HEADING] = { 60, 0, 0 }, + [PID_HEADING] = { 60, 0, 0, 0 }, [PID_POS_Z] = { .P = 40, // FW_POS_Z_P * 10 .I = 5, // FW_POS_Z_I * 10 .D = 10, // FW_POS_Z_D * 10 + .FF = 0, }, [PID_POS_XY] = { .P = 75, // FW_POS_XY_P * 100 .I = 5, // FW_POS_XY_I * 100 .D = 8, // FW_POS_XY_D * 100 + .FF = 0, } } }, @@ -175,6 +205,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .dterm_lpf_hz = 40, .yaw_lpf_hz = 30, .dterm_setpoint_weight = 1.0f, + .use_dterm_fir_filter = 1, .itermWindupPointPercent = 50, // Percent @@ -195,18 +226,17 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingItermLimitOnStickPosition = 0.5f, .loiter_direction = NAV_LOITER_RIGHT, + .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, + .iterm_relax_type = ITERM_RELAX_SETPOINT, + .iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT, + .iterm_relax = ITERM_RELAX_OFF, + .dBoostFactor = 1.0f, + .dBoostMaxAtAlleceleration = 7500.0f, + .dBoostGyroDeltaLpfHz = D_BOOST_GYRO_LPF_HZ, ); void pidInit(void) { - // Calculate derivative using 5-point noise-robust differentiators without time delay (one-sided or forward filters) - // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ - // h[0] = 5/8, h[-1] = 1/4, h[-2] = -1, h[-3] = -1/4, h[-4] = 3/8 - static const float dtermCoeffs[PID_GYRO_RATE_BUF_LENGTH] = {5.0f/8, 2.0f/8, -8.0f/8, -2.0f/8, 3.0f/8}; - for (int axis = 0; axis < 3; ++ axis) { - firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs); - } - pidResetTPAFilter(); // Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold @@ -214,6 +244,16 @@ void pidInit(void) cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH])); pidGainsUpdateRequired = false; + + itermRelax = pidProfile()->iterm_relax; + itermRelaxType = pidProfile()->iterm_relax_type; + itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff; + +#ifdef USE_D_BOOST + dBoostFactor = pidProfile()->dBoostFactor; + dBoostMaxAtAlleceleration = pidProfile()->dBoostMaxAtAlleceleration; +#endif + } bool pidInitFilters(void) @@ -224,6 +264,30 @@ bool pidInitFilters(void) return false; } + static float dtermCoeffs[PID_GYRO_RATE_BUF_LENGTH]; + + if (pidProfile()->use_dterm_fir_filter) { + // Calculate derivative using 5-point noise-robust differentiators without time delay (one-sided or forward filters) + // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ + // h[0] = 5/8, h[-1] = 1/4, h[-2] = -1, h[-3] = -1/4, h[-4] = 3/8 + dtermCoeffs[0] = 5.0f/8; + dtermCoeffs[1] = 2.0f/8; + dtermCoeffs[2] = -8.0f/8; + dtermCoeffs[3] = -2.0f/8; + dtermCoeffs[4] = 3.0f/8; + } else { + //simple d(t) - d(t-1) differentiator + dtermCoeffs[0] = 1.0f; + dtermCoeffs[1] = -1.0f; + dtermCoeffs[2] = 0.0f; + dtermCoeffs[3] = 0.0f; + dtermCoeffs[4] = 0.0f; + } + + for (int axis = 0; axis < 3; ++ axis) { + firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs); + } + #ifdef USE_DTERM_NOTCH notchFilterApplyFn = nullFilterApply; if (pidProfile()->dterm_soft_notch_hz != 0) { @@ -239,6 +303,18 @@ bool pidInitFilters(void) biquadFilterInitLPF(&pidState[axis].deltaLpfState, pidProfile()->dterm_lpf_hz, refreshRate); } + for (int i = 0; i < XYZ_AXIS_COUNT; i++) { + pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, refreshRate * 1e-6f); + } + +#ifdef USE_D_BOOST + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + biquadFilterInitLPF(&pidState[axis].dBoostGyroLpf, pidProfile()->dBoostGyroDeltaLpfHz, getLooptime()); + } +#endif + + pidFiltersConfigured = true; + return true; } @@ -324,7 +400,7 @@ static float calculateMultirotorTPAFactor(void) if (currentControlRateProfile->throttle.dynPID == 0 || rcCommand[THROTTLE] < currentControlRateProfile->throttle.pa_breakpoint) { tpaFactor = 1.0f; } else if (rcCommand[THROTTLE] < motorConfig()->maxthrottle) { - tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlRateProfile->throttle.pa_breakpoint) / (motorConfig()->maxthrottle - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f; + tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlRateProfile->throttle.pa_breakpoint) / (float)(motorConfig()->maxthrottle - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f; } else { tpaFactor = (100 - currentControlRateProfile->throttle.dynPID) / 100.0f; } @@ -337,7 +413,7 @@ void schedulePidGainsUpdate(void) pidGainsUpdateRequired = true; } -void updatePIDCoefficients(void) +void FAST_CODE NOINLINE updatePIDCoefficients(void) { STATIC_FASTRAM uint16_t prevThrottle = 0; @@ -360,7 +436,7 @@ void updatePIDCoefficients(void) * Compute stick position in range of [-1.0f : 1.0f] without deadband and expo */ for (int axis = 0; axis < 3; axis++) { - pidState[axis].stickPosition = constrain(rcData[axis] - PWM_RANGE_MIDDLE, -500, 500) / 500.0f; + pidState[axis].stickPosition = constrain(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE, -500, 500) / 500.0f; } // If nothing changed - don't waste time recalculating coefficients @@ -378,7 +454,7 @@ void updatePIDCoefficients(void) pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor; pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor; pidState[axis].kD = 0.0f; - pidState[axis].kFF = pidBank()->pid[axis].D / FP_PID_RATE_FF_MULTIPLIER * tpaFactor; + pidState[axis].kFF = pidBank()->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor; pidState[axis].kT = 0.0f; } else { @@ -459,7 +535,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h } /* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */ -static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis) +static void FAST_CODE pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis) { const uint32_t axisAccelLimit = (axis == FD_YAW) ? pidProfile()->axisAccelerationLimitYaw : pidProfile()->axisAccelerationLimitRollPitch; @@ -481,7 +557,7 @@ bool isFixedWingItermLimitActive(float stickPosition) return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition; } -static void pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis) +static void FAST_CODE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis) { const float rateError = pidState->rateTarget - pidState->gyroRate; @@ -523,7 +599,70 @@ static void pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamic #endif } -static void pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis) +static void FAST_CODE applyItermRelax(const int axis, const float gyroRate, float currentPidSetpoint, float *itermErrorRate) +{ + const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); + const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); + + if (itermRelax) { + if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) { + + const float itermRelaxFactor = MAX(0, 1 - setpointHpf / itermRelaxSetpointThreshold); + + if (itermRelaxType == ITERM_RELAX_SETPOINT) { + *itermErrorRate *= itermRelaxFactor; + } else if (itermRelaxType == ITERM_RELAX_GYRO ) { + *itermErrorRate = fapplyDeadbandf(setpointLpf - gyroRate, setpointHpf); + } else { + *itermErrorRate = 0.0f; + } + + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf)); + DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f)); + DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(*itermErrorRate)); + } + } + } +} +#ifdef USE_D_BOOST +static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t axis) { + + float dBoost = 1.0f; + + if (dBoostFactor > 1) { + const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT; + const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta)); + const float dBoostRateAcceleration = fabsf((pidState->rateTarget - pidState->previousRateTarget) / dT); + + const float acceleration = MAX(dBoostGyroAcceleration, dBoostRateAcceleration); + dBoost = scaleRangef(acceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostFactor); + dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT); + dBoost = constrainf(dBoost, 1.0f, dBoostFactor); + + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_D_BOOST, 0, dBoostGyroAcceleration); + DEBUG_SET(DEBUG_D_BOOST, 1, dBoostRateAcceleration); + DEBUG_SET(DEBUG_D_BOOST, 2, dBoost * 100); + } else if (axis == FD_PITCH) { + DEBUG_SET(DEBUG_D_BOOST, 3, dBoost * 100); + } + + pidState->previousRateTarget = pidState->rateTarget; + pidState->previousRateGyro = pidState->gyroRate; + } + + return dBoost; +} +#else +static float applyDBoost(pidState_t *pidState, flight_dynamics_index_t axis) { + UNUSED(pidState); + UNUSED(axis); + return 1.0f; +} +#endif + +static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis) { const float rateError = pidState->rateTarget - pidState->gyroRate; @@ -558,9 +697,11 @@ static void pidApplyMulticopterRateController(pidState_t *pidState, flight_dynam deltaFiltered = biquadFilterApply(&pidState->deltaLpfState, deltaFiltered); } - // Calculate derivative firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered); - newDTerm = firFilterApply(&pidState->gyroRateFilter) * (pidState->kD / dT); + newDTerm = firFilterApply(&pidState->gyroRateFilter); + + // Calculate derivative + newDTerm = newDTerm * (pidState->kD / dT) * applyDBoost(pidState, axis); // Additionally constrain D newDTerm = constrainf(newDTerm, -300.0f, 300.0f); @@ -574,7 +715,10 @@ static void pidApplyMulticopterRateController(pidState_t *pidState, flight_dynam const float motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f); const float antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f); - pidState->errorGyroIf += (rateError * pidState->kI * antiWindupScaler * dT) + float itermErrorRate = rateError; + applyItermRelax(axis, pidState->gyroRate, pidState->rateTarget, &itermErrorRate); + + pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); // Don't grow I-term if motors are at their limit @@ -686,7 +830,7 @@ float pidHeadingHold(void) New controller for 2deg error requires 2,6dps. 4dps for 3deg and so on up until mag_hold_rate_limit is reached. */ - headingHoldRate = error * pidBank()->pid[PID_HEADING].P / 30; + headingHoldRate = error * pidBank()->pid[PID_HEADING].P / 30.0f; headingHoldRate = constrainf(headingHoldRate, -pidProfile()->heading_hold_rate_limit, pidProfile()->heading_hold_rate_limit); headingHoldRate = pt1FilterApply4(&headingHoldRateFilter, headingHoldRate, HEADING_HOLD_ERROR_LPF_FREQ, dT); @@ -774,8 +918,12 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT); } -void pidController(void) +void FAST_CODE pidController(void) { + if (!pidFiltersConfigured) { + return; + } + bool canUseFpvCameraMix = true; uint8_t headingHoldState = getHeadingHoldState(); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index a0943320890..befa9c61541 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -50,6 +50,9 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13 #define FP_PID_LEVEL_P_MULTIPLIER 6.56f // Level P gain units is [1/sec] and angle error is [deg] => [deg/s] #define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f +#define MC_ITERM_RELAX_SETPOINT_THRESHOLD 40.0f +#define MC_ITERM_RELAX_CUTOFF_DEFAULT 20 + typedef enum { /* PID MC FW */ PID_ROLL, // + + @@ -69,12 +72,24 @@ typedef struct pid8_s { uint8_t P; uint8_t I; uint8_t D; + uint8_t FF; } pid8_t; typedef struct pidBank_s { pid8_t pid[PID_ITEM_COUNT]; } pidBank_t; +typedef enum { + ITERM_RELAX_OFF = 0, + ITERM_RELAX_RP, + ITERM_RELAX_RPY +} itermRelax_e; + +typedef enum { + ITERM_RELAX_GYRO = 0, + ITERM_RELAX_SETPOINT +} itermRelaxType_e; + typedef struct pidProfile_s { pidBank_t bank_fw; pidBank_t bank_mc; @@ -82,6 +97,7 @@ typedef struct pidProfile_s { uint16_t dterm_soft_notch_hz; // Dterm Notch frequency uint16_t dterm_soft_notch_cutoff; // Dterm Notch Cutoff frequency uint8_t dterm_lpf_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5 + uint8_t use_dterm_fir_filter; // Use classical INAV FIR differentiator. Very noise robust, can be quite slowish uint8_t yaw_pterm_lpf_hz; // Used for filering Pterm noise on noisy frames uint8_t yaw_lpf_hz; @@ -106,6 +122,15 @@ typedef struct pidProfile_s { float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) + float navVelXyDTermLpfHz; + + uint8_t iterm_relax_type; // Specifies type of relax algorithm + uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint + uint8_t iterm_relax; // Enable iterm suppression during stick input + + float dBoostFactor; + float dBoostMaxAtAlleceleration; + uint8_t dBoostGyroDeltaLpfHz; } pidProfile_t; typedef struct pidAutotuneConfig_s { @@ -119,8 +144,8 @@ typedef struct pidAutotuneConfig_s { PG_DECLARE_PROFILE(pidProfile_t, pidProfile); PG_DECLARE(pidAutotuneConfig_t, pidAutotuneConfig); -static inline const pidBank_t * pidBank() { return STATE(FIXED_WING) ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; } -static inline pidBank_t * pidBankMutable() { return STATE(FIXED_WING) ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; } +static inline const pidBank_t * pidBank(void) { return STATE(FIXED_WING) ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; } +static inline pidBank_t * pidBankMutable(void) { return STATE(FIXED_WING) ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; } extern int16_t axisPID[]; extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[]; diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index 0a4218990a4..fc2e7706fcd 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -20,6 +20,18 @@ #if defined(USE_ADC) && defined(USE_GPS) +/* INPUTS: + * - heading degrees + * - horizontalWindSpeed + * - windHeading degrees + * OUTPUT: + * returns same unit as horizontalWindSpeed + */ +static float forwardWindSpeed(float heading, float horizontalWindSpeed, float windHeading) { + return horizontalWindSpeed * cos_approx(DEGREES_TO_RADIANS(windHeading - heading)); +} + +#ifdef USE_WIND_ESTIMATOR /* INPUTS: * - forwardSpeed (same unit as horizontalWindSpeed) * - heading degrees @@ -44,17 +56,6 @@ static float windDriftCorrectedForwardSpeed(float forwardSpeed, float heading, f return forwardSpeed * cos_approx(DEGREES_TO_RADIANS(windDriftCompensationAngle(forwardSpeed, heading, horizontalWindSpeed, windHeading))); } -/* INPUTS: - * - heading degrees - * - horizontalWindSpeed - * - windHeading degrees - * OUTPUT: - * returns same unit as horizontalWindSpeed - */ -static float forwardWindSpeed(float heading, float horizontalWindSpeed, float windHeading) { - return horizontalWindSpeed * cos_approx(DEGREES_TO_RADIANS(windHeading - heading)); -} - /* INPUTS: * - forwardSpeed (same unit as horizontalWindSpeed) * - heading degrees @@ -66,17 +67,17 @@ static float forwardWindSpeed(float heading, float horizontalWindSpeed, float wi static float windCompensatedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) { return windDriftCorrectedForwardSpeed(forwardSpeed, heading, horizontalWindSpeed, windHeading) + forwardWindSpeed(heading, horizontalWindSpeed, windHeading); } +#endif // returns degrees -static int8_t RTHAltitudeChangePitchAngle(float altitudeChange) { +static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) { return altitudeChange < 0 ? navConfig()->fw.max_dive_angle : -navConfig()->fw.max_climb_angle; } -// altitudeChange is in meters -// idle_power and cruise_power are in deciWatt -// output is in Watt -static float estimateRTHAltitudeChangePower(float altitudeChange) { - uint16_t altitudeChangeThrottle = navConfig()->fw.cruise_throttle - RTHAltitudeChangePitchAngle(altitudeChange) * navConfig()->fw.pitch_to_throttle; +// pitch in degrees +// output in Watt +static float estimatePitchPower(float pitch) { + int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch)); altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - motorConfig()->minthrottle) / (navConfig()->fw.cruise_throttle - motorConfig()->minthrottle); return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100; @@ -88,7 +89,7 @@ static float estimateRTHAltitudeChangePower(float altitudeChange) { // output is in seconds static float estimateRTHAltitudeChangeTime(float altitudeChange, float verticalWindSpeed) { // Assuming increase in throttle keeps air speed at cruise speed - const float estimatedVerticalSpeed = (float)navConfig()->fw.cruise_speed / 100 * sin_approx(DEGREES_TO_RADIANS(RTHAltitudeChangePitchAngle(altitudeChange))) + verticalWindSpeed; + const float estimatedVerticalSpeed = (float)navConfig()->fw.cruise_speed / 100 * sin_approx(DEGREES_TO_RADIANS(RTHInitialAltitudeChangePitchAngle(altitudeChange))) + verticalWindSpeed; return altitudeChange / estimatedVerticalSpeed; } @@ -100,17 +101,22 @@ static float estimateRTHAltitudeChangeTime(float altitudeChange, float verticalW // output is in meters static float estimateRTHAltitudeChangeGroundDistance(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed) { // Assuming increase in throttle keeps air speed at cruise speed - float estimatedHorizontalSpeed = (float)navConfig()->fw.cruise_speed / 100 * cos_approx(DEGREES_TO_RADIANS(RTHAltitudeChangePitchAngle(altitudeChange))) + forwardWindSpeed(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw), horizontalWindSpeed, windHeading); + const float estimatedHorizontalSpeed = (float)navConfig()->fw.cruise_speed / 100 * cos_approx(DEGREES_TO_RADIANS(RTHInitialAltitudeChangePitchAngle(altitudeChange))) + forwardWindSpeed(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw), horizontalWindSpeed, windHeading); return estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) * estimatedHorizontalSpeed; } // altitudeChange is in m // verticalWindSpeed is in m/s // output is in Wh -static uint16_t estimateRTHAltitudeChangeEnergy(float altitudeChange, float verticalWindSpeed) { - return estimateRTHAltitudeChangePower(altitudeChange) * estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) / 3600; +static float estimateRTHInitialAltitudeChangeEnergy(float altitudeChange, float verticalWindSpeed) { + const float RTHInitialAltitudeChangePower = estimatePitchPower(altitudeChange < 0 ? navConfig()->fw.max_dive_angle : -navConfig()->fw.max_climb_angle); + return RTHInitialAltitudeChangePower * estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) / 3600; } +// horizontalWindSpeed is in m/s +// windHeading is in degrees +// verticalWindSpeed is in m/s +// altitudeChange is in m // returns distance in m // *heading is in degrees static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed, float *heading) { @@ -128,8 +134,17 @@ static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChan } } -// returns mWh -static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { +// distanceToHome is in meters +// output in Watt +static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float speedToHome) { + const float timeToHome = distanceToHome / speedToHome; // seconds + const float altitudeChangeDescentToHome = CENTIMETERS_TO_METERS(navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT ? MAX(0, getEstimatedActualPosition(Z) - getFinalRTHAltitude()) : 0); + const float pitchToHome = MIN(RADIANS_TO_DEGREES(atan2_approx(altitudeChangeDescentToHome, distanceToHome)), navConfig()->fw.max_dive_angle); + return estimatePitchPower(pitchToHome) * timeToHome / 3600; +} + +// returns Wh +static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { // Fixed wing only for now if (!STATE(FIXED_WING)) return -1; @@ -141,35 +156,40 @@ static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { )) return -1; + const float RTH_initial_altitude_change = MAX(0, (getFinalRTHAltitude() - getEstimatedActualPosition(Z)) / 100); + + float RTH_heading; // degrees #ifdef USE_WIND_ESTIMATOR uint16_t windHeading; // centidegrees const float horizontalWindSpeed = takeWindIntoAccount ? getEstimatedHorizontalWindSpeed(&windHeading) / 100 : 0; // m/s const float windHeadingDegrees = CENTIDEGREES_TO_DEGREES((float)windHeading); const float verticalWindSpeed = getEstimatedWindSpeed(Z) / 100; + + const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading); + const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, RTH_heading, horizontalWindSpeed, windHeadingDegrees); #else UNUSED(takeWindIntoAccount); - const float horizontalWindSpeed = 0; // m/s - const float windHeadingDegrees = 0; - const float verticalWindSpeed = 0; + const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, 0, 0, 0, &RTH_heading); + const float RTH_speed = (float)navConfig()->fw.cruise_speed / 100; #endif - const float RTH_altitude_change = (RTHAltitude() - getEstimatedActualPosition(Z)) / 100; - float RTH_heading; // degrees - const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading); - const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, DECIDEGREES_TO_DEGREES(attitude.values.yaw), horizontalWindSpeed, windHeadingDegrees); - - DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 0, lrintf(RTH_altitude_change * 100)); + DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 0, lrintf(RTH_initial_altitude_change * 100)); DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 1, lrintf(RTH_distance * 100)); DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 2, lrintf(RTH_speed * 100)); +#ifdef USE_WIND_ESTIMATOR DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 3, lrintf(horizontalWindSpeed * 100)); +#endif if (RTH_speed <= 0) - return -2; // wind is too strong + return -2; // wind is too strong to return at cruise throttle (TODO: might be possible to take into account min speed thr boost) - const uint32_t time_to_home = RTH_distance / RTH_speed; // seconds - const uint32_t energy_to_home = estimateRTHAltitudeChangeEnergy(RTH_altitude_change, verticalWindSpeed) * 1000 + heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power) * time_to_home / 360; // mWh - const uint32_t energy_margin_abs = (currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical) * batteryMetersConfig()->rth_energy_margin / 100; // mWh - const int32_t remaining_energy_before_rth = getBatteryRemainingCapacity() - energy_margin_abs - energy_to_home; // mWh +#ifdef USE_WIND_ESTIMATOR + const float energy_to_home = estimateRTHInitialAltitudeChangeEnergy(RTH_initial_altitude_change, verticalWindSpeed) + estimateRTHEnergyAfterInitialClimb(RTH_distance, RTH_speed); // Wh +#else + const float energy_to_home = estimateRTHInitialAltitudeChangeEnergy(RTH_initial_altitude_change, 0) + estimateRTHEnergyAfterInitialClimb(RTH_distance, RTH_speed); // Wh +#endif + const float energy_margin_abs = (currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical) * batteryMetersConfig()->rth_energy_margin / 100000; // Wh + const float remaining_energy_before_rth = getBatteryRemainingCapacity() / 1000 - energy_margin_abs - energy_to_home; // Wh if (remaining_energy_before_rth < 0) // No energy left = No time left return 0; @@ -178,31 +198,28 @@ static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { } // returns seconds -int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) { +float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) { - const int32_t remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount); + const float remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount); // error: return error code directly if (remainingEnergyBeforeRTH < 0) return remainingEnergyBeforeRTH; - const int32_t averagePower = calculateAveragePower(); + const float averagePower = (float)calculateAveragePower() / 100; if (averagePower == 0) return -1; - const uint32_t time_before_rth = remainingEnergyBeforeRTH * 360 / averagePower; - - if (time_before_rth > 0x7FFFFFFF) // int32 overflow - return -1; + const float time_before_rth = remainingEnergyBeforeRTH * 3600 / averagePower; return time_before_rth; } // returns meters -int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) { +float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) { - const int32_t remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount); + const float remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount); // error: return error code directly if (remainingFlightTimeBeforeRTH < 0) diff --git a/src/main/flight/rth_estimator.h b/src/main/flight/rth_estimator.h index 035bc0d6442..b24f48fddb8 100644 --- a/src/main/flight/rth_estimator.h +++ b/src/main/flight/rth_estimator.h @@ -1,5 +1,5 @@ #if defined(USE_ADC) && defined(USE_GPS) -int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount); -int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount); +float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount); +float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount); #endif diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 71b71683c77..90fa394280e 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -62,7 +62,22 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig, .tri_unarmed_servo = 1 ); -PG_REGISTER_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 0); +PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1); + +void pgResetFn_customServoMixers(servoMixer_t *instance) +{ + for (int i = 0; i < MAX_SERVO_RULES; i++) { + RESET_CONFIG(servoMixer_t, &instance[i], + .targetChannel = 0, + .inputSource = 0, + .rate = 0, + .speed = 0 +#ifdef USE_LOGIC_CONDITIONS + ,.conditionId = -1 +#endif + ); + } +} PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 2); @@ -129,9 +144,19 @@ void servosInit(void) mixerUsesServos = 1; } - for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) + for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servoComputeScalingFactors(i); + } +} +int getServoCount(void) +{ + if (servoRuleCount) { + return 1 + maxServoIndex - minServoIndex; + } + else { + return 0; + } } void loadCustomServoMixer(void) @@ -222,14 +247,23 @@ void servoMixer(float dT) input[INPUT_STABILIZED_YAW] = axisPID[YAW]; // Reverse yaw servo when inverted in 3D mode only for multirotor and tricopter - if (feature(FEATURE_3D) && (rcData[THROTTLE] < PWM_RANGE_MIDDLE) && + if (feature(FEATURE_3D) && (rxGetChannelValue(THROTTLE) < PWM_RANGE_MIDDLE) && (mixerConfig()->platformType == PLATFORM_MULTIROTOR || mixerConfig()->platformType == PLATFORM_TRICOPTER)) { input[INPUT_STABILIZED_YAW] *= -1; } } + input[INPUT_STABILIZED_ROLL_PLUS] = constrain(input[INPUT_STABILIZED_ROLL], 0, 1000); + input[INPUT_STABILIZED_ROLL_MINUS] = constrain(input[INPUT_STABILIZED_ROLL], -1000, 0); + input[INPUT_STABILIZED_PITCH_PLUS] = constrain(input[INPUT_STABILIZED_PITCH], 0, 1000); + input[INPUT_STABILIZED_PITCH_MINUS] = constrain(input[INPUT_STABILIZED_PITCH], -1000, 0); + input[INPUT_STABILIZED_YAW_PLUS] = constrain(input[INPUT_STABILIZED_YAW], 0, 1000); + input[INPUT_STABILIZED_YAW_MINUS] = constrain(input[INPUT_STABILIZED_YAW], -1000, 0); + input[INPUT_FEATURE_FLAPS] = FLIGHT_MODE(FLAPERON) ? servoConfig()->flaperon_throw_offset : 0; + input[INPUT_LOGIC_ONE] = 500; + if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -900, 900, -500, +500); input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500); @@ -246,22 +280,24 @@ void servoMixer(float dT) // 2000 - 1500 = +500 // 1500 - 1500 = 0 // 1000 - 1500 = -500 - input[INPUT_RC_ROLL] = rcData[ROLL] - PWM_RANGE_MIDDLE; - input[INPUT_RC_PITCH] = rcData[PITCH] - PWM_RANGE_MIDDLE; - input[INPUT_RC_YAW] = rcData[YAW] - PWM_RANGE_MIDDLE; - input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH5] = rcData[AUX1] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH6] = rcData[AUX2] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH7] = rcData[AUX3] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH8] = rcData[AUX4] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH9] = rcData[AUX5] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH10] = rcData[AUX6] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH11] = rcData[AUX7] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH12] = rcData[AUX8] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH13] = rcData[AUX9] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH14] = rcData[AUX10] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH15] = rcData[AUX11] - PWM_RANGE_MIDDLE; - input[INPUT_RC_CH16] = rcData[AUX12] - PWM_RANGE_MIDDLE; +#define GET_RX_CHANNEL_INPUT(x) (rxGetChannelValue(x) - PWM_RANGE_MIDDLE) + input[INPUT_RC_ROLL] = GET_RX_CHANNEL_INPUT(ROLL); + input[INPUT_RC_PITCH] = GET_RX_CHANNEL_INPUT(PITCH); + input[INPUT_RC_YAW] = GET_RX_CHANNEL_INPUT(YAW); + input[INPUT_RC_THROTTLE] = GET_RX_CHANNEL_INPUT(THROTTLE); + input[INPUT_RC_CH5] = GET_RX_CHANNEL_INPUT(AUX1); + input[INPUT_RC_CH6] = GET_RX_CHANNEL_INPUT(AUX2); + input[INPUT_RC_CH7] = GET_RX_CHANNEL_INPUT(AUX3); + input[INPUT_RC_CH8] = GET_RX_CHANNEL_INPUT(AUX4); + input[INPUT_RC_CH9] = GET_RX_CHANNEL_INPUT(AUX5); + input[INPUT_RC_CH10] = GET_RX_CHANNEL_INPUT(AUX6); + input[INPUT_RC_CH11] = GET_RX_CHANNEL_INPUT(AUX7); + input[INPUT_RC_CH12] = GET_RX_CHANNEL_INPUT(AUX8); + input[INPUT_RC_CH13] = GET_RX_CHANNEL_INPUT(AUX9); + input[INPUT_RC_CH14] = GET_RX_CHANNEL_INPUT(AUX10); + input[INPUT_RC_CH15] = GET_RX_CHANNEL_INPUT(AUX11); + input[INPUT_RC_CH16] = GET_RX_CHANNEL_INPUT(AUX12); +#undef GET_RX_CHANNEL_INPUT for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servo[i] = 0; @@ -269,6 +305,16 @@ void servoMixer(float dT) // mix servos according to rules for (int i = 0; i < servoRuleCount; i++) { + + /* + * Check if conditions for a rule are met, not all conditions apply all the time + */ + #ifdef USE_LOGIC_CONDITIONS + if (!logicConditionGetValue(currentServoMixer[i].conditionId)) { + continue; + } + #endif + const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t from = currentServoMixer[i].inputSource; @@ -398,12 +444,12 @@ void processServoAutotrim(void) } } -bool isServoOutputEnabled(void) +bool FAST_CODE NOINLINE isServoOutputEnabled(void) { return servoOutputEnabled; } -bool isMixerUsingServos(void) +bool FAST_CODE NOINLINE isMixerUsingServos(void) { return mixerUsesServos; } diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 20f988acb4a..196879e6bc8 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -18,34 +18,42 @@ #pragma once #include "config/parameter_group.h" +#include "common/logic_condition.h" #define MAX_SUPPORTED_SERVOS 8 // These must be consecutive typedef enum { - INPUT_STABILIZED_ROLL = 0, - INPUT_STABILIZED_PITCH = 1, - INPUT_STABILIZED_YAW = 2, - INPUT_STABILIZED_THROTTLE = 3, - INPUT_RC_ROLL = 4, - INPUT_RC_PITCH = 5, - INPUT_RC_YAW = 6, - INPUT_RC_THROTTLE = 7, - INPUT_RC_CH5 = 8, - INPUT_RC_CH6 = 9, - INPUT_RC_CH7 = 10, - INPUT_RC_CH8 = 11, - INPUT_GIMBAL_PITCH = 12, - INPUT_GIMBAL_ROLL = 13, - INPUT_FEATURE_FLAPS = 14, - INPUT_RC_CH9 = 15, - INPUT_RC_CH10 = 16, - INPUT_RC_CH11 = 17, - INPUT_RC_CH12 = 18, - INPUT_RC_CH13 = 19, - INPUT_RC_CH14 = 20, - INPUT_RC_CH15 = 21, - INPUT_RC_CH16 = 22, + INPUT_STABILIZED_ROLL = 0, + INPUT_STABILIZED_PITCH = 1, + INPUT_STABILIZED_YAW = 2, + INPUT_STABILIZED_THROTTLE = 3, + INPUT_RC_ROLL = 4, + INPUT_RC_PITCH = 5, + INPUT_RC_YAW = 6, + INPUT_RC_THROTTLE = 7, + INPUT_RC_CH5 = 8, + INPUT_RC_CH6 = 9, + INPUT_RC_CH7 = 10, + INPUT_RC_CH8 = 11, + INPUT_GIMBAL_PITCH = 12, + INPUT_GIMBAL_ROLL = 13, + INPUT_FEATURE_FLAPS = 14, + INPUT_RC_CH9 = 15, + INPUT_RC_CH10 = 16, + INPUT_RC_CH11 = 17, + INPUT_RC_CH12 = 18, + INPUT_RC_CH13 = 19, + INPUT_RC_CH14 = 20, + INPUT_RC_CH15 = 21, + INPUT_RC_CH16 = 22, + INPUT_STABILIZED_ROLL_PLUS = 23, + INPUT_STABILIZED_ROLL_MINUS = 24, + INPUT_STABILIZED_PITCH_PLUS = 25, + INPUT_STABILIZED_PITCH_MINUS = 26, + INPUT_STABILIZED_YAW_PLUS = 27, + INPUT_STABILIZED_YAW_MINUS = 28, + INPUT_LOGIC_ONE = 29, INPUT_SOURCE_COUNT } inputSource_e; @@ -93,6 +101,9 @@ typedef struct servoMixer_s { uint8_t inputSource; // input channel for this rule 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 +#ifdef USE_LOGIC_CONDITIONS + int8_t conditionId; +#endif } servoMixer_t; #define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS) @@ -138,3 +149,4 @@ void loadCustomServoMixer(void); void servoMixer(float dT); void servoComputeScalingFactors(uint8_t servoIndex); void servosInit(void); +int getServoCount(void); diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index 90a33ed4fe9..dcb49f00805 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -163,10 +163,6 @@ void updateWindEstimator(timeUs_t currentTimeUs) estimatedWind[X] = estimatedWind[X] * 0.95f + wind[X] * 0.05f; estimatedWind[Y] = estimatedWind[Y] * 0.95f + wind[Y] * 0.05f; estimatedWind[Z] = estimatedWind[Z] * 0.95f + wind[Z] * 0.05f; - - DEBUG_SET(DEBUG_WIND_ESTIMATOR, 0, estimatedWind[X]); - DEBUG_SET(DEBUG_WIND_ESTIMATOR, 1, estimatedWind[Y]); - DEBUG_SET(DEBUG_WIND_ESTIMATOR, 2, estimatedWind[Z]); } lastUpdateUs = currentTimeUs; hasValidWindEstimate = true; diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index fc230bee19d..619fb552d79 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -367,7 +367,7 @@ static void showStatusPage(void) if (feature(FEATURE_CURRENT_METER)) { i2c_OLED_set_line(rowIndex++); - tfp_sprintf(lineBuffer, "mAh: %d", getMAhDrawn()); + tfp_sprintf(lineBuffer, "mAh: %d", (int)getMAhDrawn()); padLineBufferToChar(12); i2c_OLED_send_string(lineBuffer); @@ -392,7 +392,7 @@ static void showStatusPage(void) i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); - tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER); + tfp_sprintf(lineBuffer, "La/Lo: %d/%d", (int)(gpsSol.llh.lat / GPS_DEGREES_DIVIDER), (int)(gpsSol.llh.lon / GPS_DEGREES_DIVIDER)); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); @@ -412,7 +412,7 @@ static void showStatusPage(void) #ifdef USE_BARO if (sensors(SENSOR_BARO)) { int32_t alt = baroCalculateAltitude(); - tfp_sprintf(lineBuffer, "Alt: %d", alt / 100); + tfp_sprintf(lineBuffer, "Alt: %d", (int)(alt / 100)); padHalfLineBuffer(); i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex); i2c_OLED_send_string(lineBuffer); diff --git a/src/main/io/displayport_hott.c b/src/main/io/displayport_hott.c new file mode 100644 index 00000000000..85eed39164d --- /dev/null +++ b/src/main/io/displayport_hott.c @@ -0,0 +1,176 @@ +/* + * 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 . + */ + +#include +#include +#include + +#include "platform.h" +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) + +#include "common/utils.h" +#include "cms/cms.h" +#include "telemetry/hott.h" + +displayPort_t hottDisplayPort; + +static int hottDrawScreen(displayPort_t *displayPort) +{ + UNUSED(displayPort); + return 0; +} + +static int hottScreenSize(const displayPort_t *displayPort) +{ + return displayPort->rows * displayPort->cols; +} + +static int hottWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr) +{ + UNUSED(displayPort); + UNUSED(attr); + + hottTextmodeWriteChar(col, row, (uint8_t)c); + return 0; +} + +static int hottWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *s, textAttributes_t attr) +{ + while (*s) { + hottWriteChar(displayPort, col++, row, *(s++), attr); + } + return 0; +} + +static int hottClearScreen(displayPort_t *displayPort) +{ + for (int row = 0; row < displayPort->rows; row++) { + for (int col= 0; col < displayPort->cols; col++) { + hottWriteChar(displayPort, col, row, ' ', NULL); + } + } + return 0; +} + +static bool hottIsTransferInProgress(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return false; +} + +static int hottHeartbeat(displayPort_t *displayPort) +{ + if (!hottTextmodeIsAlive()) { + cmsMenuExit(displayPort, (void*)CMS_EXIT_SAVE); + } + + return 0; +} + +static void hottResync(displayPort_t *displayPort) +{ + UNUSED(displayPort); +} + +static uint32_t hottTxBytesFree(const displayPort_t *displayPort) +{ + UNUSED(displayPort); + return UINT32_MAX; +} + +static int hottGrab(displayPort_t *displayPort) +{ + hottTextmodeGrab(); + return displayPort->grabCount = 1; +} + +static int hottRelease(displayPort_t *displayPort) +{ + int cnt = displayPort->grabCount = 0; + hottClearScreen(displayPort); + hottTextmodeExit(); + return cnt; +} + +static const displayPortVTable_t hottVTable = { + .grab = hottGrab, + .release = hottRelease, + .clearScreen = hottClearScreen, + .drawScreen = hottDrawScreen, + .screenSize = hottScreenSize, + .writeString = hottWriteString, + .writeChar = hottWriteChar, + .isTransferInProgress = hottIsTransferInProgress, + .heartbeat = hottHeartbeat, + .resync = hottResync, + .txBytesFree = hottTxBytesFree +}; + +displayPort_t *displayPortHottInit(void) +{ + hottDisplayPort.device = NULL; + displayInit(&hottDisplayPort, &hottVTable); + hottDisplayPort.useFullscreen = true; + hottDisplayPort.rows = HOTT_TEXTMODE_DISPLAY_ROWS; + hottDisplayPort.cols = HOTT_TEXTMODE_DISPLAY_COLUMNS; + return &hottDisplayPort; +} + +void hottDisplayportRegister(void) +{ + cmsDisplayPortRegister(displayPortHottInit()); +} + +void hottCmsOpen(void) +{ + if (!cmsInMenu) { + cmsDisplayPortSelect(&hottDisplayPort); + cmsMenuOpen(); + } +} + +void hottSetCmsKey(uint8_t hottKey, bool keepCmsOpen) +{ + switch (hottKey) { + case HOTTV4_BUTTON_DEC: + cmsSetExternKey(CMS_KEY_UP); + break; + case HOTTV4_BUTTON_INC: + cmsSetExternKey(CMS_KEY_DOWN); + break; + case HOTTV4_BUTTON_SET: + if (cmsInMenu) { + cmsMenuExit(cmsDisplayPortGetCurrent(), (void*)CMS_EXIT_SAVE); + } + cmsSetExternKey(CMS_KEY_NONE); + break; + case HOTTV4_BUTTON_NEXT: + cmsSetExternKey(CMS_KEY_RIGHT); + break; + case HOTTV4_BUTTON_PREV: + cmsSetExternKey(CMS_KEY_LEFT); + if (keepCmsOpen) { // Make sure CMS is open until textmode is closed. + cmsMenuOpen(); + } + break; + default: + cmsSetExternKey(CMS_KEY_NONE); + break; + } +} + +#endif diff --git a/src/main/io/displayport_hott.h b/src/main/io/displayport_hott.h new file mode 100644 index 00000000000..1735ca10334 --- /dev/null +++ b/src/main/io/displayport_hott.h @@ -0,0 +1,26 @@ +/* + * 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 . + */ + +#pragma once +#include "drivers/display.h" + +displayPort_t *displayPortHottInit(void); +extern displayPort_t hottDisplayPort; + +void hottDisplayportRegister(void); +void hottCmsOpen(void); +void hottSetCmsKey(uint8_t hottKey, bool esc); diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c index cba52cbac1d..796d5fb3710 100644 --- a/src/main/io/displayport_max7456.c +++ b/src/main/io/displayport_max7456.c @@ -29,7 +29,6 @@ #include "drivers/display.h" #include "drivers/max7456.h" -#include "drivers/vcd.h" #include "io/displayport_max7456.h" @@ -153,7 +152,7 @@ static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t { UNUSED(displayPort); - max7456Character_t chr; + osdCharacter_t chr; max7456ReadNvm(255, &chr); @@ -174,6 +173,14 @@ static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t return false; } +static int writeFontCharacter(displayPort_t *instance, uint16_t addr, const osdCharacter_t *chr) +{ + UNUSED(instance); + + max7456WriteNvm(addr, chr); + return 0; +} + static const displayPortVTable_t max7456VTable = { .grab = grab, .release = release, @@ -189,6 +196,7 @@ static const displayPortVTable_t max7456VTable = { .txBytesFree = txBytesFree, .supportedTextAttributes = supportedTextAttributes, .getFontMetadata = getFontMetadata, + .writeFontCharacter = writeFontCharacter, }; displayPort_t *max7456DisplayPortInit(const videoSystem_e videoSystem) diff --git a/src/main/io/displayport_max7456.h b/src/main/io/displayport_max7456.h index a88a3631f46..1aae64420c9 100644 --- a/src/main/io/displayport_max7456.h +++ b/src/main/io/displayport_max7456.h @@ -17,8 +17,8 @@ #pragma once -#include "config/parameter_group.h" -#include "drivers/display.h" -#include "drivers/vcd.h" +typedef struct displayPort_s displayPort_t; + +#include "drivers/osd.h" displayPort_t *max7456DisplayPortInit(const videoSystem_e videoSystem); diff --git a/src/main/io/esc_serialshot.c b/src/main/io/esc_serialshot.c new file mode 100644 index 00000000000..2a00e4d9eba --- /dev/null +++ b/src/main/io/esc_serialshot.c @@ -0,0 +1,116 @@ +/* + * 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 +#include +#include +#include + +#include "platform.h" + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/maths.h" +#include "common/crc.h" + +#include "io/serial.h" +#include "io/esc_serialshot.h" + +#if defined(USE_SERIALSHOT) + +#define SERIALSHOT_UART_BAUD 921600 +#define SERIALSHOT_PKT_DEFAULT_HEADER (0x00) // Default header (motors 1-4, regular 4-in-1 ESC) + + +typedef struct __attribute__((packed)) { + uint8_t hdr; // Header/version marker + uint8_t motorData[6]; // 12 bit per motor + uint8_t crc; // CRC8/DVB-T of hdr & motorData +} serialShortPacket_t; + + +static serialShortPacket_t txPkt; +static uint16_t motorValues[4]; +static serialPort_t * escPort = NULL; +static serialPortConfig_t * portConfig; + +bool serialshotInitialize(void) +{ + // Avoid double initialization + if (escPort) { + return true; + } + + portConfig = findSerialPortConfig(FUNCTION_SERIALSHOT); + if (!portConfig) { + return false; + } + + escPort = openSerialPort(portConfig->identifier, FUNCTION_SERIALSHOT, NULL, NULL, SERIALSHOT_UART_BAUD, MODE_RXTX, SERIAL_NOT_INVERTED | SERIAL_UNIDIR); + if (!escPort) { + return false; + } + + return true; +} + +void serialshotUpdateMotor(int index, uint16_t value) +{ + if (index < 0 || index > 3) { + return; + } + + motorValues[index] = value; +} + +void serialshotSendUpdate(void) +{ + // Check if the port is initialized + if (!escPort) { + return; + } + + // Skip update if previous one is not yet fully sent + // This helps to avoid buffer overflow and evenyually the data corruption + if (!isSerialTransmitBufferEmpty(escPort)) { + return; + } + + txPkt.hdr = SERIALSHOT_PKT_DEFAULT_HEADER; + + txPkt.motorData[0] = motorValues[0] & 0x00FF; + txPkt.motorData[1] = motorValues[1] & 0x00FF; + txPkt.motorData[2] = motorValues[2] & 0x00FF; + txPkt.motorData[3] = motorValues[3] & 0x00FF; + txPkt.motorData[4] = (((motorValues[0] & 0xF00) >> 8) << 0) | (((motorValues[1] & 0xF00) >> 8) << 4); + txPkt.motorData[5] = (((motorValues[2] & 0xF00) >> 8) << 0) | (((motorValues[3] & 0xF00) >> 8) << 4); + + txPkt.crc = crc8_dvb_s2(0x00, txPkt.hdr); + txPkt.crc = crc8_dvb_s2_update(txPkt.crc, txPkt.motorData, sizeof(txPkt.motorData)); + + serialWriteBuf(escPort, (const uint8_t *)&txPkt, sizeof(txPkt)); +} + +#endif diff --git a/src/main/io/esc_serialshot.h b/src/main/io/esc_serialshot.h new file mode 100644 index 00000000000..c915cc9f4f8 --- /dev/null +++ b/src/main/io/esc_serialshot.h @@ -0,0 +1,29 @@ +/* + * 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 + +bool serialshotInitialize(void); +void serialshotUpdateMotor(int index, uint16_t value); +void serialshotSendUpdate(void); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 8a63f43fa55..8fd74fc8b6c 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -338,7 +338,7 @@ void generateLedConfig(ledConfig_t *ledConfig, char *ledConfigBuffer, size_t buf *fptr = 0; // TODO - check buffer length - sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig)); + tfp_sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig)); } typedef enum { @@ -727,7 +727,7 @@ static void applyLedThrustRingLayer(bool updateNow, timeUs_t *timer) if (updateNow) { rotationPhase = rotationPhase > 0 ? rotationPhase - 1 : ledCounts.ringSeqLen - 1; - int scale = scaledThrottle; // ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + int scale = scaledThrottle; // ARMING_FLAG(ARMED) ? scaleRange(rxGetChannelValue(THROTTLE), PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; *timer += LED_STRIP_HZ(5) * 10 / scale; // 5 - 50Hz update rate } @@ -949,7 +949,7 @@ void ledStripUpdate(timeUs_t currentTimeUs) // apply all layers; triggered timed functions has to update timers - scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rxGetChannelValue(THROTTLE), PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; applyLedFixedLayers(); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d834bec5e2c..3626771959f 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -41,6 +41,7 @@ #include "cms/cms_menu_osd.h" #include "common/axis.h" +#include "common/constants.h" #include "common/filter.h" #include "common/olc.h" #include "common/printf.h" @@ -54,13 +55,15 @@ #include "config/parameter_group_ids.h" #include "drivers/display.h" -#include "drivers/max7456_symbols.h" +#include "drivers/osd_symbols.h" #include "drivers/time.h" #include "drivers/vtx_common.h" #include "io/flashfs.h" #include "io/gps.h" #include "io/osd.h" +#include "io/osd_hud.h" +#include "io/vtx.h" #include "io/vtx_string.h" #include "fc/config.h" @@ -83,7 +86,9 @@ #include "navigation/navigation_private.h" #include "rx/rx.h" +#include "rx/msp_override.h" +#include "sensors/acceleration.h" #include "sensors/battery.h" #include "sensors/boardalignment.h" #include "sensors/diagnostics.h" @@ -98,13 +103,7 @@ #define VIDEO_BUFFER_CHARS_PAL 480 #define IS_DISPLAY_PAL (displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL) -#define CENTIMETERS_TO_CENTIFEET(cm) (cm * (328 / 100.0)) -#define CENTIMETERS_TO_FEET(cm) (cm * (328 / 10000.0)) -#define CENTIMETERS_TO_METERS(cm) (cm / 100) -#define FEET_PER_MILE 5280 -#define FEET_PER_KILOFEET 1000 // Used for altitude -#define METERS_PER_KILOMETER 1000 -#define METERS_PER_MILE 1609 +#define GFORCE_FILTER_TC 0.2 #define DELAYED_REFRESH_RESUME_COMMAND (checkStickPosition(THR_HI) || checkStickPosition(PIT_HI)) @@ -137,6 +136,8 @@ static unsigned currentLayout = 0; static int layoutOverride = -1; static bool hasExtendedFont = false; // Wether the font supports characters > 256 static timeMs_t layoutOverrideUntil = 0; +static pt1Filter_t GForceFilter, GForceFilterAxis[XYZ_AXIS_COUNT]; +static float GForce, GForceAxis[XYZ_AXIS_COUNT]; typedef struct statistic_s { uint16_t max_speed; @@ -145,7 +146,7 @@ typedef struct statistic_s { int16_t max_power; // /100 int16_t min_rssi; int32_t max_altitude; - uint16_t max_distance; + uint32_t max_distance; } statistic_t; static statistic_t stats; @@ -170,6 +171,13 @@ static bool fullRedraw = false; static uint8_t armState; +typedef struct osdMapData_s { + uint32_t scale; + char referenceSymbol; +} osdMapData_t; + +static osdMapData_t osdMapData; + static displayPort_t *osdDisplayPort; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) @@ -181,7 +189,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, 7); +PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 9); static int digitCount(int32_t value) { @@ -203,8 +211,8 @@ static int digitCount(int32_t value) * of the same length. If the value doesn't fit into the provided length * it will be divided by scale and true will be returned. */ - static bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length) - { +bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length) +{ char *ptr = buff; char *dec; int decimals = maxDecimals; @@ -278,6 +286,18 @@ static int digitCount(int32_t value) return scaled; } +/* + * Aligns text to the left side. Adds spaces at the end to keep string length unchanged. + */ +static void osdLeftAlignString(char *buff) +{ + uint8_t sp = 0, ch = 0; + uint8_t len = strlen(buff); + while (buff[sp] == ' ') sp++; + for (ch = 0; ch < (len - sp); ch++) buff[ch] = buff[ch + sp]; + for (sp = ch; sp < len; sp++) buff[sp] = ' '; +} + /** * Converts distance into a string based on the current unit system * prefixed by a a symbol to indicate the unit used. @@ -287,20 +307,22 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist) { switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff + 1, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, 0, 3, 3)) { - buff[0] = SYM_DIST_MI; + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, 0, 3, 3)) { + buff[3] = SYM_DIST_MI; } else { - buff[0] = SYM_DIST_FT; + buff[3] = SYM_DIST_FT; } + buff[4] = '\0'; break; case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_METRIC: - if (osdFormatCentiNumber(buff + 1, dist, METERS_PER_KILOMETER, 0, 3, 3)) { - buff[0] = SYM_DIST_KM; + if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, 0, 3, 3)) { + buff[3] = SYM_DIST_KM; } else { - buff[0] = SYM_DIST_M; + buff[3] = SYM_DIST_M; } + buff[4] = '\0'; break; } } @@ -309,35 +331,35 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist) * Converts distance into a string based on the current unit system. * @param dist Distance in centimeters */ - static void osdFormatDistanceStr(char *buff, int32_t dist) - { - int32_t centifeet; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_IMPERIAL: - centifeet = CENTIMETERS_TO_CENTIFEET(dist); - if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { - // Show feet when dist < 0.5mi - tfp_sprintf(buff, "%d%c", centifeet / 100, SYM_FT); - } else { - // Show miles when dist >= 0.5mi - tfp_sprintf(buff, "%d.%02d%c", centifeet / (100*FEET_PER_MILE), - (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI); - } - break; - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_METRIC: - if (abs(dist) < METERS_PER_KILOMETER * 100) { - // Show meters when dist < 1km - tfp_sprintf(buff, "%d%c", dist / 100, SYM_M); - } else { - // Show kilometers when dist >= 1km - tfp_sprintf(buff, "%d.%02d%c", dist / (100*METERS_PER_KILOMETER), - (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM); - } - break; +static void osdFormatDistanceStr(char *buff, int32_t dist) +{ + int32_t centifeet; + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_IMPERIAL: + centifeet = CENTIMETERS_TO_CENTIFEET(dist); + if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { + // Show feet when dist < 0.5mi + tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); + } else { + // Show miles when dist >= 0.5mi + tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)), + (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI); + } + break; + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (abs(dist) < METERS_PER_KILOMETER * 100) { + // Show meters when dist < 1km + tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M); + } else { + // Show kilometers when dist >= 1km + tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)), + (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM); } + break; } +} /** * Converts velocity based on the current unit system (kmh or mph). @@ -361,16 +383,16 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) * Converts velocity into a string based on the current unit system. * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds) */ -static void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D) +void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D) { switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - tfp_sprintf(buff, "%3d%c", osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); + tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); break; case OSD_UNIT_METRIC: - tfp_sprintf(buff, "%3d%c", osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); + tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); break; } } @@ -413,29 +435,31 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) * prefixed by a a symbol to indicate the unit used. * @param alt Raw altitude/distance (i.e. as taken from baro.BaroAlt in centimeters) */ -static void osdFormatAltitudeSymbol(char *buff, int32_t alt) +void osdFormatAltitudeSymbol(char *buff, int32_t alt) { switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff + 1, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, 3)) { + if (osdFormatCentiNumber(buff , CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, 3)) { // Scaled to kft - buff[0] = SYM_ALT_KFT; + buff[3] = SYM_ALT_KFT; } else { // Formatted in feet - buff[0] = SYM_ALT_FT; + buff[3] = SYM_ALT_FT; } + buff[4] = '\0'; break; - case OSD_UNIT_UK: - FALLTHROUGH; case OSD_UNIT_METRIC: // alt is alredy in cm - if (osdFormatCentiNumber(buff+1, alt, 1000, 0, 2, 3)) { + if (osdFormatCentiNumber(buff, alt, 1000, 0, 2, 3)) { // Scaled to km - buff[0] = SYM_ALT_KM; + buff[3] = SYM_ALT_KM; } else { // Formatted in m - buff[0] = SYM_ALT_M; + buff[3] = SYM_ALT_M; } + buff[4] = '\0'; break; } } @@ -450,13 +474,13 @@ static void osdFormatAltitudeStr(char *buff, int32_t alt) switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_IMPERIAL: value = CENTIMETERS_TO_FEET(alt); - tfp_sprintf(buff, "%d%c", value, SYM_FT); + tfp_sprintf(buff, "%d%c", (int)value, SYM_FT); break; case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_METRIC: value = CENTIMETERS_TO_METERS(alt); - tfp_sprintf(buff, "%d%c", value, SYM_M); + tfp_sprintf(buff, "%d%c", (int)value, SYM_M); break; } } @@ -471,7 +495,7 @@ static void osdFormatTime(char *buff, uint32_t seconds, char sym_m, char sym_h) value = seconds / 60; } buff[0] = sym; - tfp_sprintf(buff + 1, "%02d:%02d", value / 60, value % 60); + tfp_sprintf(buff + 1, "%02d:%02d", (int)(value / 60), (int)(value % 60)); } static inline void osdFormatOnTime(char *buff) @@ -499,6 +523,14 @@ static uint16_t osdConvertRSSI(void) return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); } +static void osdGetVTXPowerChar(char *buff) +{ + buff[0] = '-'; + buff[1] = '\0'; + uint8_t powerIndex = 0; + if (vtxCommonGetPowerIndex(vtxCommonDevice(), &powerIndex)) buff[0] = '0' + powerIndex; +} + /** * Displays a temperature postfixed with a symbol depending on the current unit system * @param label to display @@ -565,11 +597,11 @@ static void osdFormatCoordinate(char *buff, char sym, int32_t val) int32_t integerPart = val / GPS_DEGREES_DIVIDER; // Latitude maximum integer width is 3 (-90) while // longitude maximum integer width is 4 (-180). - int integerDigits = tfp_sprintf(buff + 1, (integerPart == 0 && val < 0) ? "-%d" : "%d", integerPart); + int integerDigits = tfp_sprintf(buff + 1, (integerPart == 0 && val < 0) ? "-%d" : "%d", (int)integerPart); // We can show up to 7 digits in decimalPart. int32_t decimalPart = abs(val % GPS_DEGREES_DIVIDER); STATIC_ASSERT(GPS_DEGREES_DIVIDER == 1e7, adjust_max_decimal_digits); - int decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%07d", decimalPart); + int decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%07d", (int)decimalPart); // Embbed the decimal separator buff[1 + integerDigits - 1] += SYM_ZERO_HALF_TRAILING_DOT - '0'; buff[1 + integerDigits] += SYM_ZERO_HALF_LEADING_DOT - '0'; @@ -593,10 +625,6 @@ static const char * osdArmingDisabledReasonMessage(void) // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) { if (failsafeIsReceivingRxData()) { - if (isUsingSticksForArming()) { - // Need to power-cycle - return OSD_MESSAGE_STR("POWER CYCLE TO ARM"); - } // If we're not using sticks, it means the ARM switch // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING // yet @@ -613,7 +641,20 @@ static const char * osdArmingDisabledReasonMessage(void) case ARMING_DISABLED_SYSTEM_OVERLOADED: return OSD_MESSAGE_STR("SYSTEM OVERLOADED"); case ARMING_DISABLED_NAVIGATION_UNSAFE: - return OSD_MESSAGE_STR("NAVIGATION IS UNSAFE"); +#if defined(USE_NAV) + // Check the exact reason + switch (navigationIsBlockingArming(NULL)) { + case NAV_ARMING_BLOCKER_NONE: + break; + case NAV_ARMING_BLOCKER_MISSING_GPS_FIX: + return OSD_MESSAGE_STR("WAITING FOR GPS FIX"); + case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE: + return OSD_MESSAGE_STR("DISABLE NAVIGATION FIRST"); + case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR: + return OSD_MESSAGE_STR("FIRST WAYPOINT IS TOO FAR"); + } +#endif + break; case ARMING_DISABLED_COMPASS_NOT_CALIBRATED: return OSD_MESSAGE_STR("COMPASS NOT CALIBRATED"); case ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED: @@ -663,6 +704,8 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR("INVALID SETTING"); case ARMING_DISABLED_CLI: return OSD_MESSAGE_STR("CLI IS ACTIVE"); + case ARMING_DISABLED_PWM_OUTPUT_ERROR: + return OSD_MESSAGE_STR("PWM INIT ERROR"); // Cases without message case ARMING_DISABLED_CMS_MENU: FALLTHROUGH; @@ -807,21 +850,11 @@ static void osdUpdateBatteryCapacityOrVoltageTextAttributes(textAttributes_t *at TEXT_ATTRIBUTES_ADD_BLINK(*attr); } -static void osdCrosshairsBounds(uint8_t *x, uint8_t *y, uint8_t *length) +void osdCrosshairPosition(uint8_t *x, uint8_t *y) { - *x = 14 - 1; // Offset for 1 char to the left - *y = 6; - if (IS_DISPLAY_PAL) { - ++(*y); - } - int size = 3; - if (osdConfig()->crosshairs_style == OSD_CROSSHAIRS_STYLE_AIRCRAFT) { - (*x)--; - size = 5; - } - if (length) { - *length = size; - } + *x = osdDisplayPort->cols / 2; + *y = osdDisplayPort->rows / 2; + *y += osdConfig()->horizon_offset; } /** @@ -834,7 +867,7 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t { buff[0] = SYM_BLANK; buff[1] = SYM_THR; - int16_t thr = rcData[THROTTLE]; + int16_t thr = rxGetChannelValue(THROTTLE); if (autoThr && navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; @@ -845,7 +878,7 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t tfp_sprintf(buff + 2, "%3d", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); } -static inline int32_t osdGetAltitude(void) +int32_t osdGetAltitude(void) { #if defined(USE_NAV) return getEstimatedActualPosition(Z); @@ -936,7 +969,7 @@ static bool osdIsHeadingValid(void) return isImuHeadingValid(); } -static int16_t osdGetHeading(void) +int16_t osdGetHeading(void) { return attitude.values.yaw; } @@ -969,15 +1002,13 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente { // TODO: These need to be tested with several setups. We might // need to make them configurable. - const int hMargin = 1; - const int vMargin = 1; + const int hMargin = 5; + const int vMargin = 3; // TODO: Get this from the display driver? const int charWidth = 12; const int charHeight = 18; - char buf[16]; - uint8_t minX = hMargin; uint8_t maxX = osdDisplayPort->cols - 1 - hMargin; uint8_t minY = vMargin; @@ -986,11 +1017,6 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente uint8_t midY = osdDisplayPort->rows / 2; // Fixed marks - if (referenceSym) { - displayWriteChar(osdDisplayPort, maxX, minY, SYM_DIRECTION); - displayWriteChar(osdDisplayPort, maxX, minY + 1, referenceSym); - } - displayWriteChar(osdDisplayPort, minX, maxY, SYM_SCALE); displayWriteChar(osdDisplayPort, midX, midY, centerSym); // First, erase the previous drawing. @@ -1000,11 +1026,6 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente } uint32_t initialScale; - float scaleToUnit; - int scaleUnitDivisor; - char symUnscaled; - char symScaled; - int maxDecimals; const unsigned scaleMultiplier = 2; // We try to reduce the scale when the POI will be around half the distance // between the center and the closers map edge, to avoid too much jumping @@ -1013,21 +1034,11 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente switch (osdConfig()->units) { case OSD_UNIT_IMPERIAL: initialScale = 16; // 16m ~= 0.01miles - scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber() - scaleUnitDivisor = 0; - symUnscaled = SYM_MI; - symScaled = SYM_MI; - maxDecimals = 2; break; case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_METRIC: initialScale = 10; // 10m as initial scale - scaleToUnit = 100; // scale to cm for osdFormatCentiNumber() - scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m - symUnscaled = SYM_M; - symScaled = SYM_KM; - maxDecimals = 0; break; } @@ -1110,12 +1121,11 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente } } - // Draw the used scale - bool scaled = osdFormatCentiNumber(buf, scale * scaleToUnit, scaleUnitDivisor, maxDecimals, 2, 3); - buf[3] = scaled ? symScaled : symUnscaled; - buf[4] = '\0'; - displayWrite(osdDisplayPort, minX + 1, maxY, buf); *usedScale = scale; + + // Update global map data for scale and reference + osdMapData.scale = scale; + osdMapData.referenceSymbol = referenceSym; } /* Draws a map with the home in the center and the craft moving around. @@ -1170,8 +1180,10 @@ static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_ displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - osdFormatCentiNumber(buff, voltage, 0, decimals, 0, MIN(digits, 4)); - strcat(buff, "V"); + digits = MIN(digits, 4); + osdFormatCentiNumber(buff, voltage, 0, decimals, 0, digits); + buff[digits] = SYM_VOLT; + buff[digits+1] = '\0'; if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getBatteryVoltage() <= getBatteryWarningVoltage())) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); @@ -1221,7 +1233,6 @@ static bool osdDrawSingleElement(uint8_t item) if (!OSD_VISIBLE(pos)) { return false; } - uint8_t elemPosX = OSD_X(pos); uint8_t elemPosY = OSD_Y(pos); textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; @@ -1248,34 +1259,44 @@ static bool osdDrawSingleElement(uint8_t item) return true; case OSD_CURRENT_DRAW: - buff[0] = SYM_AMP; - osdFormatCentiNumber(buff + 1, getAmperage(), 0, 2, 0, 3); + osdFormatCentiNumber(buff, getAmperage(), 0, 2, 0, 3); + buff[3] = SYM_AMP; + buff[4] = '\0'; + + uint8_t current_alarm = osdConfig()->current_alarm; + if ((current_alarm > 0) && ((getAmperage() / 100.0f) > current_alarm)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } break; case OSD_MAH_DRAWN: - buff[0] = SYM_MAH; - tfp_sprintf(buff + 1, "%-4d", getMAhDrawn()); + tfp_sprintf(buff, "%4d", (int)getMAhDrawn()); + buff[4] = SYM_MAH; + buff[5] = '\0'; osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); break; case OSD_WH_DRAWN: - buff[0] = SYM_WH; - osdFormatCentiNumber(buff + 1, getMWhDrawn() / 10, 0, 2, 0, 3); + osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); + buff[3] = SYM_WH; + buff[4] = '\0'; break; case OSD_BATTERY_REMAINING_CAPACITY: - buff[0] = (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH); if (currentBatteryProfile->capacity.value == 0) - tfp_sprintf(buff + 1, "NA"); + tfp_sprintf(buff, " NA"); else if (!batteryWasFullWhenPluggedIn()) - tfp_sprintf(buff + 1, "NF"); + tfp_sprintf(buff, " NF"); else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) - tfp_sprintf(buff + 1, "%-4lu", getBatteryRemainingCapacity()); + tfp_sprintf(buff, "%4lu", getBatteryRemainingCapacity()); else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3); + buff[3] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH; + buff[4] = '\0'; + if ((getBatteryState() != BATTERY_NOT_PRESENT) && batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= currentBatteryProfile->capacity.warning - currentBatteryProfile->capacity.critical)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -1348,7 +1369,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = ARMING_FLAG(ARMED) ? '-' : SYM_ARROW_UP; TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } - buff[1] = 0; + buff[1] = '\0'; break; } @@ -1381,7 +1402,7 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_TRIP_DIST: - buff[0] = SYM_TRIP_DIST; + buff[0] = SYM_TOTAL; osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance()); break; @@ -1397,8 +1418,7 @@ static bool osdDrawSingleElement(uint8_t item) } else { buff[1] = buff[2] = buff[3] = '-'; } - buff[4] = SYM_DEGREES; - buff[5] = '\0'; + buff[4] = '\0'; break; } @@ -1499,8 +1519,8 @@ static bool osdDrawSingleElement(uint8_t item) int32_t alt = osdGetAltitudeMsl(); osdFormatAltitudeSymbol(buff, alt); break; - } - + } + case OSD_ONTIME: { osdFormatOnTime(buff); @@ -1562,12 +1582,13 @@ static bool osdDrawSingleElement(uint8_t item) } buff[0] = SYM_TRIP_DIST; if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) { - buff[1] = SYM_DIST_M; - strcpy(buff + 2, "---"); + buff[4] = SYM_DIST_M; + buff[5] = '\0'; + strcpy(buff + 1, "---"); } else if (distanceMeters == -2) { // Wind is too strong to come back with cruise throttle - buff[1] = SYM_DIST_M; - buff[2] = buff[3] = buff[4] = SYM_WIND_HORIZONTAL; + buff[1] = buff[2] = buff[3] = SYM_WIND_HORIZONTAL; + buff[4] = SYM_DIST_M; buff[5] = '\0'; TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else { @@ -1604,7 +1625,7 @@ static bool osdDrawSingleElement(uint8_t item) p = "ANGL"; else if (FLIGHT_MODE(HORIZON_MODE)) p = "HOR "; - else if (isAirmodeActive()) + else if (STATE(AIRMODE_ACTIVE)) p = "AIR "; displayWrite(osdDisplayPort, elemPosX, elemPosY, p); @@ -1638,43 +1659,73 @@ static bool osdDrawSingleElement(uint8_t item) { uint8_t band = 0; uint8_t channel = 0; - uint8_t powerIndex = 0; char bandChr = '-'; const char *channelStr = "-"; - char powerChr = '-'; - vtxDevice_t *vtxDevice = vtxCommonDevice(); - if (vtxDevice) { - if (vtxCommonGetBandAndChannel(vtxDevice, &band, &channel)) { - bandChr = vtx58BandLetter[band]; - channelStr = vtx58ChannelNames[channel]; - } - if (vtxCommonGetPowerIndex(vtxDevice, &powerIndex)) { - powerChr = '0' + powerIndex; - } + if (vtxCommonGetBandAndChannel(vtxCommonDevice(), &band, &channel)) { + bandChr = vtx58BandLetter[band]; + channelStr = vtx58ChannelNames[channel]; } - tfp_sprintf(buff, "CH:%c%s:%c", bandChr, channelStr, powerChr); + tfp_sprintf(buff, "CH:%c%s:", bandChr, channelStr); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + + osdGetVTXPowerChar(buff); + if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 6, elemPosY, buff, elemAttr); + return true; } #endif break; - case OSD_CROSSHAIRS: - osdCrosshairsBounds(&elemPosX, &elemPosY, NULL); - switch ((osd_crosshairs_style_e)osdConfig()->crosshairs_style) { - case OSD_CROSSHAIRS_STYLE_DEFAULT: - buff[0] = SYM_AH_CH_LEFT; - buff[1] = SYM_AH_CH_CENTER; - buff[2] = SYM_AH_CH_RIGHT; - buff[3] = '\0'; - break; - case OSD_CROSSHAIRS_STYLE_AIRCRAFT: - buff[0] = SYM_AH_CH_AIRCRAFT0; - buff[1] = SYM_AH_CH_AIRCRAFT1; - buff[2] = SYM_AH_CH_AIRCRAFT2; - buff[3] = SYM_AH_CH_AIRCRAFT3; - buff[4] = SYM_AH_CH_AIRCRAFT4; - buff[5] = '\0'; - break; + case OSD_VTX_POWER: + { + osdGetVTXPowerChar(buff); + if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + return true; } + + case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair + + osdCrosshairPosition(&elemPosX, &elemPosY); + osdHudDrawCrosshair(elemPosX, elemPosY); + + if (osdConfig()->hud_homing && STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + osdHudDrawHoming(elemPosX, elemPosY); + } + + if (STATE(GPS_FIX) && isImuHeadingValid()) { + + if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0) { + osdHudClear(); + } + + if (osdConfig()->hud_homepoint) { // Display the home point (H) + osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, 5, SYM_HOME); + } + + if (osdConfig()->hud_radar_disp > 0) { // Display the POI from the radar + for (uint8_t i = 0; i < osdConfig()->hud_radar_disp; i++) { + fpVector3_t poi; + geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &radar_pois[i].gps, GEO_ALT_RELATIVE); + radar_pois[i].distance = calculateDistanceToDestination(&poi) / 100; // In meters + + if ((radar_pois[i].distance >= (osdConfig()->hud_radar_range_min)) && (radar_pois[i].distance <= (osdConfig()->hud_radar_range_max)) && (radar_pois[i].state != 2)) { + radar_pois[i].direction = calculateBearingToDestination(&poi) / 100; // In ° + radar_pois[i].altitude = (radar_pois[i].gps.alt - osdGetAltitudeMsl()) / 100; + osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, radar_pois[i].heading, radar_pois[i].lq, 65 + i); + } + } + + if (osdConfig()->hud_radar_nearest > 0) { // Display extra datas for 1 POI closer than a set distance + int poi_id = radarGetNearestPOI(); + if (poi_id >= 0 && radar_pois[poi_id].distance <= osdConfig()->hud_radar_nearest) { + osdHudDrawExtras(poi_id); + } + } + } + } + + return true; break; case OSD_ATTITUDE_ROLL: @@ -1696,9 +1747,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_ARTIFICIAL_HORIZON: { - - elemPosX = 14; - elemPosY = 6; // Center of the AH area + osdCrosshairPosition(&elemPosX, &elemPosY); // Store the positions we draw over to erase only these at the next iteration static int8_t previous_written[AH_PREV_SIZE]; @@ -1713,10 +1762,6 @@ static bool osdDrawSingleElement(uint8_t item) rollAngle = -rollAngle; } - if (IS_DISPLAY_PAL) { - ++elemPosY; - } - float ky = sin_approx(rollAngle); float kx = cos_approx(rollAngle); @@ -1775,12 +1820,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HORIZON_SIDEBARS: { - elemPosX = 14; - elemPosY = 6; - - if (IS_DISPLAY_PAL) { - ++elemPosY; - } + osdCrosshairPosition(&elemPosX, &elemPosY); static osd_sidebar_t left; static osd_sidebar_t right; @@ -1814,8 +1854,8 @@ static bool osdDrawSingleElement(uint8_t item) } // AH level indicators - displayWriteChar(osdDisplayPort, elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT); - displayWriteChar(osdDisplayPort, elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT); + displayWriteChar(osdDisplayPort, elemPosX - hudwidth + 1, elemPosY, SYM_AH_RIGHT); + displayWriteChar(osdDisplayPort, elemPosX + hudwidth - 1, elemPosY, SYM_AH_LEFT); return true; } @@ -1864,13 +1904,13 @@ static bool osdDrawSingleElement(uint8_t item) int16_t value = getEstimatedActualVelocity(Z); char sym; switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; case OSD_UNIT_IMPERIAL: // Convert to centifeet/s value = CENTIMETERS_TO_CENTIFEET(value); sym = SYM_FTS; break; - case OSD_UNIT_UK: - FALLTHROUGH; case OSD_UNIT_METRIC: // Already in cm/s sym = SYM_MS; @@ -1909,12 +1949,12 @@ static bool osdDrawSingleElement(uint8_t item) return true; case OSD_VEL_XY_PIDS: - if (STATE(FIXED_WING)) + if (!STATE(FIXED_WING)) osdDisplayPIDValues(elemPosX, elemPosY, "VXY", &pidBank()->pid[PID_VEL_XY], ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D); return true; case OSD_VEL_Z_PIDS: - if (STATE(FIXED_WING)) + if (!STATE(FIXED_WING)) osdDisplayPIDValues(elemPosX, elemPosY, "VZ", &pidBank()->pid[PID_VEL_Z], ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D); return true; @@ -2050,19 +2090,20 @@ static bool osdDrawSingleElement(uint8_t item) const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); strcpy(buff, "POSO "); // display requested velocity cm/s - tfp_sprintf(buff + 5, "%4d", lrintf(nav_pids->pos[X].output_constrained * 100)); + tfp_sprintf(buff + 5, "%4d", (int)lrintf(nav_pids->pos[X].output_constrained * 100)); buff[9] = ' '; - tfp_sprintf(buff + 10, "%4d", lrintf(nav_pids->pos[Y].output_constrained * 100)); + tfp_sprintf(buff + 10, "%4d", (int)lrintf(nav_pids->pos[Y].output_constrained * 100)); buff[14] = ' '; - tfp_sprintf(buff + 15, "%4d", lrintf(nav_pids->pos[Z].output_constrained * 100)); + tfp_sprintf(buff + 15, "%4d", (int)lrintf(nav_pids->pos[Z].output_constrained * 100)); buff[19] = '\0'; break; } case OSD_POWER: { - buff[0] = SYM_WATT; - osdFormatCentiNumber(buff + 1, getPower(), 0, 2, 0, 3); + osdFormatCentiNumber(buff, getPower(), 0, 2, 0, 3); + buff[3] = SYM_WATT; + buff[4] = '\0'; break; } @@ -2272,7 +2313,7 @@ static bool osdDrawSingleElement(uint8_t item) } } if (value > 0 && value <= 999) { - tfp_sprintf(buff, "%3d", value); + tfp_sprintf(buff, "%3d", (int)value); } else { buff[0] = buff[1] = buff[2] = '-'; } @@ -2313,13 +2354,36 @@ static bool osdDrawSingleElement(uint8_t item) break; } + case OSD_GFORCE: + { + buff[0] = SYM_GFORCE; + osdFormatCentiNumber(buff + 1, GForce, 0, 2, 0, 3); + if (GForce > osdConfig()->gforce_alarm * 100) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } + + case OSD_GFORCE_X: + case OSD_GFORCE_Y: + case OSD_GFORCE_Z: + { + float GForceValue = GForceAxis[item - OSD_GFORCE_X]; + buff[0] = SYM_GFORCE_X + item - OSD_GFORCE_X; + osdFormatCentiNumber(buff + 1, GForceValue, 0, 2, 0, 4); + if ((GForceValue < osdConfig()->gforce_axis_alarm_min * 100) || (GForceValue > osdConfig()->gforce_axis_alarm_max * 100)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } + case OSD_DEBUG: { - // Longest representable string is -32768, hence 6 characters - tfp_sprintf(buff, "[0]=%6d [1]=%6d", debug[0], debug[1]); - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - elemPosY++; - tfp_sprintf(buff, "[2]=%6d [3]=%6d", debug[2], debug[3]); + // Longest representable string is -2147483648, hence 11 characters + for (uint8_t index; index < DEBUG32_VALUE_COUNT; ++elemPosY, index += 2) { + tfp_sprintf(buff, "[%u]=%11ld [%u]=%11ld", index, debug[index], index+1, debug[index+1]); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + } break; } @@ -2341,50 +2405,15 @@ static bool osdDrawSingleElement(uint8_t item) #ifdef USE_TEMPERATURE_SENSOR case OSD_TEMP_SENSOR_0_TEMPERATURE: - { - osdDisplayTemperatureSensor(elemPosX, elemPosY, 0); - return true; - } - case OSD_TEMP_SENSOR_1_TEMPERATURE: - { - osdDisplayTemperatureSensor(elemPosX, elemPosY, 1); - return true; - } - case OSD_TEMP_SENSOR_2_TEMPERATURE: - { - osdDisplayTemperatureSensor(elemPosX, elemPosY, 2); - return true; - } - case OSD_TEMP_SENSOR_3_TEMPERATURE: - { - osdDisplayTemperatureSensor(elemPosX, elemPosY, 3); - return true; - } - case OSD_TEMP_SENSOR_4_TEMPERATURE: - { - osdDisplayTemperatureSensor(elemPosX, elemPosY, 4); - return true; - } - case OSD_TEMP_SENSOR_5_TEMPERATURE: - { - osdDisplayTemperatureSensor(elemPosX, elemPosY, 5); - return true; - } - case OSD_TEMP_SENSOR_6_TEMPERATURE: - { - osdDisplayTemperatureSensor(elemPosX, elemPosY, 6); - return true; - } - case OSD_TEMP_SENSOR_7_TEMPERATURE: { - osdDisplayTemperatureSensor(elemPosX, elemPosY, 7); + osdDisplayTemperatureSensor(elemPosX, elemPosY, item - OSD_TEMP_SENSOR_0_TEMPERATURE); return true; } #endif /* ifdef USE_TEMPERATURE_SENSOR */ @@ -2452,6 +2481,69 @@ static bool osdDrawSingleElement(uint8_t item) break; } + case OSD_MAP_SCALE: + { + float scaleToUnit; + int scaleUnitDivisor; + char symUnscaled; + char symScaled; + int maxDecimals; + + switch (osdConfig()->units) { + case OSD_UNIT_IMPERIAL: + scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber() + scaleUnitDivisor = 0; + symUnscaled = SYM_MI; + symScaled = SYM_MI; + maxDecimals = 2; + break; + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_METRIC: + scaleToUnit = 100; // scale to cm for osdFormatCentiNumber() + scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m + symUnscaled = SYM_M; + symScaled = SYM_KM; + maxDecimals = 0; + break; + } + buff[0] = SYM_SCALE; + if (osdMapData.scale > 0) { + bool scaled = osdFormatCentiNumber(&buff[1], osdMapData.scale * scaleToUnit, scaleUnitDivisor, maxDecimals, 2, 3); + buff[4] = scaled ? symScaled : symUnscaled; + // Make sure this is cleared if the map stops being drawn + osdMapData.scale = 0; + } else { + memset(&buff[1], '-', 4); + } + buff[5] = '\0'; + break; + } + case OSD_MAP_REFERENCE: + { + char referenceSymbol; + if (osdMapData.referenceSymbol) { + referenceSymbol = osdMapData.referenceSymbol; + // Make sure this is cleared if the map stops being drawn + osdMapData.referenceSymbol = 0; + } else { + referenceSymbol = '-'; + } + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DIRECTION); + displayWriteChar(osdDisplayPort, elemPosX, elemPosY + 1, referenceSymbol); + return true; + } + +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + case OSD_RC_SOURCE: + { + const char *source_text = IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && !mspOverrideIsInFailsafe() ? "MSP" : "STD"; + if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && mspOverrideIsInFailsafe()) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, source_text, elemAttr); + return true; + } +#endif + default: return false; } @@ -2512,7 +2604,7 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex) if (elementIndex == OSD_TRIP_DIST) { elementIndex = OSD_ATTITUDE_PITCH; } - if (elementIndex == OSD_MAP_NORTH) { + if (elementIndex == OSD_WIND_SPEED_HORIZONTAL) { elementIndex = OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE; } if (elementIndex == OSD_3D_SPEED) { @@ -2560,7 +2652,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[0][OSD_CRUISE_HEADING_ERROR] = OSD_POS(12, 2); osdConfig->item_pos[0][OSD_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2); osdConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2); - osdConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(1, 3) | OSD_VISIBLE_FLAG; + osdConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG; osdConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG; osdConfig->item_pos[0][OSD_WH_DRAWN] = OSD_POS(1, 5); osdConfig->item_pos[0][OSD_BATTERY_REMAINING_CAPACITY] = OSD_POS(1, 6); @@ -2650,6 +2742,17 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[0][OSD_WIND_SPEED_HORIZONTAL] = OSD_POS(3, 6); osdConfig->item_pos[0][OSD_WIND_SPEED_VERTICAL] = OSD_POS(3, 7); + osdConfig->item_pos[0][OSD_GFORCE] = OSD_POS(12, 4); + osdConfig->item_pos[0][OSD_GFORCE_X] = OSD_POS(12, 5); + osdConfig->item_pos[0][OSD_GFORCE_Y] = OSD_POS(12, 6); + osdConfig->item_pos[0][OSD_GFORCE_Z] = OSD_POS(12, 7); + + osdConfig->item_pos[0][OSD_VTX_POWER] = OSD_POS(3, 5); + +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + osdConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4); +#endif + // Under OSD_FLYMODE. TODO: Might not be visible on NTSC? osdConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG; @@ -2664,8 +2767,12 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->alt_alarm = 100; osdConfig->dist_alarm = 1000; osdConfig->neg_alt_alarm = 5; + osdConfig->current_alarm = 0; osdConfig->imu_temp_alarm_min = -200; osdConfig->imu_temp_alarm_max = 600; + osdConfig->gforce_alarm = 5; + osdConfig->gforce_axis_alarm_min = -5; + osdConfig->gforce_axis_alarm_max = 5; #ifdef USE_BARO osdConfig->baro_temp_alarm_min = -200; osdConfig->baro_temp_alarm_max = 600; @@ -2680,6 +2787,18 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->ahi_reverse_roll = 0; osdConfig->ahi_max_pitch = AH_MAX_PITCH_DEFAULT; osdConfig->crosshairs_style = OSD_CROSSHAIRS_STYLE_DEFAULT; + osdConfig->horizon_offset = 0; + osdConfig->camera_uptilt = 0; + osdConfig->camera_fov_h = 135; + osdConfig->camera_fov_v = 85; + osdConfig->hud_margin_h = 3; + osdConfig->hud_margin_v = 3; + osdConfig->hud_homing = 0; + osdConfig->hud_homepoint = 0; + osdConfig->hud_radar_disp = 0; + osdConfig->hud_radar_range_min = 1; + osdConfig->hud_radar_range_max = 4000; + osdConfig->hud_radar_nearest = 0; osdConfig->left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE; osdConfig->right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE; osdConfig->sidebar_scroll_arrows = 0; @@ -2755,10 +2874,10 @@ void osdInit(displayPort_t *osdDisplayPortToUse) if (statsConfig()->stats_enabled) { displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "ODOMETER:"); if (osdConfig()->units == OSD_UNIT_IMPERIAL) { - tfp_sprintf(string_buffer, "%5d", statsConfig()->stats_total_dist / METERS_PER_MILE); + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE)); string_buffer[5] = SYM_MI; } else { - tfp_sprintf(string_buffer, "%5d", statsConfig()->stats_total_dist / METERS_PER_KILOMETER); + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); string_buffer[5] = SYM_KM; } string_buffer[6] = '\0'; @@ -2766,7 +2885,7 @@ void osdInit(displayPort_t *osdDisplayPortToUse) displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "TOTAL TIME:"); uint32_t tot_mins = statsConfig()->stats_total_time / 60; - tfp_sprintf(string_buffer, "%2d:%02dHM", tot_mins / 60, tot_mins % 60); + tfp_sprintf(string_buffer, "%2d:%02dHM", (int)(tot_mins / 60), (int)(tot_mins % 60)); displayWrite(osdDisplayPort, STATS_VALUE_X_POS-5, y, string_buffer); #ifdef USE_ADC @@ -2853,6 +2972,7 @@ static void osdShowStats(void) if (STATE(GPS_FIX)) { displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); osdFormatVelocityStr(buff, stats.max_speed, true); + osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :"); @@ -2871,6 +2991,7 @@ static void osdShowStats(void) displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); strcat(buff, "V"); + osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); @@ -2891,7 +3012,7 @@ static void osdShowStats(void) if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { displayWrite(osdDisplayPort, statNameX, top, "USED MAH :"); - tfp_sprintf(buff, "%d%c", getMAhDrawn(), SYM_MAH); + tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); } else { displayWrite(osdDisplayPort, statNameX, top, "USED WH :"); osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); @@ -2903,7 +3024,7 @@ static void osdShowStats(void) if (totalDistance > 0) { displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) - tfp_sprintf(buff, "%d%c%c", getMAhDrawn() * 100000 / totalDistance, + tfp_sprintf(buff, "%d%c%c", (int)(getMAhDrawn() * 100000 / totalDistance), SYM_MAH_KM_0, SYM_MAH_KM_1); else { osdFormatCentiNumber(buff, getMWhDrawn() * 10000 / totalDistance, 0, 2, 0, 3); @@ -2924,6 +3045,19 @@ static void osdShowStats(void) tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); displayWrite(osdDisplayPort, statValuesX, top++, buff); + const float max_gforce = accGetMeasuredMaxG(); + displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); + osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); + displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); + osdFormatCentiNumber(buff, acc_extremes[Z].min * 100, 0, 2, 0, 4); + strcat(buff,"/"); + displayWrite(osdDisplayPort, statValuesX, top, buff); + osdFormatCentiNumber(buff, acc_extremes[Z].max * 100, 0, 2, 0, 3); + displayWrite(osdDisplayPort, statValuesX + 5, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); } @@ -2951,7 +3085,7 @@ static void osdShowArmed(void) displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf); int digits = osdConfig()->plus_code_digits; olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf); y += 4; } else { strcpy(buf, "!NO HOME POSITION!"); @@ -2970,8 +3104,33 @@ static void osdShowArmed(void) } } +static void osdFilterData(timeUs_t currentTimeUs) { + static timeUs_t lastRefresh = 0; + float refresh_dT = cmpTimeUs(currentTimeUs, lastRefresh) * 1e-6; + + GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS; + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS; + + if (lastRefresh) { + GForce = pt1FilterApply3(&GForceFilter, GForce, refresh_dT); + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) pt1FilterApply3(GForceFilterAxis + axis, GForceAxis[axis], refresh_dT); + } else { + pt1FilterInitRC(&GForceFilter, GFORCE_FILTER_TC, 0); + pt1FilterReset(&GForceFilter, GForce); + + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) { + pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0); + pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]); + } + } + + lastRefresh = currentTimeUs; +} + static void osdRefresh(timeUs_t currentTimeUs) { + osdFilterData(currentTimeUs); + #ifdef USE_CMS if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) { #else @@ -3134,4 +3293,9 @@ bool osdItemIsFixed(osd_items_e item) item == OSD_HORIZON_SIDEBARS; } +displayPort_t *osdGetDisplayPort(void) +{ + return osdDisplayPort; +} + #endif // OSD diff --git a/src/main/io/osd.h b/src/main/io/osd.h index b65027cfb12..65cf0fcc00b 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -18,9 +18,10 @@ #pragma once #include "common/time.h" + #include "config/parameter_group.h" -#include "drivers/vcd.h" +#include "drivers/osd.h" #ifndef OSD_ALTERNATE_LAYOUT_COUNT #define OSD_ALTERNATE_LAYOUT_COUNT 3 @@ -35,6 +36,13 @@ #define OSD_POS_MAX 0x3FF #define OSD_POS_MAX_CLI (OSD_POS_MAX | OSD_VISIBLE_FLAG) +#define OSD_HOMING_LIM_H1 6 +#define OSD_HOMING_LIM_H2 16 +#define OSD_HOMING_LIM_H3 38 +#define OSD_HOMING_LIM_V1 5 +#define OSD_HOMING_LIM_V2 10 +#define OSD_HOMING_LIM_V3 15 + typedef enum { OSD_RSSI_VALUE, OSD_MAIN_BATT_VOLTAGE, @@ -134,6 +142,14 @@ typedef enum { OSD_TEMP_SENSOR_7_TEMPERATURE, OSD_ALTITUDE_MSL, OSD_PLUS_CODE, + OSD_MAP_SCALE, + OSD_MAP_REFERENCE, + OSD_GFORCE, + OSD_GFORCE_X, + OSD_GFORCE_Y, + OSD_GFORCE_Z, + OSD_RC_SOURCE, + OSD_VTX_POWER, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -151,6 +167,11 @@ typedef enum { typedef enum { OSD_CROSSHAIRS_STYLE_DEFAULT, OSD_CROSSHAIRS_STYLE_AIRCRAFT, + OSD_CROSSHAIRS_STYLE_TYPE3, + OSD_CROSSHAIRS_STYLE_TYPE4, + OSD_CROSSHAIRS_STYLE_TYPE5, + OSD_CROSSHAIRS_STYLE_TYPE6, + OSD_CROSSHAIRS_STYLE_TYPE7, } osd_crosshairs_style_e; typedef enum { @@ -175,8 +196,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 + uint8_t current_alarm; // current consumption in A int16_t imu_temp_alarm_min; int16_t imu_temp_alarm_max; + float gforce_alarm; + float gforce_axis_alarm_min; + float gforce_axis_alarm_max; #ifdef USE_BARO int16_t baro_temp_alarm_min; int16_t baro_temp_alarm_max; @@ -193,6 +218,19 @@ typedef struct osdConfig_s { uint8_t ahi_reverse_roll; uint8_t ahi_max_pitch; uint8_t crosshairs_style; // from osd_crosshairs_style_e + int8_t horizon_offset; + int8_t camera_uptilt; + uint8_t camera_fov_h; + uint8_t camera_fov_v; + uint8_t hud_margin_h; + uint8_t hud_margin_v; + bool hud_homing; + bool hud_homepoint; + uint8_t hud_radar_disp; + uint16_t hud_radar_range_min; + uint16_t hud_radar_range_max; + uint16_t hud_radar_nearest; + uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e uint8_t sidebar_scroll_arrows; @@ -209,8 +247,9 @@ typedef struct osdConfig_s { PG_DECLARE(osdConfig_t, osdConfig); -struct displayPort_s; -void osdInit(struct displayPort_s *osdDisplayPort); +typedef struct displayPort_s displayPort_t; + +void osdInit(displayPort_t *osdDisplayPort); void osdUpdate(timeUs_t currentTimeUs); void osdStartFullRedraw(void); // Sets a fixed OSD layout ignoring the RC input. Set it @@ -223,3 +262,12 @@ void osdOverrideLayout(int layout, timeMs_t duration); // set by the user configuration (modes, etc..) or by overriding it. int osdGetActiveLayout(bool *overridden); bool osdItemIsFixed(osd_items_e item); + +displayPort_t *osdGetDisplayPort(void); + +int16_t osdGetHeading(void); +int32_t osdGetAltitude(void); +void osdCrosshairPosition(uint8_t *x, uint8_t *y); +bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length); +void osdFormatAltitudeSymbol(char *buff, int32_t alt); +void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D); diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c new file mode 100644 index 00000000000..35c1f58078c --- /dev/null +++ b/src/main/io/osd_hud.c @@ -0,0 +1,340 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "common/constants.h" +#include "common/printf.h" + +#include "flight/imu.h" + +#include "io/osd.h" +#include "io/osd_hud.h" + +#include "drivers/display.h" +#include "drivers/osd.h" +#include "drivers/osd_symbols.h" +#include "drivers/time.h" + +#include "navigation/navigation.h" + +#ifdef USE_OSD + +#define HUD_DRAWN_MAXCHARS 40 // 6 POI (1 home, 5 radar) x 7 chars max for each minus 2 for H (no icons for heading and signal) + +static int8_t hud_drawn[HUD_DRAWN_MAXCHARS][2]; +static int8_t hud_drawn_pt; + +/* + * Overwrite all previously written positions on the OSD with a blank + */ +void osdHudClear(void) +{ + for (int i = 0; i < HUD_DRAWN_MAXCHARS; i++) { + if (hud_drawn[i][0] > -1) { + displayWriteChar(osdGetDisplayPort(), hud_drawn[i][0], hud_drawn[i][1], SYM_BLANK); + hud_drawn[i][0] = -1; + } + } + hud_drawn_pt = 0; +} + +/* + * Write a single char on the OSD, and record the position for the next loop + */ +static int osdHudWrite(uint8_t px, uint8_t py, uint16_t symb, bool crush) +{ + if (!crush) { + uint16_t c; + if (displayReadCharWithAttr(osdGetDisplayPort(), px, py, &c, NULL) && c != SYM_BLANK) { + return false; + } + } + + displayWriteChar(osdGetDisplayPort(), px, py, symb); + hud_drawn[hud_drawn_pt][0] = px; + hud_drawn[hud_drawn_pt][1] = py; + hud_drawn_pt++; + if (hud_drawn_pt >= HUD_DRAWN_MAXCHARS) hud_drawn_pt = 0; + return true; +} + +/* + * Constrain an angle between -180 and +180° + */ +static int16_t hudWrap180(int16_t angle) +{ + while (angle < -179) angle += 360; + while (angle > 180) angle -= 360; + return angle; +} + +/* + * Constrain an angle between 0 and +360° + */ +static int16_t hudWrap360(int16_t angle) +{ + while (angle < 0) angle += 360; + while (angle > 360) angle -= 360; + return angle; +} + +/* + * Radar, get the nearest POI + */ +int8_t radarGetNearestPOI(void) +{ + int8_t poi = -1; + uint16_t min = 10000; // 10kms + + for (int i = 0; i < RADAR_MAX_POIS; i++) { + if ((radar_pois[i].distance > 0) && (radar_pois[i].distance < min) && (radar_pois[i].state != 2)) { + min = radar_pois[i].distance; + poi = i; + } + } + return poi; +} + +/* + * Display one POI on the hud, centered on crosshair position. + * Distance (m), Direction (°), Altitude (relative, m, negative means below), Heading (°), Signal 0 to 5, Symbol 0 to 480 + */ +void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, int16_t poiHeading, uint8_t poiSignal, uint16_t poiSymbol) +{ + int poi_x; + int poi_y; + uint8_t center_x; + uint8_t center_y; + bool poi_is_oos = 0; + + uint8_t minX = osdConfig()->hud_margin_h + 2; + uint8_t maxX = osdGetDisplayPort()->cols - osdConfig()->hud_margin_h - 3; + uint8_t minY = osdConfig()->hud_margin_v; + uint8_t maxY = osdGetDisplayPort()->rows - osdConfig()->hud_margin_v - 2; + + osdCrosshairPosition(¢er_x, ¢er_y); + + int16_t error_x = hudWrap180(poiDirection - DECIDEGREES_TO_DEGREES(osdGetHeading())); + + if ((error_x > -(osdConfig()->camera_fov_h / 2)) && (error_x < osdConfig()->camera_fov_h / 2)) { // POI might be in sight, extra geometry needed + float scaled_x = sin_approx(DEGREES_TO_RADIANS(error_x)) / sin_approx(DEGREES_TO_RADIANS(osdConfig()->camera_fov_h / 2)); + poi_x = center_x + 15 * scaled_x; + + if (poi_x < minX || poi_x > maxX ) { // In camera view, but out of the hud area + poi_is_oos = 1; + } + else { // POI is on sight, compute the vertical + float poi_angle = atan2_approx(-poiAltitude, poiDistance); + poi_angle = RADIANS_TO_DEGREES(poi_angle); + int16_t plane_angle = attitude.values.pitch / 10; + int camera_angle = osdConfig()->camera_uptilt; + int16_t error_y = poi_angle - plane_angle + camera_angle; + float scaled_y = sin_approx(DEGREES_TO_RADIANS(error_y)) / sin_approx(DEGREES_TO_RADIANS(osdConfig()->camera_fov_v / 2)); + poi_y = constrain(center_y + (osdGetDisplayPort()->rows / 2) * scaled_y, minY, maxY - 1); + } + } + else { + poi_is_oos = 1; // POI is out of camera view for sure + } + + // Out-of-sight arrows and stacking + + if (poi_is_oos) { + uint16_t d; + uint16_t c; + + poi_x = (error_x > 0 ) ? maxX : minX; + poi_y = center_y; + + if (displayReadCharWithAttr(osdGetDisplayPort(), poi_x, poi_y, &c, NULL) && c != SYM_BLANK) { + poi_y = center_y - 2; + while (displayReadCharWithAttr(osdGetDisplayPort(), poi_x, poi_y, &c, NULL) && c != SYM_BLANK && poi_y < maxY - 3) { // Stacks the out-of-sight POI from top to bottom + poi_y += 2; + } + } + + if (error_x > 0 ) { + d = SYM_HUD_ARROWS_R3 - constrain ((180 - error_x) / 45, 0, 2); + osdHudWrite(poi_x + 2, poi_y, d, 1); + } + else { + d = SYM_HUD_ARROWS_L3 - constrain ((180 + error_x) / 45, 0, 2); + osdHudWrite(poi_x - 2, poi_y, d, 1); + } + } + + // POI marker (A B C ...) + + osdHudWrite(poi_x, poi_y, poiSymbol, 1); + + // Signal on the right, heading on the left + + if (poiSignal < 5) { // 0 to 4 = signal bars, 5 = No LQ and no heading displayed + error_x = hudWrap360(poiHeading - DECIDEGREES_TO_DEGREES(osdGetHeading())); + osdHudWrite(poi_x - 1, poi_y, SYM_DIRECTION + ((error_x + 22) / 45) % 8, 1); + osdHudWrite(poi_x + 1, poi_y, SYM_HUD_SIGNAL_0 + poiSignal, 1); + } + + // Distance + + char buff[3]; + if ((osd_unit_e)osdConfig()->units == OSD_UNIT_IMPERIAL) { + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3); + } + else { + osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 3, 3); + } + + osdHudWrite(poi_x - 1, poi_y + 1, buff[0], 1); + osdHudWrite(poi_x , poi_y + 1, buff[1], 1); + osdHudWrite(poi_x + 1, poi_y + 1, buff[2], 1); +} + +/* + * Draw the crosshair + */ +void osdHudDrawCrosshair(uint8_t px, uint8_t py) +{ + static const uint16_t crh_style_all[] = { + SYM_AH_CH_LEFT, SYM_AH_CH_CENTER, SYM_AH_CH_RIGHT, + SYM_AH_CH_AIRCRAFT1, SYM_AH_CH_AIRCRAFT2, SYM_AH_CH_AIRCRAFT3, + SYM_AH_CH_TYPE3, SYM_AH_CH_TYPE3 + 1, SYM_AH_CH_TYPE3 + 2, + SYM_AH_CH_TYPE4, SYM_AH_CH_TYPE4 + 1, SYM_AH_CH_TYPE4 + 2, + SYM_AH_CH_TYPE5, SYM_AH_CH_TYPE5 + 1, SYM_AH_CH_TYPE5 + 2, + SYM_AH_CH_TYPE6, SYM_AH_CH_TYPE6 + 1, SYM_AH_CH_TYPE6 + 2, + SYM_AH_CH_TYPE7, SYM_AH_CH_TYPE7 + 1, SYM_AH_CH_TYPE7 + 2, + }; + + uint8_t crh_crosshair = (osd_crosshairs_style_e)osdConfig()->crosshairs_style; + + displayWriteChar(osdGetDisplayPort(), px - 1, py,crh_style_all[crh_crosshair * 3]); + displayWriteChar(osdGetDisplayPort(), px, py, crh_style_all[crh_crosshair * 3 + 1]); + displayWriteChar(osdGetDisplayPort(), px + 1, py, crh_style_all[crh_crosshair * 3 + 2]); +} + + +/* + * Draw the homing arrows around the crosshair + */ +void osdHudDrawHoming(uint8_t px, uint8_t py) +{ + int crh_l = SYM_BLANK; + int crh_r = SYM_BLANK; + int crh_u = SYM_BLANK; + int crh_d = SYM_BLANK; + + int16_t crh_diff_head = hudWrap180(GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading())); + + if (crh_diff_head <= -162 || crh_diff_head >= 162) { + crh_l = SYM_HUD_ARROWS_L3; + crh_r = SYM_HUD_ARROWS_R3; + } else if (crh_diff_head > -162 && crh_diff_head <= -126) { + crh_l = SYM_HUD_ARROWS_L3; + crh_r = SYM_HUD_ARROWS_R2; + } else if (crh_diff_head > -126 && crh_diff_head <= -90) { + crh_l = SYM_HUD_ARROWS_L3; + crh_r = SYM_HUD_ARROWS_R1; + } else if (crh_diff_head > -90 && crh_diff_head <= -OSD_HOMING_LIM_H3) { + crh_l = SYM_HUD_ARROWS_L3; + } else if (crh_diff_head > -OSD_HOMING_LIM_H3 && crh_diff_head <= -OSD_HOMING_LIM_H2) { + crh_l = SYM_HUD_ARROWS_L2; + } else if (crh_diff_head > -OSD_HOMING_LIM_H2 && crh_diff_head <= -OSD_HOMING_LIM_H1) { + crh_l = SYM_HUD_ARROWS_L1; + } else if (crh_diff_head >= OSD_HOMING_LIM_H1 && crh_diff_head < OSD_HOMING_LIM_H2) { + crh_r = SYM_HUD_ARROWS_R1; + } else if (crh_diff_head >= OSD_HOMING_LIM_H2 && crh_diff_head < OSD_HOMING_LIM_H3) { + crh_r = SYM_HUD_ARROWS_R2; + } else if (crh_diff_head >= OSD_HOMING_LIM_H3 && crh_diff_head < 90) { + crh_r = SYM_HUD_ARROWS_R3; + } else if (crh_diff_head >= 90 && crh_diff_head < 126) { + crh_l = SYM_HUD_ARROWS_L1; + crh_r = SYM_HUD_ARROWS_R3; + } else if (crh_diff_head >= 126 && crh_diff_head < 162) { + crh_l = SYM_HUD_ARROWS_L2; + crh_r = SYM_HUD_ARROWS_R3; + } + + if (ABS(crh_diff_head) < 90) { + + int32_t crh_altitude = osdGetAltitude() / 100; + int32_t crh_distance = GPS_distanceToHome; + + float crh_home_angle = atan2_approx(crh_altitude, crh_distance); + crh_home_angle = RADIANS_TO_DEGREES(crh_home_angle); + int crh_plane_angle = attitude.values.pitch / 10; + int crh_camera_angle = osdConfig()->camera_uptilt; + int crh_diff_vert = crh_home_angle - crh_plane_angle + crh_camera_angle; + + if (crh_diff_vert > -OSD_HOMING_LIM_V2 && crh_diff_vert <= -OSD_HOMING_LIM_V1 ) { + crh_u = SYM_HUD_ARROWS_U1; + } else if (crh_diff_vert > -OSD_HOMING_LIM_V3 && crh_diff_vert <= -OSD_HOMING_LIM_V2) { + crh_u = SYM_HUD_ARROWS_U2; + } else if (crh_diff_vert <= -OSD_HOMING_LIM_V3) { + crh_u = SYM_HUD_ARROWS_U3; + } else if (crh_diff_vert >= OSD_HOMING_LIM_V1 && crh_diff_vert < OSD_HOMING_LIM_V2) { + crh_d = SYM_HUD_ARROWS_D1; + } else if (crh_diff_vert >= OSD_HOMING_LIM_V2 && crh_diff_vert < OSD_HOMING_LIM_V3) { + crh_d = SYM_HUD_ARROWS_D2; + } else if (crh_diff_vert >= OSD_HOMING_LIM_V3) { + crh_d = SYM_HUD_ARROWS_D3; + } + } + + displayWriteChar(osdGetDisplayPort(), px - 2, py, crh_l); + displayWriteChar(osdGetDisplayPort(), px + 2, py, crh_r); + displayWriteChar(osdGetDisplayPort(), px, py - 1, crh_u); + displayWriteChar(osdGetDisplayPort(), px, py + 1, crh_d); +} + + +/* + * Draw extra datas for a radar POI + */ +void osdHudDrawExtras(uint8_t poi_id) +{ + char buftmp[6]; + + uint8_t minX = osdConfig()->hud_margin_h + 1; + uint8_t maxX = osdGetDisplayPort()->cols - osdConfig()->hud_margin_h - 2; + uint8_t lineY = osdGetDisplayPort()->rows - osdConfig()->hud_margin_v - 1; + + displayWriteChar(osdGetDisplayPort(), minX + 1, lineY, 65 + poi_id); + displayWriteChar(osdGetDisplayPort(), minX + 2, lineY, SYM_HUD_SIGNAL_0 + radar_pois[poi_id].lq); + + if (radar_pois[poi_id].altitude < 0) { + osdFormatAltitudeSymbol(buftmp, -radar_pois[poi_id].altitude * 100); + displayWriteChar(osdGetDisplayPort(), minX + 8, lineY, SYM_HUD_ARROWS_D2); + } + else { + osdFormatAltitudeSymbol(buftmp, radar_pois[poi_id].altitude * 100); + displayWriteChar(osdGetDisplayPort(), minX + 8, lineY, SYM_HUD_ARROWS_U2); + } + + displayWrite(osdGetDisplayPort(), minX + 4, lineY, buftmp); + + osdFormatVelocityStr(buftmp, radar_pois[poi_id].speed, false); + displayWrite(osdGetDisplayPort(), maxX - 9, lineY, buftmp); + + tfp_sprintf(buftmp, "%3d%c", radar_pois[poi_id].heading, SYM_HEADING); + displayWrite(osdGetDisplayPort(), maxX - 4, lineY, buftmp); + +} + +#endif // USE_OSD diff --git a/src/main/io/osd_hud.h b/src/main/io/osd_hud.h new file mode 100644 index 00000000000..9744ad0ec4b --- /dev/null +++ b/src/main/io/osd_hud.h @@ -0,0 +1,29 @@ +/* + * 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 . + */ + +#pragma once + +#include + + +void osdHudClear(void); +void osdHudDrawCrosshair(uint8_t px, uint8_t py); +void osdHudDrawHoming(uint8_t px, uint8_t py); +void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, int16_t poiHeading, uint8_t poiSignal, uint16_t poiSymbol); +void osdHudDrawExtras(uint8_t poi_id); +int8_t radarGetNearestPOI(void); + diff --git a/src/main/io/pwmdriver_i2c.c b/src/main/io/pwmdriver_i2c.c index 2fda679d41a..727b8428f94 100644 --- a/src/main/io/pwmdriver_i2c.c +++ b/src/main/io/pwmdriver_i2c.c @@ -30,7 +30,7 @@ pwmDriverDriver_t pwmDrivers[PWM_DRIVER_IMPLEMENTATION_COUNT] = { } }; -bool isPwmDriverEnabled() { +bool isPwmDriverEnabled(void) { return driverEnabled; } diff --git a/src/main/io/rangefinder_benewake.c b/src/main/io/rangefinder_benewake.c index c0a0518211a..08e8d0cdc6f 100644 --- a/src/main/io/rangefinder_benewake.c +++ b/src/main/io/rangefinder_benewake.c @@ -123,13 +123,6 @@ static void benewakeRangefinderUpdate(void) if (sensorData == 0 || qual <= BENEWAKE_MIN_QUALITY) { sensorData = -1; } - - /* - debug[0] = ((tfminiPacket_t *) &buffer[0])->distL; - debug[1] = ((tfminiPacket_t *) &buffer[0])->distH; - debug[2] = ((tfminiPacket_t *) &buffer[0])->strengthH; - debug[3] = ((tfminiPacket_t *) &buffer[0])->strengthL; - */ } // Prepare for new packet diff --git a/src/main/io/rcdevice.c b/src/main/io/rcdevice.c index 48bd5888b34..3bfdb5972b2 100644 --- a/src/main/io/rcdevice.c +++ b/src/main/io/rcdevice.c @@ -24,6 +24,9 @@ #include "common/streambuf.h" #include "common/utils.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/time.h" #include "io/serial.h" @@ -32,11 +35,8 @@ #ifdef USE_RCDEVICE -typedef enum { - RCDP_SETTING_PARSE_WAITING_ID, - RCDP_SETTING_PARSE_WAITING_NAME, - RCDP_SETTING_PARSE_WAITING_VALUE, -} runcamDeviceSettingParseStep_e; +#define RCDEVICE_INIT_DEVICE_ATTEMPTS 6 +#define RCDEVICE_INIT_DEVICE_ATTEMPT_INTERVAL 1000 typedef struct runcamDeviceExpectedResponseLength_s { uint8_t command; @@ -44,15 +44,16 @@ typedef struct runcamDeviceExpectedResponseLength_s { } runcamDeviceExpectedResponseLength_t; static runcamDeviceExpectedResponseLength_t expectedResponsesLength[] = { - { RCDEVICE_PROTOCOL_COMMAND_READ_SETTING_DETAIL, 0xFF}, { RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, 5}, { RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, 2}, { RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, 2}, { RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, 3}, - { RCDEVICE_PROTOCOL_COMMAND_WRITE_SETTING, 4}, }; -static uint8_t runcamDeviceGetResponseLength(uint8_t command) +rcdeviceWaitingResponseQueue watingResponseQueue; +static uint8_t recvBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; // all the response contexts using same recv buffer + +static uint8_t runcamDeviceGetRespLen(uint8_t command) { for (unsigned int i = 0; i < ARRAYLEN(expectedResponsesLength); i++) { if (expectedResponsesLength[i].command == command) { @@ -63,80 +64,49 @@ static uint8_t runcamDeviceGetResponseLength(uint8_t command) return 0; } -// Parse the variable length response, e.g the response of settings data and the detail of setting -static uint8_t runcamDeviceIsResponseReceiveDone(uint8_t command, uint8_t *data, uint8_t dataLen, bool *isDone) +static bool rcdeviceRespCtxQueuePush(rcdeviceWaitingResponseQueue *queue, rcdeviceResponseParsingContext_t *respCtx) { - if (isDone == NULL) { + if (queue == NULL || (queue->itemCount + 1) > MAX_WAITING_RESPONSES) { return false; } - uint8_t expectedResponseDataLength = runcamDeviceGetResponseLength(command); - if (expectedResponseDataLength == 0xFF) { - uint8_t settingDataLength = 0x00; - // get setting datalen first - if (dataLen >= 3) { - settingDataLength = data[2]; - if (dataLen >= (settingDataLength + 4)) { - *isDone = true; - return true; - } - } + queue->buffer[queue->tailPos] = *respCtx; - if (settingDataLength > 60) { - return false; - } - } else if (dataLen >= expectedResponseDataLength) { - *isDone = true; - return true; + int newTailPos = queue->tailPos + 1; + if (newTailPos >= MAX_WAITING_RESPONSES) { + newTailPos = 0; } + queue->itemCount += 1; + queue->tailPos = newTailPos; return true; } -// a common way to receive packet and verify it -static uint8_t runcamDeviceReceivePacket(runcamDevice_t *device, uint8_t command, uint8_t *data, int timeoutms) +static rcdeviceResponseParsingContext_t* rcdeviceRespCtxQueuePeekFront(rcdeviceWaitingResponseQueue *queue) { - uint8_t dataPos = 0; - uint8_t crc = 0; - uint8_t responseDataLen = 0; - - // wait for reply until timeout(specialy by timeoutms) - timeMs_t timeout = millis() + timeoutms; - bool isWaitingHeader = true; - while (millis() < timeout) { - if (serialRxBytesWaiting(device->serialPort) > 0) { - uint8_t c = serialRead(device->serialPort); - crc = crc8_dvb_s2(crc, c); - - if (data) { - data[dataPos] = c; - } - dataPos++; + if (queue == NULL || queue->itemCount == 0) { + return NULL; + } - if (isWaitingHeader) { - if (c == RCDEVICE_PROTOCOL_HEADER) { - isWaitingHeader = false; - } - } else { - bool isDone = false; - if (!runcamDeviceIsResponseReceiveDone(command, data, dataPos, &isDone)) { - return 0; - } + rcdeviceResponseParsingContext_t *ctx = &queue->buffer[queue->headPos]; + return ctx; +} - if (isDone) { - responseDataLen = dataPos; - break; - } - } - } +STATIC_UNIT_TESTED rcdeviceResponseParsingContext_t* rcdeviceRespCtxQueueShift(rcdeviceWaitingResponseQueue *queue) +{ + if (queue == NULL || queue->itemCount == 0) { + return NULL; } - // check crc - if (crc != 0) { - return 0; + rcdeviceResponseParsingContext_t *ctx = &queue->buffer[queue->headPos]; + int newHeadPos = queue->headPos + 1; + if (newHeadPos >= MAX_WAITING_RESPONSES) { + newHeadPos = 0; } + queue->itemCount -= 1; + queue->headPos = newHeadPos; - return responseDataLen; + return ctx; } // every time send packet to device, and want to get something from device, @@ -166,7 +136,7 @@ static void runcamDeviceSendPacket(runcamDevice_t *device, uint8_t command, uint sbufWriteU8(&buf, RCDEVICE_PROTOCOL_HEADER); sbufWriteU8(&buf, command); - if (paramData) { + if (paramData && paramDataLen <= RCDEVICE_PROTOCOL_MAX_DATA_SIZE) { sbufWriteData(&buf, paramData, paramDataLen); } @@ -177,52 +147,54 @@ static void runcamDeviceSendPacket(runcamDevice_t *device, uint8_t command, uint sbufSwitchToReader(&buf, device->buffer); // send data if possible - serialWriteBuf(device->serialPort, sbufPtr(&buf), sbufBytesRemaining(&buf)); + serialWriteBuf(device->serialPort, device->buffer, sbufBytesRemaining(&buf)); } // a common way to send a packet to device, and get response from the device. -static bool runcamDeviceSendRequestAndWaitingResp(runcamDevice_t *device, uint8_t commandID, uint8_t *paramData, uint8_t paramDataLen, uint8_t *outputBuffer, uint8_t *outputBufferLen) +static void runcamDeviceSendRequestAndWaitingResp(runcamDevice_t *device, uint8_t commandID, uint8_t *paramData, uint8_t paramDataLen, timeMs_t tiemout, int maxRetryTimes, void *userInfo, rcdeviceResponseCallback parseFunc) { - int max_retries = 1; - // here using 1000ms as timeout, because the response from 5 key simulation command need a long time about >= 600ms, - // so set a max value to ensure we can receive the response - int timeoutMs = 1000; - - // only the command sending on initializing step need retry logic, - // otherwise, the timeout of 1000 ms is enough for the response from device - if (commandID == RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO) { - max_retries = 3; - timeoutMs = 300; // we have test some device, 100ms as timeout, and retry times be 3, it's stable for most case - } - - while (max_retries--) { - // flush rx buffer - runcamDeviceFlushRxBuffer(device); - - // send packet - runcamDeviceSendPacket(device, commandID, paramData, paramDataLen); - - // waiting response - uint8_t responseLength = runcamDeviceReceivePacket(device, commandID, outputBuffer, timeoutMs); - if (responseLength) { - if (outputBufferLen) { - *outputBufferLen = responseLength; - } + // runcamDeviceFlushRxBuffer(device); + + rcdeviceResponseParsingContext_t responseCtx; + memset(&responseCtx, 0, sizeof(rcdeviceResponseParsingContext_t)); + responseCtx.recvBuf = recvBuf; + responseCtx.command = commandID; + responseCtx.maxRetryTimes = maxRetryTimes; + responseCtx.expectedRespLen = runcamDeviceGetRespLen(commandID); + responseCtx.timeout = tiemout; + responseCtx.timeoutTimestamp = millis() + tiemout; + responseCtx.parserFunc = parseFunc; + responseCtx.device = device; + responseCtx.protocolVersion = RCDEVICE_PROTOCOL_VERSION_1_0; + if (paramData != NULL && paramDataLen <= RCDEVICE_PROTOCOL_MAX_DATA_SIZE) { + memcpy(responseCtx.paramData, paramData, paramDataLen); + responseCtx.paramDataLen = paramDataLen; + } + responseCtx.userInfo = userInfo; + rcdeviceRespCtxQueuePush(&watingResponseQueue, &responseCtx); + + // send packet + runcamDeviceSendPacket(device, commandID, paramData, paramDataLen); +} - return true; - } +static void runcamDeviceParseV1DeviceInfo(rcdeviceResponseParsingContext_t *ctx) +{ + if (ctx->result != RCDEVICE_RESP_SUCCESS) { + return; } - return false; + runcamDevice_t *device = ctx->device; + device->info.protocolVersion = RCDEVICE_PROTOCOL_RCSPLIT_VERSION; + device->info.features = RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON | RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON | RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE; + device->isReady = true; } -static uint8_t crc_high_first(uint8_t *ptr, uint8_t len) +static uint8_t crc8HighFirst(uint8_t *ptr, uint8_t len) { - uint8_t i; - uint8_t crc=0x00; + uint8_t crc = 0x00; while (len--) { crc ^= *ptr++; - for (i=8; i>0; --i) { + for (unsigned i = 8; i > 0; --i) { if (crc & 0x80) crc = (crc << 1) ^ 0x31; else @@ -232,10 +204,12 @@ static uint8_t crc_high_first(uint8_t *ptr, uint8_t len) return (crc); } -static void sendCtrlCommand(runcamDevice_t *device, rcsplit_ctrl_argument_e argument) +// for the rcsplits that firmware <= 1.1.0 +static void runcamSplitSendCommand(runcamDevice_t *device, uint8_t argument) { - if (!device->serialPort) - return ; + if (!device->serialPort) { + return; + } uint8_t uart_buffer[5] = {0}; uint8_t crc = 0; @@ -244,7 +218,7 @@ static void sendCtrlCommand(runcamDevice_t *device, rcsplit_ctrl_argument_e argu uart_buffer[1] = RCSPLIT_PACKET_CMD_CTRL; uart_buffer[2] = argument; uart_buffer[3] = RCSPLIT_PACKET_TAIL; - crc = crc_high_first(uart_buffer, 4); + crc = crc8HighFirst(uart_buffer, 4); // build up a full request [header]+[command]+[argument]+[crc]+[tail] uart_buffer[3] = crc; @@ -254,119 +228,72 @@ static void sendCtrlCommand(runcamDevice_t *device, rcsplit_ctrl_argument_e argu serialWriteBuf(device->serialPort, uart_buffer, 5); } -// get the device info(firmware version, protocol version and features, see the -// definition of runcamDeviceInfo_t to know more) -static bool runcamDeviceGetDeviceInfo(runcamDevice_t *device, uint8_t *outputBuffer) +static void runcamDeviceSendV1Initialize(runcamDevice_t *device) { - // Send "who are you" command to device to detect the device whether is running RCSplit FW1.0 or RCSplit FW1.1 - uint32_t max_retries = 3; - while (max_retries--) { - runcamDeviceFlushRxBuffer(device); - sendCtrlCommand(device, RCSPLIT_CTRL_ARGU_WHO_ARE_YOU); - - timeMs_t timeout = millis() + 300; - uint8_t response[5] = { 0 }; - while (millis() < timeout) { - if (serialRxBytesWaiting(device->serialPort) >= 5) { - response[0] = serialRead(device->serialPort); - response[1] = serialRead(device->serialPort); - response[2] = serialRead(device->serialPort); - response[3] = serialRead(device->serialPort); - response[4] = serialRead(device->serialPort); - if (response[0] != RCSPLIT_PACKET_HEADER || response[1] != RCSPLIT_PACKET_CMD_CTRL || response[2] != RCSPLIT_CTRL_ARGU_WHO_ARE_YOU || response[4] != RCSPLIT_PACKET_TAIL) { - break; - } - - uint8_t crcFromPacket = response[3]; - response[3] = response[4]; // move packet tail field to crc field, and calc crc with first 4 bytes - uint8_t crc = crc_high_first(response, 4); - if (crc != crcFromPacket) { - break; - } - - // generate response for RCSplit FW 1.0 and FW 1.1 - outputBuffer[0] = RCDEVICE_PROTOCOL_HEADER; - // protocol version - outputBuffer[1] = RCDEVICE_PROTOCOL_RCSPLIT_VERSION; - // features - outputBuffer[2] = RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON | RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON | RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE; - outputBuffer[3] = 0; - - crc = 0; - const uint8_t * const end = outputBuffer + 4; - for (const uint8_t *ptr = outputBuffer; ptr < end; ++ptr) { - crc = crc8_dvb_s2(crc, *ptr); - } - outputBuffer[4] = crc; - return true; - } - } - } - - return runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, NULL, 0, outputBuffer, NULL); + runcamDeviceFlushRxBuffer(device); + + rcdeviceResponseParsingContext_t responseCtx; + memset(&responseCtx, 0, sizeof(rcdeviceResponseParsingContext_t)); + responseCtx.recvBuf = recvBuf; + responseCtx.command = 0xFF; + responseCtx.maxRetryTimes = RCDEVICE_INIT_DEVICE_ATTEMPTS; + responseCtx.expectedRespLen = 5; + responseCtx.timeout = RCDEVICE_INIT_DEVICE_ATTEMPT_INTERVAL; + responseCtx.timeoutTimestamp = millis() + RCDEVICE_INIT_DEVICE_ATTEMPT_INTERVAL; + responseCtx.parserFunc = runcamDeviceParseV1DeviceInfo; + responseCtx.device = device; + responseCtx.protocolVersion = RCDEVICE_PROTOCOL_RCSPLIT_VERSION; + rcdeviceRespCtxQueuePush(&watingResponseQueue, &responseCtx); + + runcamSplitSendCommand(device, 0xFF); } -static bool runcamDeviceSend5KeyOSDCableConnectionEvent(runcamDevice_t *device, uint8_t operation, uint8_t *outActionID, uint8_t *outErrorCode) +static void runcamDeviceParseV2DeviceInfo(rcdeviceResponseParsingContext_t *ctx) { - uint8_t outputDataLen = RCDEVICE_PROTOCOL_MAX_PACKET_SIZE; - uint8_t respBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; - if (!runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), respBuf, &outputDataLen)) { - return false; + if (ctx->result != RCDEVICE_RESP_SUCCESS) { + runcamDeviceSendV1Initialize(ctx->device); + return; } - // the high 4 bits is the operationID that we sent - // the low 4 bits is the result code - uint8_t operationID = (respBuf[1] & 0xF0) >> 4; - bool errorCode = (respBuf[1] & 0x0F); - if (outActionID) { - *outActionID = operationID; - } + runcamDevice_t *device = ctx->device; + device->info.protocolVersion = ctx->recvBuf[1]; - if (outErrorCode) { - *outErrorCode = errorCode; - } + uint8_t featureLowBits = ctx->recvBuf[2]; + uint8_t featureHighBits = ctx->recvBuf[3]; + device->info.features = (featureHighBits << 8) | featureLowBits; + device->isReady = true; +} - return true; +// get the device info(firmware version, protocol version and features, see the +// definition of runcamDeviceInfo_t to know more) +static void runcamDeviceGetDeviceInfo(runcamDevice_t *device) +{ + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, NULL, 0, RCDEVICE_INIT_DEVICE_ATTEMPT_INTERVAL, RCDEVICE_INIT_DEVICE_ATTEMPTS, NULL, runcamDeviceParseV2DeviceInfo); } // init the runcam device, it'll search the UART port with FUNCTION_RCDEVICE id -// this function will delay 400ms in the first loop to wait the device prepared, -// as we know, there are has some camera need about 200~400ms to initialization, -// and then we can send/receive from it. -bool runcamDeviceInit(runcamDevice_t *device) +// this function will delay 3 seconds to wait the device prepared(special for runcam split) +void runcamDeviceInit(runcamDevice_t *device) { + device->isReady = false; serialPortFunction_e portID = FUNCTION_RCDEVICE; serialPortConfig_t *portConfig = findSerialPortConfig(portID); if (portConfig != NULL) { - device->serialPort = openSerialPort(portConfig->identifier, portID, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED); - + device->serialPort = openSerialPort(portConfig->identifier, portID, NULL, NULL, baudRates[BAUD_115200], MODE_RXTX, SERIAL_NOT_INVERTED); + // device->info.protocolVersion = rcdeviceConfig()->protocolVersion; + device->info.protocolVersion = RCDEVICE_PROTOCOL_VERSION_1_0; if (device->serialPort != NULL) { - // send RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO to device to retrive - // device info, e.g protocol version, supported features - uint8_t respBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; - if (runcamDeviceGetDeviceInfo(device, respBuf)) { - device->info.protocolVersion = respBuf[1]; - - uint8_t featureLowBits = respBuf[2]; - uint8_t featureHighBits = respBuf[3]; - device->info.features = (featureHighBits << 8) | featureLowBits; - return true; - } - - closeSerialPort(device->serialPort); + runcamDeviceGetDeviceInfo(device); } } - - device->serialPort = NULL; - return false; } bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation) { - if (device->info.protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { - sendCtrlCommand(device, operation + 1); - } else if (device->info.protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0) { + if (device->info.protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0) { runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL, &operation, sizeof(operation)); + } else if (device->info.protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { + runcamSplitSendCommand(device, operation + 1); } else { return false; } @@ -376,234 +303,117 @@ bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation) // every time start to control the OSD menu of camera, must call this method to // camera -bool runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device) +void runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceResponseCallback callback) { - uint8_t actionID = 0xFF; - uint8_t code = 0xFF; - bool r = runcamDeviceSend5KeyOSDCableConnectionEvent(device, RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN, &actionID, &code); - return r && (code == 1) && (actionID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN); + uint8_t operation = RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN; + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 400, 2, NULL, callback); } // when the control was stop, must call this method to the camera to disconnect // with camera. -bool runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device) +void runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceResponseCallback callback) { - uint8_t actionID = 0xFF; - uint8_t code = 0xFF; - bool r = runcamDeviceSend5KeyOSDCableConnectionEvent(device, RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE, &actionID, &code); - - return r && (code == 1) && (actionID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE); + uint8_t operation = RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE; + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 400, 2, NULL, callback); } // simulate button press event of 5 key osd cable with special button -bool runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation) +void runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation, rcdeviceResponseCallback callback) { if (operation == RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE) { - return false; - } - - if (runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, &operation, sizeof(uint8_t), NULL, NULL)) { - return true; + return; } - return false; + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, &operation, sizeof(uint8_t), 400, 2, NULL, callback); } // simulate button release event of 5 key osd cable -bool runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device) +void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdeviceResponseCallback callback) { - return runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, NULL, 0, NULL, NULL); + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, NULL, 0, 400, 2, NULL, callback); } -static bool runcamDeviceDecodeSettingDetail(sbuf_t *buf, runcamDeviceSettingDetail_t *outSettingDetail) +static rcdeviceResponseParsingContext_t* getWaitingResponse(timeMs_t currentTimeMs) { - char * saveptr; - - if (outSettingDetail == NULL || sbufBytesRemaining(buf) == 0) { - return false; - } + rcdeviceResponseParsingContext_t *respCtx = rcdeviceRespCtxQueuePeekFront(&watingResponseQueue); + while (respCtx != NULL && respCtx->timeoutTimestamp != 0 && currentTimeMs > respCtx->timeoutTimestamp) { + if (respCtx->maxRetryTimes > 0) { + if (respCtx->protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0) { + runcamDeviceSendPacket(respCtx->device, respCtx->command, respCtx->paramData, respCtx->paramDataLen); + } else if (respCtx->protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { + runcamSplitSendCommand(respCtx->device, respCtx->command); + } - rcdeviceSettingType_e settingType = sbufReadU8(buf); - outSettingDetail->type = settingType; - switch (settingType) { - case RCDEVICE_PROTOCOL_SETTINGTYPE_UINT8: - case RCDEVICE_PROTOCOL_SETTINGTYPE_INT8: - outSettingDetail->value = sbufReadU8(buf); - outSettingDetail->minValue = sbufReadU8(buf); - outSettingDetail->maxValue = sbufReadU8(buf); - outSettingDetail->stepSize = sbufReadU8(buf); - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_UINT16: - case RCDEVICE_PROTOCOL_SETTINGTYPE_INT16: - outSettingDetail->value = sbufReadU16(buf); - outSettingDetail->minValue = sbufReadU16(buf); - outSettingDetail->maxValue = sbufReadU16(buf); - outSettingDetail->stepSize = sbufReadU8(buf); - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_FLOAT: - outSettingDetail->value = sbufReadU32(buf); - outSettingDetail->minValue = sbufReadU32(buf); - outSettingDetail->maxValue = sbufReadU32(buf); - outSettingDetail->decimalPoint = sbufReadU8(buf); - outSettingDetail->stepSize = sbufReadU32(buf); - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_TEXT_SELECTION: { - outSettingDetail->value = sbufReadU8(buf); - - const char *tmp = (const char *)sbufConstPtr(buf); - const uint16_t maxLen = RCDEVICE_PROTOCOL_MAX_DATA_SIZE * RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS; - char textSels[maxLen]; - memset(textSels, 0, maxLen); - strncpy(textSels, tmp, maxLen); - char delims[] = ";"; - char *result = strtok_r(textSels, delims, &saveptr); - int i = 0; - runcamDeviceSettingTextSelection_t *iterator = outSettingDetail->textSelections; - while (result != NULL) { - if (i >= RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS) { - break; + respCtx->recvRespLen = 0; + respCtx->timeoutTimestamp = currentTimeMs + respCtx->timeout; + respCtx->maxRetryTimes -= 1; + respCtx = NULL; + break; + } else { + respCtx->result = RCDEVICE_RESP_TIMEOUT; + if (respCtx->parserFunc != NULL) { + respCtx->parserFunc(respCtx); } - memset(iterator->text, 0, RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH); - strncpy(iterator->text, result, RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH); - iterator++; - result = strtok_r(NULL, delims, &saveptr); - i++; + // dequeue and get next waiting response context + rcdeviceRespCtxQueueShift(&watingResponseQueue); + respCtx = rcdeviceRespCtxQueuePeekFront(&watingResponseQueue); } } - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_STRING: { - const char *tmp = (const char *)sbufConstPtr(buf); - strncpy(outSettingDetail->stringValue, tmp, RCDEVICE_PROTOCOL_MAX_STRING_LENGTH); - sbufAdvance(buf, strlen(tmp) + 1); - - outSettingDetail->maxStringSize = sbufReadU8(buf); - } - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_FOLDER: - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_INFO: { - const char *tmp = (const char *)sbufConstPtr(buf); - strncpy(outSettingDetail->stringValue, tmp, RCDEVICE_PROTOCOL_MAX_STRING_LENGTH); - sbufAdvance(buf, strlen(outSettingDetail->stringValue) + 1); - } - break; - case RCDEVICE_PROTOCOL_SETTINGTYPE_UNKNOWN: - break; - } - return true; + return respCtx; } -static bool runcamDeviceGetResponseWithMultipleChunk(runcamDevice_t *device, uint8_t command, uint8_t settingID, uint8_t *responseData, uint16_t *responseDatalen) +void rcdeviceReceive(timeUs_t currentTimeUs) { - if (responseData == NULL || responseDatalen == NULL) { - return false; - } - - // fill parameters buf - uint8_t paramsBuf[2]; - uint8_t chunkIndex = 0; - paramsBuf[0] = settingID; // parent setting id - paramsBuf[1] = chunkIndex; // chunk index - - uint8_t outputBufLen = RCDEVICE_PROTOCOL_MAX_PACKET_SIZE; - uint8_t outputBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; - bool result = runcamDeviceSendRequestAndWaitingResp(device, command, paramsBuf, sizeof(paramsBuf), outputBuf, &outputBufLen); - if (!result) { - return false; - } - - uint8_t remainingChunk = outputBuf[1]; - // Every response chunk count must less than or equal to RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE - if (remainingChunk >= RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE) { - return false; - } - - // save setting data to sbuf_t object - const uint16_t maxDataLen = RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE * RCDEVICE_PROTOCOL_MAX_DATA_SIZE; - // uint8_t data[maxDataLen]; - sbuf_t dataBuf; - dataBuf.ptr = responseData; - dataBuf.end = responseData + maxDataLen; - sbufWriteData(&dataBuf, outputBuf + 3, outputBufLen - 4); - - // get the remaining chunks - while (remainingChunk > 0) { - paramsBuf[1] = ++chunkIndex; // chunk index + UNUSED(currentTimeUs); - outputBufLen = RCDEVICE_PROTOCOL_MAX_PACKET_SIZE; - result = runcamDeviceSendRequestAndWaitingResp(device, command, paramsBuf, sizeof(paramsBuf), outputBuf, &outputBufLen); - - if (!result) { - return false; + rcdeviceResponseParsingContext_t *respCtx = NULL; + while ((respCtx = getWaitingResponse(millis())) != NULL) { + if (!serialRxBytesWaiting(respCtx->device->serialPort)) { + break; } - // append the trailing chunk to the sbuf_t object, - // but only append the actually setting data - sbufWriteData(&dataBuf, outputBuf + 3, outputBufLen - 4); - - remainingChunk--; - } - - sbufSwitchToReader(&dataBuf, responseData); - *responseDatalen = sbufBytesRemaining(&dataBuf); - - return true; -} - -// get the setting details with setting id -// after this function called, the setting detail will fill into -// outSettingDetail argument -bool runcamDeviceGetSettingDetail(runcamDevice_t *device, uint8_t settingID, runcamDeviceSettingDetail_t *outSettingDetail) -{ - if (outSettingDetail == NULL) - return false; - - uint16_t responseDataLength = 0; - uint8_t data[RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE * RCDEVICE_PROTOCOL_MAX_DATA_SIZE]; - if (!runcamDeviceGetResponseWithMultipleChunk(device, RCDEVICE_PROTOCOL_COMMAND_READ_SETTING_DETAIL, settingID, data, &responseDataLength)) { - return false; - } - - sbuf_t dataBuf; - dataBuf.ptr = data; - dataBuf.end = data + responseDataLength; - - // parse the settings data and convert them into a runcamDeviceSettingDetail_t - if (!runcamDeviceDecodeSettingDetail(&dataBuf, outSettingDetail)) { - return false; - } + const uint8_t c = serialRead(respCtx->device->serialPort); + if (respCtx->recvRespLen == 0) { + // Only start receiving packet when we found a header + if ((respCtx->protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0 && c != RCDEVICE_PROTOCOL_HEADER) || (respCtx->protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION && c != RCSPLIT_PACKET_HEADER)) { + continue; + } + } - return true; -} + respCtx->recvBuf[respCtx->recvRespLen] = c; + respCtx->recvRespLen += 1; -// write new value with to the setting -bool runcamDeviceWriteSetting(runcamDevice_t *device, uint8_t settingID, const void *paramData, uint8_t paramDataLen, runcamDeviceWriteSettingResponse_t *response) -{ - if (response == NULL || paramDataLen > (RCDEVICE_PROTOCOL_MAX_DATA_SIZE - 1)) { - return false; - } + // if data received done, trigger callback to parse response data, and update rcdevice state + if (respCtx->recvRespLen == respCtx->expectedRespLen) { + if (respCtx->protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0) { + uint8_t crc = 0; + for (int i = 0; i < respCtx->recvRespLen; i++) { + crc = crc8_dvb_s2(crc, respCtx->recvBuf[i]); + } - memset(response, 0, sizeof(runcamDeviceWriteSettingResponse_t)); - response->resultCode = 1; // initialize the result code to failed + respCtx->result = (crc == 0) ? RCDEVICE_RESP_SUCCESS : RCDEVICE_RESP_INCORRECT_CRC; + } else if (respCtx->protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { + if (respCtx->recvBuf[0] == RCSPLIT_PACKET_HEADER && respCtx->recvBuf[1] == RCSPLIT_PACKET_CMD_CTRL && respCtx->recvBuf[2] == 0xFF && respCtx->recvBuf[4] == RCSPLIT_PACKET_TAIL) { + uint8_t crcFromPacket = respCtx->recvBuf[3]; + respCtx->recvBuf[3] = respCtx->recvBuf[4]; // move packet tail field to crc field, and calc crc with first 4 bytes + uint8_t crc = crc8HighFirst(respCtx->recvBuf, 4); + respCtx->result = crc == crcFromPacket ? RCDEVICE_RESP_SUCCESS : RCDEVICE_RESP_INCORRECT_CRC; + } else { + respCtx->result = RCDEVICE_RESP_INCORRECT_CRC; + } + } - uint8_t paramsBufLen = sizeof(uint8_t) + paramDataLen; - uint8_t paramsBuf[RCDEVICE_PROTOCOL_MAX_DATA_SIZE]; - paramsBuf[0] = settingID; - memcpy(paramsBuf + 1, paramData, paramDataLen); + if (respCtx->parserFunc != NULL) { + respCtx->parserFunc(respCtx); + } - uint8_t outputBufLen = RCDEVICE_PROTOCOL_MAX_PACKET_SIZE; - uint8_t outputBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; - bool result = runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_WRITE_SETTING, paramsBuf, paramsBufLen, outputBuf, &outputBufLen); - if (!result) { - return false; + if (respCtx->result == RCDEVICE_RESP_SUCCESS) { + rcdeviceRespCtxQueueShift(&watingResponseQueue); + } + } } - - response->resultCode = outputBuf[1]; - response->needUpdateMenuItems = outputBuf[2]; - - return true; } -#endif \ No newline at end of file +#endif diff --git a/src/main/io/rcdevice.h b/src/main/io/rcdevice.h index 0ea090c0da1..54463717640 100644 --- a/src/main/io/rcdevice.h +++ b/src/main/io/rcdevice.h @@ -25,13 +25,7 @@ #define RCDEVICE_PROTOCOL_HEADER 0xCC #define RCDEVICE_PROTOCOL_MAX_PACKET_SIZE 64 -#define RCDEVICE_PROTOCOL_MAX_DATA_SIZE 62 -#define RCDEVICE_PROTOCOL_MAX_DATA_SIZE_WITH_CRC_FIELD 63 -#define RCDEVICE_PROTOCOL_MAX_SETTING_NAME_LENGTH 20 -#define RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH 20 -#define RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE 12 -#define RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS 30 -#define RCDEVICE_PROTOCOL_MAX_STRING_LENGTH 58 +#define RCDEVICE_PROTOCOL_MAX_DATA_SIZE 20 // Commands #define RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO 0x00 @@ -41,9 +35,11 @@ #define RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS 0x02 #define RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE 0x03 #define RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION 0x04 -// device setting access -#define RCDEVICE_PROTOCOL_COMMAND_READ_SETTING_DETAIL 0x11 -#define RCDEVICE_PROTOCOL_COMMAND_WRITE_SETTING 0x13 + +// Old protocol defines +#define RCSPLIT_PACKET_HEADER 0x55 +#define RCSPLIT_PACKET_CMD_CTRL 0x01 +#define RCSPLIT_PACKET_TAIL 0xaa // Feature Flag sets, it's a uint16_t flag typedef enum { @@ -51,8 +47,9 @@ typedef enum { RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON = (1 << 1), RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE = (1 << 2), RCDEVICE_PROTOCOL_FEATURE_SIMULATE_5_KEY_OSD_CABLE = (1 << 3), - RCDEVICE_PROTOCOL_FEATURE_DEVICE_SETTINGS_ACCESS = (1 << 4), - RCDEVICE_PROTOCOL_FEATURE_ADVANCE_CAM_CONTROL = (1 << 6), + RCDEVICE_PROTOCOL_FEATURE_START_RECORDING = (1 << 6), + RCDEVICE_PROTOCOL_FEATURE_STOP_RECORDING = (1 << 7), + RCDEVICE_PROTOCOL_FEATURE_CMS_MENU = (1 << 8), } rcdevice_features_e; // Operation of Camera Button Simulation @@ -60,13 +57,8 @@ typedef enum { RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN = 0x00, RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN = 0x01, RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE = 0x02, - RCDEVICE_PROTOCOL_CAM_CTRL_START_RECORDING = 0x03, // RCDEVICE_PROTOCOL_FEATURE_ADVANCE_CAM_CONTROL required - RCDEVICE_PROTOCOL_CAM_CTRL_STOP_RECORDING = 0x04, // RCDEVICE_PROTOCOL_FEATURE_ADVANCE_CAM_CONTROL required - RCDEVICE_PROTOCOL_CAM_CTRL_TAKE_A_PHOTO = 0X05, // RCDEVICE_PROTOCOL_FEATURE_ADVANCE_CAM_CONTROL required - RCDEVICE_PROTOCOL_CAM_CTRL_TURN_ON_WIFI = 0X06, // RCDEVICE_PROTOCOL_FEATURE_ADVANCE_CAM_CONTROL required - RCDEVICE_PROTOCOL_CAM_CTRL_TURN_OFF_WIFI = 0X07, // RCDEVICE_PROTOCOL_FEATURE_ADVANCE_CAM_CONTROL required - RCDEVICE_PROTOCOL_CAM_CTRL_OPEN_OSD_SETTING = 0X08, // RCDEVICE_PROTOCOL_FEATURE_ADVANCE_CAM_CONTROL required - RCDEVICE_PROTOCOL_CAM_CTRL_CLOSE_OSD_SETTING = 0X09, // RCDEVICE_PROTOCOL_FEATURE_ADVANCE_CAM_CONTROL required + RCDEVICE_PROTOCOL_CAM_CTRL_START_RECORDING = 0x03, + RCDEVICE_PROTOCOL_CAM_CTRL_STOP_RECORDING = 0x04, RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION = 0xFF } rcdevice_camera_control_opeation_e; @@ -106,60 +98,8 @@ typedef enum { RCDEVICE_PROTOCOL_UNKNOWN } rcdevice_protocol_version_e; -// Reserved setting ids -typedef enum { - RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET = 0, // type: text_selection, read&write, 0: use charset with betaflight logo, 1 use - // charset with cleanflight logo, other id are not used - RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS = 1, // type: uint8_t, read only, the column count of the OSD layer - RCDEVICE_PROTOCOL_SETTINGID_DISP_TV_MODE = 2, // type: text_selection, read&write, 0:NTSC, 1:PAL - RCDEVICE_PROTOCOL_SETTINGID_SDCARD_CAPACITY = 3, // type: info, read only, return sd card capacity - RCDEVICE_PROTOCOL_SETTINGID_REMAINING_RECORDING_TIME = 4, // type: info, read only, return remaining recording time - RCDEVICE_PROTOCOL_SETTINGID_RESOLUTION = 5, // type: text selection, read&write, return the current resolution and all available resolutions - RCDEVICE_PROTOCOL_SETTINGID_CAMERA_TIME = 6, // type: string, read&write, update the camera time, the time attribute of medias file in camera will use this time. - RCDEVICE_PROTOCOL_SETTINGID_RESERVED7 = 7, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED8 = 8, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED9 = 9, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED10 = 10, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED11 = 11, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED12 = 12, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED13 = 13, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED14 = 14, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED15 = 15, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED16 = 16, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED17 = 17, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED18 = 18, - RCDEVICE_PROTOCOL_SETTINGID_RESERVED19 = 19, -} rcdeviceReservedSettingID_e; - -typedef enum { - RCDEVICE_PROTOCOL_SETTINGTYPE_UINT8 = 0, - RCDEVICE_PROTOCOL_SETTINGTYPE_INT8 = 1, - RCDEVICE_PROTOCOL_SETTINGTYPE_UINT16 = 2, - RCDEVICE_PROTOCOL_SETTINGTYPE_INT16 = 3, - RCDEVICE_PROTOCOL_SETTINGTYPE_FLOAT = 8, - RCDEVICE_PROTOCOL_SETTINGTYPE_TEXT_SELECTION = 9, - RCDEVICE_PROTOCOL_SETTINGTYPE_STRING = 10, - RCDEVICE_PROTOCOL_SETTINGTYPE_FOLDER = 11, - RCDEVICE_PROTOCOL_SETTINGTYPE_INFO = 12, - RCDEVICE_PROTOCOL_SETTINGTYPE_UNKNOWN -} rcdeviceSettingType_e; // end of Runcam Device definition -// Old version defination(RCSplit firmware v1.0.0 and v1.1.0) -// packet header and tail -#define RCSPLIT_PACKET_HEADER 0x55 -#define RCSPLIT_PACKET_CMD_CTRL 0x01 -#define RCSPLIT_PACKET_TAIL 0xaa - -typedef enum { - RCSPLIT_CTRL_ARGU_INVALID = 0x0, - RCSPLIT_CTRL_ARGU_WIFI_BTN = 0x1, - RCSPLIT_CTRL_ARGU_POWER_BTN = 0x2, - RCSPLIT_CTRL_ARGU_CHANGE_MODE = 0x3, - RCSPLIT_CTRL_ARGU_WHO_ARE_YOU = 0xFF, -} rcsplit_ctrl_argument_e; -// end of old version protocol definition - typedef struct runcamDeviceInfo_s { rcdevice_protocol_version_e protocolVersion; uint16_t features; @@ -169,40 +109,53 @@ typedef struct runcamDevice_s { serialPort_t *serialPort; uint8_t buffer[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; runcamDeviceInfo_t info; + bool isReady; } runcamDevice_t; -typedef struct runcamDeviceSettingTextSelection_s { - char text[RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH]; -} runcamDeviceSettingTextSelection_t; - -typedef struct runcamDeviceSettingDetail_s { - uint8_t type; - uint32_t value; - uint32_t minValue; - uint32_t maxValue; - uint8_t decimalPoint; - uint32_t stepSize; - uint8_t maxStringSize; - char stringValue[RCDEVICE_PROTOCOL_MAX_STRING_LENGTH]; // when settingType is RCDEVICE_PROTOCOL_SETTINGTYPE_INFO or RCDEVICE_PROTOCOL_SETTINGTYPE_STRING, this field store the string/info value; - runcamDeviceSettingTextSelection_t textSelections[RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS]; -} runcamDeviceSettingDetail_t; - -typedef struct runcamDeviceWriteSettingResponse_s { - uint8_t resultCode; - uint8_t needUpdateMenuItems; -} runcamDeviceWriteSettingResponse_t; - -bool runcamDeviceInit(runcamDevice_t *device); +#define MAX_WAITING_RESPONSES 2 + +typedef enum { + RCDEVICE_RESP_SUCCESS = 0, + RCDEVICE_RESP_INCORRECT_CRC = 1, + RCDEVICE_RESP_TIMEOUT = 2 +} rcdeviceResponseStatus_e; + +typedef struct rcdeviceResponseParsingContext_s rcdeviceResponseParsingContext_t; +typedef void(*rcdeviceResponseCallback)(rcdeviceResponseParsingContext_t* context); + +typedef struct rcdeviceResponseParsingContext_s { + uint8_t command; + uint8_t expectedRespLen; // total length of response data + uint8_t recvRespLen; // length of the data received + uint8_t *recvBuf; // response data buffer + timeMs_t timeout; + timeMs_t timeoutTimestamp; // if zero, it's means keep waiting for the response + rcdeviceResponseCallback parserFunc; + runcamDevice_t *device; + uint8_t paramData[RCDEVICE_PROTOCOL_MAX_DATA_SIZE]; + uint8_t paramDataLen; + uint8_t protocolVersion; + int maxRetryTimes; + void *userInfo; + rcdeviceResponseStatus_e result; +} rcdeviceResponseParsingContext_t; + +typedef struct { + uint8_t headPos; // current head position of the queue + uint8_t tailPos; + uint8_t itemCount; // the item count in the queue + rcdeviceResponseParsingContext_t buffer[MAX_WAITING_RESPONSES]; + rcdeviceResponseCallback parseFunc; +} rcdeviceWaitingResponseQueue; + +void runcamDeviceInit(runcamDevice_t *device); +void rcdeviceReceive(timeUs_t currentTimeUs); // camera button simulation bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation); // 5 key osd cable simulation -bool runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device); -bool runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device); -bool runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation); -bool runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device); - -// Device Setting Access -bool runcamDeviceGetSettingDetail(runcamDevice_t *device, uint8_t settingID, runcamDeviceSettingDetail_t *outSettingDetail); -bool runcamDeviceWriteSetting(runcamDevice_t *device, uint8_t settingID, const void *data, uint8_t dataLen, runcamDeviceWriteSettingResponse_t *response); \ No newline at end of file +void runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceResponseCallback callback); +void runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceResponseCallback callback); +void runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation, rcdeviceResponseCallback callback); +void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdeviceResponseCallback callback); diff --git a/src/main/io/rcdevice_cam.c b/src/main/io/rcdevice_cam.c index aa890e2317f..36a0f998690 100644 --- a/src/main/io/rcdevice_cam.c +++ b/src/main/io/rcdevice_cam.c @@ -34,14 +34,16 @@ #ifdef USE_RCDEVICE -#define IS_HI(X) (rcData[X] > 1750) -#define IS_LO(X) (rcData[X] < 1250) -#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) +#define IS_HI(X) (rxGetChannelValue(X) > FIVE_KEY_CABLE_JOYSTICK_MAX) +#define IS_LO(X) (rxGetChannelValue(X) < FIVE_KEY_CABLE_JOYSTICK_MIN) +#define IS_MID(X) (rxGetChannelValue(X) > FIVE_KEY_CABLE_JOYSTICK_MID_START && rxGetChannelValue(X) < FIVE_KEY_CABLE_JOYSTICK_MID_END) static runcamDevice_t runcamDevice; runcamDevice_t *camDevice = &runcamDevice; rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; -bool rcdeviceInMenu; -bool needRelease = false; +bool rcdeviceInMenu = false; +bool isButtonPressed = false; +bool waitingDeviceResponse = false; + static bool isFeatureSupported(uint8_t feature) { @@ -52,67 +54,9 @@ static bool isFeatureSupported(uint8_t feature) return false; } -static bool rcdeviceIsCameraControlEnabled(void) -{ - bool isPowerSimulationSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON); - bool isWiFiSimulationSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON); - bool isChangeModeSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE); - - if (camDevice->serialPort != NULL && (isPowerSimulationSupported || isWiFiSimulationSupported || isChangeModeSupported)) { - return true; - } - - return false; -} - bool rcdeviceIsEnabled(void) { - bool is5KeySimulationSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_5_KEY_OSD_CABLE); - - if (camDevice->serialPort != NULL && (rcdeviceIsCameraControlEnabled() || is5KeySimulationSupported)) { - return true; - } - - return false; -} - -static bool rcdeviceIs5KeyEnabled(void) -{ - if (camDevice->serialPort != NULL && isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_5_KEY_OSD_CABLE)) { - return true; - } - - return false; -} - -static void rcdeviceCameraUpdateTime(void) -{ - static bool hasSynchronizedTime = false; - // don't try more than 3 times to avoid overloading the CPU if - // the camera doesn't accept the command for some reason. - static int retries = 0; - runcamDeviceWriteSettingResponse_t updateSettingResponse; - // Format is yyyyMMddThhmmss.0 plus null terminator, hence 18 - // characters. - char buf[18]; - dateTime_t dt; - - if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_DEVICE_SETTINGS_ACCESS) && - !hasSynchronizedTime && retries < 3) { - - if (rtcGetDateTimeLocal(&dt)) { - retries++; - tfp_sprintf(buf, "%04d%02d%02dT%02d%02d%02d.0", - dt.year, dt.month, dt.day, - dt.hours, dt.minutes, dt.seconds); - - bool ok = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_CAMERA_TIME, - buf, sizeof(buf), &updateSettingResponse); - if (ok && updateSettingResponse.resultCode == 0) { - hasSynchronizedTime = true; - } - } - } + return camDevice->serialPort != NULL; } static void rcdeviceCameraControlProcess(void) @@ -131,7 +75,12 @@ static void rcdeviceCameraControlProcess(void) switch (i) { case BOXCAMERA1: if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON)) { - behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN; + // avoid display wifi page when arming, in the next firmware(>2.0) of rcsplit we have change the wifi page logic: + // when the wifi was turn on it won't turn off the analog video output, + // and just put a wifi indicator on the right top of the video output. here is for the old split firmware + if (!ARMING_FLAG(ARMED)) { + behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN; + } } break; case BOXCAMERA2: @@ -141,7 +90,10 @@ static void rcdeviceCameraControlProcess(void) break; case BOXCAMERA3: if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE)) { - behavior = RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE; + // avoid change camera mode when arming + if (!ARMING_FLAG(ARMED)) { + behavior = RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE; + } } break; default: @@ -155,10 +107,62 @@ static void rcdeviceCameraControlProcess(void) switchStates[switchIndex].isActivated = false; } } - rcdeviceCameraUpdateTime(); } -static bool rcdeviceCamSimulate5KeyCablePress(rcdeviceCamSimulationKeyEvent_e key) +static void rcdeviceSimulationOSDCableFailed(rcdeviceResponseParsingContext_t *ctx) +{ + waitingDeviceResponse = false; + if (ctx->command == RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION) { + uint8_t operationID = ctx->paramData[0]; + if (operationID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE) { + return; + } + } +} + +static void rcdeviceSimulationRespHandle(rcdeviceResponseParsingContext_t *ctx) +{ + if (ctx->result != RCDEVICE_RESP_SUCCESS) { + rcdeviceSimulationOSDCableFailed(ctx); + waitingDeviceResponse = false; + return; + } + + switch (ctx->command) { + case RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE: + isButtonPressed = false; + break; + case RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION: + { + // the high 4 bits is the operationID that we sent + // the low 4 bits is the result code + isButtonPressed = true; + uint8_t operationID = ctx->paramData[0]; + bool errorCode = (ctx->recvBuf[1] & 0x0F); + if (operationID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN) { + if (errorCode == 1) { + rcdeviceInMenu = true; + beeper(BEEPER_CAM_CONNECTION_OPEN); + } else { + beeper(BEEPER_CAM_CONNECTION_CLOSE); + } + } else if (operationID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE) { + if (errorCode == 1) { + rcdeviceInMenu = false; + beeper(BEEPER_CAM_CONNECTION_CLOSE); + } + } + } + break; + case RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS: + isButtonPressed = true; + break; + } + + waitingDeviceResponse = false; +} + +static void rcdeviceCamSimulate5KeyCablePress(rcdeviceCamSimulationKeyEvent_e key) { uint8_t operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE; switch (key) { @@ -177,48 +181,38 @@ static bool rcdeviceCamSimulate5KeyCablePress(rcdeviceCamSimulationKeyEvent_e ke case RCDEVICE_CAM_KEY_ENTER: operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET; break; + case RCDEVICE_CAM_KEY_NONE: default: operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE; break; } - return runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, operation); + runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, operation, rcdeviceSimulationRespHandle); } -static bool rcdeviceSend5KeyOSDCableSimualtionEvent(rcdeviceCamSimulationKeyEvent_e key) +void rcdeviceSend5KeyOSDCableSimualtionEvent(rcdeviceCamSimulationKeyEvent_e key) { - bool reqResult = false; switch (key) { case RCDEVICE_CAM_KEY_CONNECTION_OPEN: - reqResult = runcamDeviceOpen5KeyOSDCableConnection(camDevice); - if (reqResult) { - rcdeviceInMenu = true; - beeper(BEEPER_CAM_CONNECTION_OPEN); - } + runcamDeviceOpen5KeyOSDCableConnection(camDevice, rcdeviceSimulationRespHandle); break; case RCDEVICE_CAM_KEY_CONNECTION_CLOSE: - reqResult = runcamDeviceClose5KeyOSDCableConnection(camDevice); - if (reqResult) { - rcdeviceInMenu = false; - beeper(BEEPER_CAM_CONNECTION_CLOSE); - } + runcamDeviceClose5KeyOSDCableConnection(camDevice, rcdeviceSimulationRespHandle); break; case RCDEVICE_CAM_KEY_ENTER: case RCDEVICE_CAM_KEY_LEFT: case RCDEVICE_CAM_KEY_UP: case RCDEVICE_CAM_KEY_RIGHT: case RCDEVICE_CAM_KEY_DOWN: - reqResult = rcdeviceCamSimulate5KeyCablePress(key); + rcdeviceCamSimulate5KeyCablePress(key); break; case RCDEVICE_CAM_KEY_RELEASE: - reqResult = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice); + runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice, rcdeviceSimulationRespHandle); break; + case RCDEVICE_CAM_KEY_NONE: default: - reqResult = false; break; } - - return reqResult; } static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) @@ -231,26 +225,23 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) } #endif - if (camDevice->serialPort == 0) { + if (camDevice->serialPort == 0 || ARMING_FLAG(ARMED)) { return; } - rcdeviceCamSimulationKeyEvent_e key = RCDEVICE_CAM_KEY_NONE; - - if (needRelease) { + if (isButtonPressed) { if (IS_MID(YAW) && IS_MID(PITCH) && IS_MID(ROLL)) { - key = RCDEVICE_CAM_KEY_RELEASE; - if (rcdeviceSend5KeyOSDCableSimualtionEvent(key)) { - needRelease = false; - } else { - rcdeviceInMenu = false; - } - return; - } else { - return; + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + waitingDeviceResponse = true; } } else { - if (IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH) && IS_LO(YAW)) { // Disconnect HI YAW + if (waitingDeviceResponse) { + return; + } + + rcdeviceCamSimulationKeyEvent_e key = RCDEVICE_CAM_KEY_NONE; + + if (IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH) && IS_LO(YAW)) { // Disconnect Lo YAW if (rcdeviceInMenu) { key = RCDEVICE_CAM_KEY_CONNECTION_CLOSE; } @@ -268,46 +259,38 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) key = RCDEVICE_CAM_KEY_ENTER; } } else { - if (IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH) && IS_HI(YAW) && !ARMING_FLAG(ARMED)) { // Enter HI YAW + if (IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH) && IS_HI(YAW)) { // Enter HI YAW key = RCDEVICE_CAM_KEY_CONNECTION_OPEN; } } } - } - if (key != RCDEVICE_CAM_KEY_NONE) { - if (rcdeviceSend5KeyOSDCableSimualtionEvent(key)) { - needRelease = true; - } else { - rcdeviceInMenu = false; + if (key != RCDEVICE_CAM_KEY_NONE) { + rcdeviceSend5KeyOSDCableSimualtionEvent(key); + isButtonPressed = true; + waitingDeviceResponse = true; } } } void rcdeviceUpdate(timeUs_t currentTimeUs) { - if (rcdeviceIsCameraControlEnabled()) { - rcdeviceCameraControlProcess(); - } + rcdeviceReceive(currentTimeUs); - if (rcdeviceIs5KeyEnabled()) { - rcdevice5KeySimulationProcess(currentTimeUs); - } + rcdeviceCameraControlProcess(); + + rcdevice5KeySimulationProcess(currentTimeUs); } -bool rcdeviceInit(void) +void rcdeviceInit(void) { // open serial port - if (!runcamDeviceInit(camDevice)) { - return false; - } + runcamDeviceInit(camDevice); for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) { uint8_t switchIndex = i - BOXCAMERA1; switchStates[switchIndex].isActivated = true; } - - return true; } #endif diff --git a/src/main/io/rcdevice_cam.h b/src/main/io/rcdevice_cam.h index 0e61bec1392..7463cdf4407 100644 --- a/src/main/io/rcdevice_cam.h +++ b/src/main/io/rcdevice_cam.h @@ -26,6 +26,11 @@ #include "io/rcdevice.h" #include "fc/rc_modes.h" +#define FIVE_KEY_CABLE_JOYSTICK_MIN 1080 +#define FIVE_KEY_CABLE_JOYSTICK_MAX 1920 +#define FIVE_KEY_CABLE_JOYSTICK_MID_START 1350 +#define FIVE_KEY_CABLE_JOYSTICK_MID_END 1650 + typedef struct rcdeviceSwitchState_s { bool isActivated; } rcdeviceSwitchState_t; @@ -33,10 +38,9 @@ typedef struct rcdeviceSwitchState_s { extern runcamDevice_t *camDevice; extern bool rcdeviceInMenu; -bool rcdeviceInit(void); +void rcdeviceInit(void); void rcdeviceUpdate(timeUs_t currentTimeUs); bool rcdeviceIsEnabled(void); -// used for unit test -extern rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; +void rcdeviceSend5KeyOSDCableSimualtionEvent(rcdeviceCamSimulationKeyEvent_e key); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 3b6cd3d4bac..d8dc546c6e9 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -115,7 +115,7 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig) serialConfig->portConfigs[i].peripheral_baudrateIndex = BAUD_115200; } - serialConfig->portConfigs[0].functionMask = FUNCTION_MSP | FUNCTION_DEBUG_TRACE; + serialConfig->portConfigs[0].functionMask = FUNCTION_MSP; #ifdef SERIALRX_UART serialPortConfig_t *serialRxUartConfig = serialFindPortConfiguration(SERIALRX_UART); @@ -263,7 +263,7 @@ serialPort_t *findNextSharedSerialPort(uint32_t functionMask, serialPortFunction } #define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) -#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK | FUNCTION_DEBUG_TRACE) +#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK | FUNCTION_LOG) bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck) { diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 565543c6f9b..617fb19b6d2 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -46,9 +46,11 @@ typedef enum { FUNCTION_VTX_TRAMP = (1 << 12), // 4096 FUNCTION_UAV_INTERCONNECT = (1 << 13), // 8192 FUNCTION_OPTICAL_FLOW = (1 << 14), // 16384 - FUNCTION_DEBUG_TRACE = (1 << 15), // 32768 + FUNCTION_LOG = (1 << 15), // 32768 FUNCTION_RANGEFINDER = (1 << 16), // 65536 FUNCTION_VTX_FFPV = (1 << 17), // 131072 + FUNCTION_SERIALSHOT = (1 << 18), // 262144 + FUNCTION_TELEMETRY_SIM = (1 << 19) // 524288 } serialPortFunction_e; typedef enum { diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index f64fe6d803d..95ef85a465c 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -136,17 +136,17 @@ uint8_t esc4wayInit(void) pwmDisableMotors(); escCount = 0; memset(&escHardware, 0, sizeof(escHardware)); - pwmIOConfiguration_t *pwmIOConfiguration = pwmGetOutputConfiguration(); - for (volatile uint8_t i = 0; i < pwmIOConfiguration->ioCount; i++) { - if ((pwmIOConfiguration->ioConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { - if (motor[pwmIOConfiguration->ioConfigurations[i].index] > 0) { - escHardware[escCount].io = IOGetByTag(pwmIOConfiguration->ioConfigurations[i].timerHardware->tag); - setEscInput(escCount); - setEscHi(escCount); - escCount++; - } + + for (int idx = 0; idx < getMotorCount(); idx++) { + ioTag_t tag = pwmGetMotorPinTag(idx); + if (tag != IOTAG_NONE) { + escHardware[escCount].io = IOGetByTag(tag); + setEscInput(escCount); + setEscHi(escCount); + escCount++; } } + return escCount; } diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index bc86209927d..6beb5c18119 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -86,7 +86,8 @@ enum { SA_CMD_SET_CHAN, SA_CMD_SET_FREQ, SA_CMD_SET_MODE, - SA_CMD_GET_SETTINGS_V2 = 0x09 // Response only + SA_CMD_GET_SETTINGS_V2 = 0x09, // Response only + SA_CMD_GET_SETTINGS_V21 = 0x11, } smartAudioCommand_e; // This is not a good design; can't distinguish command from response this way. @@ -145,7 +146,7 @@ static uint8_t saLockMode = SA_MODE_SET_UNLOCK; // saCms variable? bool saDeferred = true; // saCms variable? // Receive frame reassembly buffer -#define SA_MAX_RCVLEN 11 +#define SA_MAX_RCVLEN 15 static uint8_t sa_rbuf[SA_MAX_RCVLEN+4]; // XXX delete 4 byte guard // @@ -266,7 +267,7 @@ static void saProcessResponse(uint8_t *buf, int len) if (resp == sa_outstanding) { sa_outstanding = SA_CMD_NONE; - } else if ((resp == SA_CMD_GET_SETTINGS_V2) && (sa_outstanding == SA_CMD_GET_SETTINGS)) { + } else if ((resp == SA_CMD_GET_SETTINGS_V2 || resp == SA_CMD_GET_SETTINGS_V21) && (sa_outstanding == SA_CMD_GET_SETTINGS)) { sa_outstanding = SA_CMD_NONE; } else { saStat.ooopresp++; @@ -274,17 +275,21 @@ static void saProcessResponse(uint8_t *buf, int len) } switch (resp) { + case SA_CMD_GET_SETTINGS_V21: // Version 2.1 Get Settings case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings case SA_CMD_GET_SETTINGS: // Version 1 Get Settings if (len < 7) { break; } + // XXX(fujin): For now handle V21 saDevice.version as '2'. + // From spec: "Bit 7-3 is holding the Smart audio version where 0 is V1, 1 is V2, 2 is V2.1" saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2; saDevice.channel = buf[2]; saDevice.power = buf[3]; saDevice.mode = buf[4]; saDevice.freq = (buf[5] << 8)|buf[6]; + // XXX(fujin): Receive additional SA2.1 fields here for dBm based power level. DEBUG_SET(DEBUG_SMARTAUDIO, 0, saDevice.version * 100 + saDevice.mode); DEBUG_SET(DEBUG_SMARTAUDIO, 1, saDevice.channel); diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h index 80a4c8faf44..82b7f754079 100644 --- a/src/main/msp/msp_protocol_v2_common.h +++ b/src/main/msp/msp_protocol_v2_common.h @@ -15,16 +15,21 @@ * along with INAV. If not, see . */ -#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16)) -#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16)) -#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting -#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting +#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16)) +#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16)) +#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting +#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting -#define MSP2_COMMON_MOTOR_MIXER 0x1005 -#define MSP2_COMMON_SET_MOTOR_MIXER 0x1006 +#define MSP2_COMMON_MOTOR_MIXER 0x1005 +#define MSP2_COMMON_SET_MOTOR_MIXER 0x1006 -#define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..). -#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings +#define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..). +#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings #define MSP2_COMMON_SERIAL_CONFIG 0x1009 -#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A \ No newline at end of file +#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A + +// radar commands +#define MSP2_COMMON_SET_RADAR_POS 0x100B //SET radar position information +#define MSP2_COMMON_SET_RADAR_ITD 0x100C //SET radar information to display + diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 89ac4e433ff..027a01a2c5d 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -52,3 +52,13 @@ #define MSP2_INAV_TEMP_SENSOR_CONFIG 0x201C #define MSP2_INAV_SET_TEMP_SENSOR_CONFIG 0x201D #define MSP2_INAV_TEMPERATURES 0x201E + +#define MSP2_INAV_SERVO_MIXER 0x2020 +#define MSP2_INAV_SET_SERVO_MIXER 0x2021 +#define MSP2_INAV_LOGIC_CONDITIONS 0x2022 +#define MSP2_INAV_SET_LOGIC_CONDITIONS 0x2023 + +#define MSP2_PID 0x2030 +#define MSP2_SET_PID 0x2031 + +#define MSP2_INAV_OPFLOW_CALIBRATION 0x2032 diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index d3e9490e8fc..1434ed1a828 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -268,6 +268,11 @@ static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int l #define JUMBO_FRAME_SIZE_LIMIT 255 static int mspSerialSendFrame(mspPort_t *msp, const uint8_t * hdr, int hdrLen, const uint8_t * data, int dataLen, const uint8_t * crc, int crcLen) { + // VSP MSP port might be unconnected. To prevent blocking - check if it's connected first + if (!serialIsConnected(msp->port)) { + return 0; + } + // We are allowed to send out the response if // a) TX buffer is completely empty (we are talking to well-behaving party that follows request-response scheduling; // this allows us to transmit jumbo frames bigger than TX buffer (serialWriteBuf will block, but for jumbo frames we don't care) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 5575dc8348a..8510cc506b5 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -50,6 +50,8 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "rx/rx.h" + #include "sensors/sensors.h" #include "sensors/acceleration.h" #include "sensors/boardalignment.h" @@ -68,27 +70,28 @@ * Compatibility for home position *-----------------------------------------------------------*/ gpsLocation_t GPS_home; -uint16_t GPS_distanceToHome; // distance to home point in meters +uint32_t GPS_distanceToHome; // distance to home point in meters int16_t GPS_directionToHome; // direction to home point in degrees +radar_pois_t radar_pois[RADAR_MAX_POIS]; + #if defined(USE_NAV) #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) -PG_DECLARE_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList); PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { .flags = { .use_thr_mid_for_althold = 0, - .extra_arming_safety = 1, + .extra_arming_safety = NAV_EXTRA_ARMING_SAFETY_ON, .user_control_mode = NAV_GPS_ATTI, .rth_alt_control_mode = NAV_RTH_AT_LEAST_ALT, - .rth_climb_first = 1, // Climb first, turn after reaching safe altitude - .rth_climb_ignore_emerg = 0, // Ignore GPS loss on initial climb + .rth_climb_first = 1, // Climb first, turn after reaching safe altitude + .rth_climb_ignore_emerg = 0, // Ignore GPS loss on initial climb .rth_tail_first = 0, .disarm_on_landing = 0, .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS, @@ -96,29 +99,29 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, }, // General navigation parameters - .pos_failure_timeout = 5, // 5 sec - .waypoint_radius = 100, // 2m diameter - .waypoint_safe_distance = 10000, // 100m - first waypoint should be closer than this - .max_auto_speed = 300, // 3 m/s = 10.8 km/h - .max_auto_climb_rate = 500, // 5 m/s + .pos_failure_timeout = 5, // 5 sec + .waypoint_radius = 100, // 2m diameter + .waypoint_safe_distance = 10000, // centimeters - first waypoint should be closer than this + .max_auto_speed = 300, // 3 m/s = 10.8 km/h + .max_auto_climb_rate = 500, // 5 m/s .max_manual_speed = 500, .max_manual_climb_rate = 200, - .land_descent_rate = 200, // 2 m/s - .land_slowdown_minalt = 500, // 5 meters of altitude - .land_slowdown_maxalt = 2000, // 20 meters of altitude - .emerg_descent_rate = 500, // 5 m/s - .min_rth_distance = 500, // If closer than 5m - land immediately - .rth_altitude = 1000, // 10m - .rth_home_altitude = 0, - .rth_abort_threshold = 50000, // 500m - should be safe for all aircraft - .max_terrain_follow_altitude = 100, // max 1m altitude in terrain following mode + .land_descent_rate = 200, // centimeters/s + .land_slowdown_minalt = 500, // altitude in centimeters + .land_slowdown_maxalt = 2000, // altitude in meters + .emerg_descent_rate = 500, // centimeters/s + .min_rth_distance = 500, // centimeters, if closer than this land immediately + .rth_altitude = 1000, // altitude in centimeters + .rth_home_altitude = 0, // altitude in centimeters + .rth_abort_threshold = 50000, // centimeters - 500m should be safe for all aircraft + .max_terrain_follow_altitude = 100, // max altitude in centimeters in terrain following mode }, // MC-specific .mc = { - .max_bank_angle = 30, // 30 deg + .max_bank_angle = 30, // degrees .hover_throttle = 1500, - .auto_disarm_delay = 2000, + .auto_disarm_delay = 2000, // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed .braking_speed_threshold = 100, // Braking can become active above 1m/s .braking_disengage_speed = 75, // Stop when speed goes below 0.75m/s .braking_timeout = 2000, // Timeout barking after 2s @@ -127,37 +130,39 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .braking_boost_speed_threshold = 150, // Boost can happen only above 1.5m/s .braking_boost_disengage_speed = 100, // Disable boost at 1m/s .braking_bank_angle = 40, // Max braking angle + .posDecelerationTime = 120, // posDecelerationTime * 100 + .posResponseExpo = 10, // posResponseExpo * 100 }, // Fixed wing .fw = { - .max_bank_angle = 20, // 30 deg - .max_climb_angle = 20, - .max_dive_angle = 15, + .max_bank_angle = 35, // degrees + .max_climb_angle = 20, // degrees + .max_dive_angle = 15, // degrees .cruise_throttle = 1400, - .cruise_speed = 0, // cm/s + .cruise_speed = 0, // cm/s .max_throttle = 1700, .min_throttle = 1200, - .pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle) - .loiter_radius = 5000, // 50m + .pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle) + .loiter_radius = 5000, // 50m //Fixed wing landing - .land_dive_angle = 2, // 2 degrees dive by default + .land_dive_angle = 2, // 2 degrees dive by default // Fixed wing launch - .launch_velocity_thresh = 300, // 3 m/s - .launch_accel_thresh = 1.9f * 981, // cm/s/s (1.9*G) - .launch_time_thresh = 40, // 40ms + .launch_velocity_thresh = 300, // 3 m/s + .launch_accel_thresh = 1.9f * 981, // cm/s/s (1.9*G) + .launch_time_thresh = 40, // 40ms .launch_throttle = 1700, - .launch_idle_throttle = 1000, // Motor idle or MOTOR_STOP - .launch_motor_timer = 500, // ms - .launch_motor_spinup_time = 100, // ms, time to gredually increase throttle from idle to launch - .launch_min_time = 0, // ms, min time in launch mode - .launch_timeout = 5000, // ms, timeout for launch procedure - .launch_max_altitude = 0, // cm, altitude where to consider launch ended - .launch_climb_angle = 18, // 18 degrees - .launch_max_angle = 45, // 45 deg - .cruise_yaw_rate = 20, // 20dps + .launch_idle_throttle = 1000, // Motor idle or MOTOR_STOP + .launch_motor_timer = 500, // ms + .launch_motor_spinup_time = 100, // ms, time to gredually increase throttle from idle to launch + .launch_min_time = 0, // ms, min time in launch mode + .launch_timeout = 5000, // ms, timeout for launch procedure + .launch_max_altitude = 0, // cm, altitude where to consider launch ended + .launch_climb_angle = 18, // 18 degrees + .launch_max_angle = 45, // 45 deg + .cruise_yaw_rate = 20, // 20dps .allow_manual_thr_increase = false } ); @@ -180,6 +185,7 @@ uint16_t navEPV; int16_t navAccNEU[3]; #endif +static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode); static void updateDesiredRTHAltitude(void); static void resetAltitudeController(bool useTerrainFollowing); static void resetPositionController(void); @@ -192,6 +198,7 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos void calculateInitialHoldPosition(fpVector3_t * pos); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance); void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance); +static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome); void initializeRTHSanityChecker(const fpVector3_t * pos); bool validateRTHSanityChecker(void); @@ -473,6 +480,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, } @@ -1105,48 +1113,57 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n // If we have valid pos sensor OR we are configured to ignore GPS loss if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) { - const float rthAltitudeMargin = STATE(FIXED_WING) ? - MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)) : // Airplane - MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)); // Copters + const uint8_t rthClimbMarginPercent = STATE(FIXED_WING) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT; + const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); + + // If we reached desired initial RTH altitude or we don't want to climb first + if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) { - if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homeWaypointAbove.pos.z) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) { // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance if (STATE(FIXED_WING)) { initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); } + // Save initial home distance for future use + posControl.rthState.rthInitialDistance = posControl.homeDistance; + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL); + + if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) { + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); + } + else { + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + } + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME - } - else { + + } else { + + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL); + /* For multi-rotors execute sanity check during initial ascent as well */ - if (!STATE(FIXED_WING)) { - if (!validateRTHSanityChecker()) { - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } + if (!STATE(FIXED_WING) && !validateRTHSanityChecker()) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } // Climb to safe altitude and turn to correct direction if (STATE(FIXED_WING)) { - fpVector3_t pos = posControl.homeWaypointAbove.pos; - pos.z += FW_RTH_CLIMB_OVERSHOOT_CM; - - setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z); - } - else { + tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM; + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); + } else { // Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach // it in a reasonable time. Immediately after we finish this phase - target the original altitude. - fpVector3_t pos = posControl.homeWaypointAbove.pos; - pos.z += MR_RTH_CLIMB_OVERSHOOT_CM; + tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM; if (navConfig()->general.flags.rth_tail_first) { - setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); - } - else { - setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); + } else { + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); } } return NAV_FSM_EVENT_NONE; + } } /* Position sensor failure timeout - land */ @@ -1165,9 +1182,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio // If we have position sensor - continue home if ((posControl.flags.estPosStatus >= EST_USABLE)) { - if (isWaypointReached(&posControl.homeWaypointAbove, true)) { + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); + + if (isWaypointPositionReached(tmpHomePos, true)) { // Successfully reached position target - update XYZ-position - setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING } else if (!validateRTHSanityChecker()) { @@ -1175,13 +1194,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } else { - // Update XYZ-position target - if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) { - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); - } - else { - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); - } + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); return NAV_FSM_EVENT_NONE; } } @@ -1213,7 +1226,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; } else { - if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { + if (ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { resetLandingDetector(); updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; @@ -1223,7 +1236,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } else { - setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_NONE; } } @@ -1240,17 +1254,22 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na if (!(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout())) return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - if ((navConfig()->general.rth_home_altitude) && (posControl.desiredState.pos.z != navConfig()->general.rth_home_altitude)) { - int8_t altitudeChangeDirection = navConfig()->general.rth_home_altitude > posControl.homeWaypointAbove.pos.z ? 1 : -1; - float timeToReachHomeAltitude = altitudeChangeDirection * (navConfig()->general.rth_home_altitude - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate; + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER); + + if (navConfig()->general.rth_home_altitude) { + float timeToReachHomeAltitude = fabsf(tmpHomePos->z - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate; + if (timeToReachHomeAltitude < 1) { // we almost reached the target home altitude so set the desired altitude to the desired home altitude - posControl.homeWaypointAbove.pos.z = navConfig()->general.rth_home_altitude; - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z); + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); } else { + float altitudeChangeDirection = tmpHomePos->z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); } } + else { + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); + } return NAV_FSM_EVENT_NONE; } @@ -1280,8 +1299,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f); } else { + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND); + // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm. - float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homePosition.pos.z - navConfig()->general.land_slowdown_minalt) + float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - tmpHomePos->z - navConfig()->general.land_slowdown_minalt) / (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f); @@ -1351,8 +1372,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav case NAV_WP_ACTION_RTH: default: + posControl.rthState.rthInitialDistance = posControl.homeDistance; initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); - calculateAndSetActiveWaypointToLocalPosition(&posControl.homeWaypointAbove.pos); + calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL)); return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS }; } @@ -1363,19 +1385,25 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na // If no position sensor available - land immediately if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) { - const bool isDoingRTH = (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH); - switch (posControl.waypointList[posControl.activeWaypointIndex].action) { case NAV_WP_ACTION_WAYPOINT: - case NAV_WP_ACTION_RTH: default: - if (isWaypointReached(&posControl.activeWaypoint, isDoingRTH) || isWaypointMissed(&posControl.activeWaypoint)) { - // Waypoint reached + if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) { + return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED + } + else { + setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); + return NAV_FSM_EVENT_NONE; // will re-process state in >10ms + } + break; + + case NAV_WP_ACTION_RTH: + if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) { return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED } else { - // Update XY-position target to active waypoint setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); + setDesiredPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL), 0, NAV_POS_UPDATE_Z); return NAV_FSM_EVENT_NONE; // will re-process state in >10ms } break; @@ -1624,6 +1652,49 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) } } +static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) +{ + posControl.rthState.homeTmpWaypoint = posControl.rthState.homePosition.pos; + + switch (mode) { + case RTH_HOME_ENROUTE_INITIAL: + posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude; + break; + + case RTH_HOME_ENROUTE_PROPORTIONAL: + { + float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0); + if (rthTotalDistanceToTravel >= 100) { + float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f); + posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled)); + } + else { + posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude; + } + } + break; + + case RTH_HOME_ENROUTE_FINAL: + posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude; + break; + + case RTH_HOME_FINAL_HOVER: + if (navConfig()->general.rth_home_altitude) { + posControl.rthState.homeTmpWaypoint.z = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude; + } + else { + // If home altitude not defined - fall back to final ENROUTE altitude + posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude; + } + break; + + case RTH_HOME_FINAL_LAND: + break; + } + + return &posControl.rthState.homeTmpWaypoint; +} + /*----------------------------------------------------------- * Float point PID-controller implementation *-----------------------------------------------------------*/ @@ -1632,7 +1703,7 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) // http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler) { - float newProportional, newDerivative; + float newProportional, newDerivative, newFeedForward; float error = setpoint - measurement; /* P-term */ @@ -1654,19 +1725,29 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu pid->last_input = measurement; } - newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, NAV_DTERM_CUT_HZ, dt) * gainScaler; + if (pid->dTermLpfHz > 0) { + newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt) * gainScaler; + } else { + newDerivative = pid->param.kD * newDerivative; + } if (pidFlags & PID_ZERO_INTEGRATOR) { pid->integrator = 0.0f; } + /* + * Compute FeedForward parameter + */ + newFeedForward = setpoint * pid->param.kFF * gainScaler; + /* Pre-calculate output and limit it if actuator is saturating */ - const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative; + const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward; const float outValConstrained = constrainf(outVal, outMin, outMax); pid->proportional = newProportional; pid->integral = pid->integrator; pid->derivative = newDerivative; + pid->feedForward = newFeedForward; pid->output_constrained = outValConstrained; /* Update I-term */ @@ -1692,7 +1773,6 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f); } - void navPidReset(pidController_t *pid) { pid->reset = true; @@ -1701,16 +1781,17 @@ void navPidReset(pidController_t *pid) pid->derivative = 0.0f; pid->integrator = 0.0f; pid->last_input = 0.0f; - pid->dterm_filter_state.state = 0.0f; - pid->dterm_filter_state.RC = 0.0f; + pid->feedForward = 0.0f; + pt1FilterReset(&pid->dterm_filter_state, 0.0f); pid->output_constrained = 0.0f; } -void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD) +void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz) { pid->param.kP = _kP; pid->param.kI = _kI; pid->param.kD = _kD; + pid->param.kFF = _kFF; if (_kI > 1e-6f && _kP > 1e-6f) { float Ti = _kP / _kI; @@ -1721,18 +1802,10 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD) pid->param.kI = 0.0; pid->param.kT = 0.0; } - + pid->dTermLpfHz = _dTermLpfHz; navPidReset(pid); } -/*----------------------------------------------------------- - * Float point P-controller implementation - *-----------------------------------------------------------*/ -void navPInit(pController_t *p, float _kP) -{ - p->param.kP = _kP; -} - /*----------------------------------------------------------- * Detects if thrust vector is facing downwards *-----------------------------------------------------------*/ @@ -1870,22 +1943,22 @@ void updateActualHeading(bool headingValid, int32_t newHeading) */ navigationEstimateStatus_e newEstHeading = headingValid ? EST_TRUSTED : EST_NONE; if (newEstHeading >= EST_USABLE && posControl.flags.estHeadingStatus < EST_USABLE && - (posControl.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) && - (posControl.homeFlags & NAV_HOME_VALID_HEADING) == 0) { + (posControl.rthState.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) && + (posControl.rthState.homeFlags & NAV_HOME_VALID_HEADING) == 0) { // Home was stored using the fake heading (assuming boot as 0deg). Calculate // the offset from the fake to the actual yaw and apply the same rotation // to the home point. int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw; - posControl.homePosition.yaw += fakeToRealYawOffset; - if (posControl.homePosition.yaw < 0) { - posControl.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360); + posControl.rthState.homePosition.yaw += fakeToRealYawOffset; + if (posControl.rthState.homePosition.yaw < 0) { + posControl.rthState.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360); } - if (posControl.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) { - posControl.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360); + if (posControl.rthState.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) { + posControl.rthState.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360); } - posControl.homeFlags |= NAV_HOME_VALID_HEADING; + posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING; } posControl.actualState.yaw = newHeading; posControl.flags.estHeadingStatus = newEstHeading; @@ -1964,10 +2037,10 @@ bool isWaypointMissed(const navWaypointPosition_t * waypoint) return ABS(bearingError) > 10000; // TRUE if we passed the waypoint by 100 degrees } -bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome) +static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome) { // We consider waypoint reached if within specified radius - const uint32_t wpDistance = calculateDistanceToDestination(&waypoint->pos); + const uint32_t wpDistance = calculateDistanceToDestination(pos); if (STATE(FIXED_WING) && isWaypointHome) { // Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check @@ -1979,27 +2052,22 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp } } +bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome) +{ + return isWaypointPositionReached(&waypoint->pos, isWaypointHome); +} + static void updateHomePositionCompatibility(void) { - geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.homePosition.pos); + geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos); GPS_distanceToHome = posControl.homeDistance / 100; GPS_directionToHome = posControl.homeDirection / 100; } -float RTHAltitude() { - switch (navConfig()->general.flags.rth_alt_control_mode) { - case NAV_RTH_NO_ALT: - return(posControl.actualState.abs.pos.z); - case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin - return(posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude); - case NAV_RTH_MAX_ALT: - return(MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z)); - case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home - return(MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z)); - case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home - default: - return(posControl.homePosition.pos.z + navConfig()->general.rth_altitude); - } +// Backdoor for RTH estimator +float getFinalRTHAltitude(void) +{ + return posControl.rthState.rthFinalAltitude; } /*----------------------------------------------------------- @@ -2009,11 +2077,41 @@ static void updateDesiredRTHAltitude(void) { if (ARMING_FLAG(ARMED)) { if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) { - posControl.homeWaypointAbove.pos.z = RTHAltitude(); + switch (navConfig()->general.flags.rth_alt_control_mode) { + case NAV_RTH_NO_ALT: + posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; + posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; + break; + + case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin + posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude; + posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; + break; + + case NAV_RTH_MAX_ALT: + posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude, posControl.actualState.abs.pos.z); + posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; + break; + + case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home + posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z); + posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; + break; + + case NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT: + posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z); + posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude; + break; + + case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home + default: + posControl.rthState.rthInitialAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude; + posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; + } } - } - else { - posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z; + } else { + posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; + posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z; } } @@ -2026,7 +2124,7 @@ void initializeRTHSanityChecker(const fpVector3_t * pos) posControl.rthSanityChecker.lastCheckTime = currentTimeMs; posControl.rthSanityChecker.initialPosition = *pos; - posControl.rthSanityChecker.minimalDistanceToHome = calculateDistanceToDestination(&posControl.homePosition.pos); + posControl.rthSanityChecker.minimalDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos); } bool validateRTHSanityChecker(void) @@ -2041,7 +2139,7 @@ bool validateRTHSanityChecker(void) // Check at 10Hz rate if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) { - const float currentDistanceToHome = calculateDistanceToDestination(&posControl.homePosition.pos); + const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos); if (currentDistanceToHome < posControl.rthSanityChecker.minimalDistanceToHome) { posControl.rthSanityChecker.minimalDistanceToHome = currentDistanceToHome; @@ -2064,33 +2162,33 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t { // XY-position if ((useMask & NAV_POS_UPDATE_XY) != 0) { - posControl.homePosition.pos.x = pos->x; - posControl.homePosition.pos.y = pos->y; + posControl.rthState.homePosition.pos.x = pos->x; + posControl.rthState.homePosition.pos.y = pos->y; if (homeFlags & NAV_HOME_VALID_XY) { - posControl.homeFlags |= NAV_HOME_VALID_XY; + posControl.rthState.homeFlags |= NAV_HOME_VALID_XY; } else { - posControl.homeFlags &= ~NAV_HOME_VALID_XY; + posControl.rthState.homeFlags &= ~NAV_HOME_VALID_XY; } } // Z-position if ((useMask & NAV_POS_UPDATE_Z) != 0) { - posControl.homePosition.pos.z = pos->z; + posControl.rthState.homePosition.pos.z = pos->z; if (homeFlags & NAV_HOME_VALID_Z) { - posControl.homeFlags |= NAV_HOME_VALID_Z; + posControl.rthState.homeFlags |= NAV_HOME_VALID_Z; } else { - posControl.homeFlags &= ~NAV_HOME_VALID_Z; + posControl.rthState.homeFlags &= ~NAV_HOME_VALID_Z; } } // Heading if ((useMask & NAV_POS_UPDATE_HEADING) != 0) { // Heading - posControl.homePosition.yaw = yaw; + posControl.rthState.homePosition.yaw = yaw; if (homeFlags & NAV_HOME_VALID_HEADING) { - posControl.homeFlags |= NAV_HOME_VALID_HEADING; + posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING; } else { - posControl.homeFlags &= ~NAV_HOME_VALID_HEADING; + posControl.rthState.homeFlags &= ~NAV_HOME_VALID_HEADING; } } @@ -2098,7 +2196,6 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t posControl.homeDirection = 0; // Update target RTH altitude as a waypoint above home - posControl.homeWaypointAbove = posControl.homePosition; updateDesiredRTHAltitude(); updateHomePositionCompatibility(); @@ -2129,7 +2226,7 @@ void updateHomePosition(void) if (!ARMING_FLAG(ARMED)) { if (posControl.flags.estPosStatus >= EST_USABLE) { const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z; - bool setHome = (posControl.homeFlags & validHomeFlags) != validHomeFlags; + bool setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags; switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) { case NAV_RESET_NEVER: break; @@ -2162,8 +2259,9 @@ void updateHomePosition(void) // Update distance and direction to home if armed (home is not updated when armed) if (STATE(GPS_FIX_HOME)) { - posControl.homeDistance = calculateDistanceToDestination(&posControl.homePosition.pos); - posControl.homeDirection = calculateBearingToDestination(&posControl.homePosition.pos); + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND); + posControl.homeDistance = calculateDistanceToDestination(tmpHomePos); + posControl.homeDirection = calculateBearingToDestination(tmpHomePos); updateHomePositionCompatibility(); } } @@ -2184,7 +2282,7 @@ static void updateNavigationFlightStatistics(void) } } -int32_t getTotalTravelDistance(void) +uint32_t getTotalTravelDistance(void) { return lrintf(posControl.totalTripDistance); } @@ -2681,6 +2779,7 @@ void applyWaypointNavigationAndAltitudeHold(void) if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0); if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1); if (posControl.flags.estPosStatus == EST_TRUSTED) navFlags |= (1 << 2); + if (posControl.flags.isTerrainFollowEnabled) navFlags |= (1 << 3); #if defined(NAV_GPS_GLITCH_DETECTION) if (isGPSGlitchDetected()) navFlags |= (1 << 4); #endif @@ -2915,23 +3014,34 @@ bool navigationTerrainFollowingEnabled(void) return posControl.flags.isTerrainFollowEnabled; } -bool navigationBlockArming(void) +navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) { const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE)); const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)); - bool shouldBlockArming = false; - if (!navConfig()->general.flags.extra_arming_safety) - return false; + if (usedBypass) { + *usedBypass = false; + } + + if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_OFF) { + return NAV_ARMING_BLOCKER_NONE; + } // Apply extra arming safety only if pilot has any of GPS modes configured if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME))) { - shouldBlockArming = true; + if (navConfig()->general.flags.extra_arming_safety == NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS && + (STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED) || rxGetChannelValue(YAW) > 1750)) { + if (usedBypass) { + *usedBypass = true; + } + return NAV_ARMING_BLOCKER_NONE; + } + return NAV_ARMING_BLOCKER_MISSING_GPS_FIX; } // Don't allow arming if any of NAV modes is active if (!ARMING_FLAG(ARMED) && navBoxModesEnabled && !navLaunchComboModesEnabled) { - shouldBlockArming = true; + return NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE; } // Don't allow arming if first waypoint is farther than configured safe distance @@ -2942,14 +3052,13 @@ bool navigationBlockArming(void) const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance; if (navWpMissionStartTooFar) { - shouldBlockArming = true; + return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR; } } - return shouldBlockArming; + return NAV_ARMING_BLOCKER_NONE; } - bool navigationPositionEstimateIsHealthy(void) { return (posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME); @@ -3017,41 +3126,70 @@ void navigationUsePIDs(void) { /** Multicopter PIDs */ // Brake time parameter - posControl.posDecelerationTime = (float)pidProfile()->bank_mc.pid[PID_POS_XY].I / 100.0f; + posControl.posDecelerationTime = (float)navConfig()->mc.posDecelerationTime / 100.0f; // Position controller expo (taret vel expo for MC) - posControl.posResponseExpo = constrainf((float)pidProfile()->bank_mc.pid[PID_POS_XY].D / 100.0f, 0.0f, 1.0f); + posControl.posResponseExpo = constrainf((float)navConfig()->mc.posResponseExpo / 100.0f, 0.0f, 1.0f); // Initialize position hold P-controller for (int axis = 0; axis < 2; axis++) { - navPInit(&posControl.pids.pos[axis], (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f); + navPidInit( + &posControl.pids.pos[axis], + (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f, + 0.0f, + 0.0f, + 0.0f, + NAV_DTERM_CUT_HZ + ); navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f, (float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f, - (float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f); + (float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f, + (float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f, + pidProfile()->navVelXyDTermLpfHz + ); } // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z - navPInit(&posControl.pids.pos[Z], (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f); + navPidInit( + &posControl.pids.pos[Z], + (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f, + 0.0f, + 0.0f, + 0.0f, + NAV_DTERM_CUT_HZ + ); navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f, (float)pidProfile()->bank_mc.pid[PID_VEL_Z].I / 20.0f, - (float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f); + (float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f, + 0.0f, + NAV_DTERM_CUT_HZ + ); // Initialize surface tracking PID navPidInit(&posControl.pids.surface, 2.0f, 0.0f, - 0.0f); + 0.0f, + 0.0f, + NAV_DTERM_CUT_HZ + ); /** Airplane PIDs */ // Initialize fixed wing PID controllers navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->bank_fw.pid[PID_POS_XY].P / 100.0f, (float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f); + (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f, + 0.0f, + NAV_DTERM_CUT_HZ + ); navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f, (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f); + (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f, + 0.0f, + NAV_DTERM_CUT_HZ + ); } void navigationInit(void) @@ -3102,6 +3240,7 @@ float getEstimatedActualPosition(int axis) *-----------------------------------------------------------*/ void activateForcedRTH(void) { + abortFixedWingLaunch(); posControl.flags.forcedRTHActivated = true; navProcessFSMEvents(selectNavEventFromBoxModeInput()); } @@ -3154,19 +3293,21 @@ bool navigationRTHAllowsLanding(void) (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE)); } -bool isNavLaunchEnabled(void) +bool FAST_CODE isNavLaunchEnabled(void) { return IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH); } int32_t navigationGetHomeHeading(void) { - return posControl.homePosition.yaw; + return posControl.rthState.homePosition.yaw; } // returns m/s float calculateAverageSpeed() { - return (float)getTotalTravelDistance() / (getFlightTime() * 100); + float flightTime = getFlightTime(); + if (flightTime == 0.0f) return 0; + return (float)getTotalTravelDistance() / (flightTime * 100); } const navigationPIDControllers_t* getNavigationPIDControllers(void) { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 6a7c9d19e99..d8e81ca16d1 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -32,7 +32,7 @@ /* GPS Home location data */ extern gpsLocation_t GPS_home; -extern uint16_t GPS_distanceToHome; // distance to home point in meters +extern uint32_t GPS_distanceToHome; // distance to home point in meters extern int16_t GPS_directionToHome; // direction to home point in degrees extern bool autoThrottleManuallyIncreased; @@ -49,6 +49,8 @@ void onNewGPSData(void); #define NAV_MAX_WAYPOINTS 15 #endif +#define NAV_ACCEL_CUTOFF_FREQUENCY_HZ 2 // low-pass filter on XY-acceleration target + enum { NAV_GPS_ATTI = 0, // Pitch/roll stick controls attitude (pitch/roll lean angles) NAV_GPS_CRUISE = 1 // Pitch/roll stick controls velocity (forward/right speed) @@ -61,11 +63,13 @@ enum { }; enum { - NAV_RTH_NO_ALT = 0, // Maintain current altitude - NAV_RTH_EXTRA_ALT = 1, // Maintain current altitude + predefined safety margin - NAV_RTH_CONST_ALT = 2, // Climb/descend to predefined altitude - NAV_RTH_MAX_ALT = 3, // Track maximum altitude and climb to it when RTH - NAV_RTH_AT_LEAST_ALT = 4, // Climb to predefined altitude if below it + NAV_RTH_NO_ALT = 0, // Maintain current altitude + NAV_RTH_EXTRA_ALT = 1, // Maintain current altitude + predefined safety margin + NAV_RTH_CONST_ALT = 2, // Climb/descend to predefined altitude + NAV_RTH_MAX_ALT = 3, // Track maximum altitude and climb to it when RTH + NAV_RTH_AT_LEAST_ALT = 4, // Climb to predefined altitude if below it + NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT = 5, // Climb to predefined altitude if below it, + // descend linearly to reach home at predefined altitude if above it }; enum { @@ -86,6 +90,19 @@ typedef enum { NAV_RTH_ALLOW_LANDING_FS_ONLY = 2, // Allow landing only if RTH was triggered by failsafe } navRTHAllowLanding_e; +typedef enum { + NAV_EXTRA_ARMING_SAFETY_OFF = 0, + NAV_EXTRA_ARMING_SAFETY_ON = 1, + NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS = 2, // Allow disabling by holding THR+YAW low +} navExtraArmingSafety_e; + +typedef enum { + NAV_ARMING_BLOCKER_NONE = 0, + NAV_ARMING_BLOCKER_MISSING_GPS_FIX = 1, + NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE = 2, + NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR = 3, +} navArmingBlocker_e; + typedef struct positionEstimationConfig_s { uint8_t automatic_mag_declination; uint8_t reset_altitude_type; // from nav_reset_type_e @@ -114,6 +131,7 @@ typedef struct positionEstimationConfig_s { float w_xy_res_v; float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. + float w_xyz_acc_p; float max_eph_epv; // Max estimated position error acceptable for estimation (cm) float baro_epv; // Baro position error @@ -126,7 +144,7 @@ typedef struct navConfig_s { struct { struct { uint8_t use_thr_mid_for_althold; // Don't remember throttle when althold was initiated, assume that throttle is at Thr Mid = zero climb rate - uint8_t extra_arming_safety; // Forcibly apply 100% throttle tilt compensation + uint8_t extra_arming_safety; // from navExtraArmingSafety_e uint8_t user_control_mode; // NAV_GPS_ATTI or NAV_GPS_CRUISE uint8_t rth_alt_control_mode; // Controls the logic for choosing the RTH altitude uint8_t rth_climb_first; // Controls the logic for initial RTH climbout @@ -167,6 +185,8 @@ typedef struct navConfig_s { uint16_t braking_boost_speed_threshold; // Above this speed braking boost mode can engage uint16_t braking_boost_disengage_speed; // Below this speed braking boost will disengage uint8_t braking_bank_angle; // Max angle [deg] that MR is allowed duing braking boost phase + uint8_t posDecelerationTime; // Brake time parameter + uint8_t posResponseExpo; // Position controller expo (taret vel expo for MC) } mc; struct { @@ -225,6 +245,21 @@ typedef struct { uint8_t flag; } navWaypoint_t; +typedef struct radar_pois_s { + gpsLocation_t gps; + uint8_t state; + uint16_t heading; // ° + uint16_t speed; // cm/s + uint8_t lq; // from 0 t o 4 + uint16_t distance; // m + int16_t altitude; // m + int16_t direction; // ° +} radar_pois_t; + +#define RADAR_MAX_POIS 5 + +extern radar_pois_t radar_pois[RADAR_MAX_POIS]; + typedef struct { fpVector3_t pos; int32_t yaw; // deg * 100 @@ -240,6 +275,7 @@ typedef struct { float kI; float kD; float kT; // Tracking gain (anti-windup) + float kFF; // FeedForward Component } pidControllerParam_t; typedef struct { @@ -250,23 +286,20 @@ typedef struct { bool reset; pidControllerParam_t param; pt1Filter_t dterm_filter_state; // last derivative for low-pass filter + float dTermLpfHz; // dTerm low pass filter cutoff frequency float integrator; // integrator value float last_input; // last input for derivative float integral; // used integral value in output float proportional; // used proportional value in output float derivative; // used derivative value in output + float feedForward; // used FeedForward value in output float output_constrained; // controller output constrained } pidController_t; -typedef struct { - pControllerParam_t param; - float output_constrained; -} pController_t; - typedef struct navigationPIDControllers_s { /* Multicopter PIDs */ - pController_t pos[XYZ_AXIS_COUNT]; + pidController_t pos[XYZ_AXIS_COUNT]; pidController_t vel[XYZ_AXIS_COUNT]; pidController_t surface; @@ -350,7 +383,10 @@ bool navigationRequiresAngleMode(void); bool navigationRequiresThrottleTiltCompensation(void); bool navigationRequiresTurnAssistance(void); int8_t navigationGetHeadingControlState(void); -bool navigationBlockArming(void); +// Returns wether arming is blocked by the navigation system. +// If usedBypass is provided, it will indicate wether any checks +// were bypassed due to user input. +navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass); bool navigationPositionEstimateIsHealthy(void); bool navIsCalibrationComplete(void); bool navigationTerrainFollowingEnabled(void); @@ -368,7 +404,7 @@ typedef struct { float getEstimatedActualVelocity(int axis); float getEstimatedActualPosition(int axis); -int32_t getTotalTravelDistance(void); +uint32_t getTotalTravelDistance(void); void getEstimatedPositionAndVelocity(navPositionAndVelocity_t * pos); /* Waypoint list access functions */ @@ -380,7 +416,8 @@ void resetWaypointList(void); bool loadNonVolatileWaypointList(void); bool saveNonVolatileWaypointList(void); -float RTHAltitude(); +float getFinalRTHAltitude(void); +int16_t fixedWingPitchToThrottleCorrection(int16_t pitch); /* Geodetic functions */ typedef enum { @@ -436,7 +473,7 @@ bool isNavLaunchEnabled(void); bool isFixedWingLaunchDetected(void); bool isFixedWingLaunchFinishedOrAborted(void); -float calculateAverageSpeed(); +float calculateAverageSpeed(void); const navigationPIDControllers_t* getNavigationPIDControllers(void); diff --git a/src/main/navigation/navigation_declination_gen.c b/src/main/navigation/navigation_declination_gen.c new file mode 100644 index 00000000000..04e1c731726 --- /dev/null +++ b/src/main/navigation/navigation_declination_gen.c @@ -0,0 +1,63 @@ +/* this file is automatically generated by src/utils/declination.py - DO NOT EDIT! */ + + +/* Updated on 2019-05-16 19:59:45.672184 */ + + +#include + + + +#if defined(NAV_AUTO_MAG_DECLINATION_PRECISE) +#define SAMPLING_RES 10.00000f +#define SAMPLING_MIN_LON -180.00000f +#define SAMPLING_MAX_LON 180.00000f +#define SAMPLING_MIN_LAT -90.00000f +#define SAMPLING_MAX_LAT 90.00000f + +static const float declination_table[19][37] = { + {149.21545f,139.21545f,129.21545f,119.21545f,109.21545f,99.21545f,89.21545f,79.21545f,69.21545f,59.21545f,49.21545f,39.21545f,29.21545f,19.21545f,9.21545f,-0.78455f,-10.78455f,-20.78455f,-30.78455f,-40.78455f,-50.78455f,-60.78455f,-70.78455f,-80.78455f,-90.78455f,-100.78455f,-110.78455f,-120.78455f,-130.78455f,-140.78455f,-150.78455f,-160.78455f,-170.78455f,179.21545f,169.21545f,159.21545f,149.21545f}, + {129.49173f,117.24379f,106.10409f,95.92286f,86.51411f,77.69583f,69.30955f,61.22803f,53.35622f,45.62814f,38.00071f,30.44524f,22.93795f,15.45138f,7.94874f,0.38309f,-7.29841f,-15.14665f,-23.20230f,-31.49030f,-40.01947f,-48.78754f,-57.79007f,-67.03110f,-76.53295f,-86.34401f,-96.54357f,-107.24276f,-118.57946f,-130.70159f,-143.72988f,-157.69312f,-172.44642f,172.37735f,157.31228f,142.89420f,129.49173f}, + {85.58549f,77.67225f,71.30884f,65.86377f,60.92589f,56.17990f,51.37001f,46.30480f,40.87538f,35.06788f,28.95999f,22.69753f,16.44959f,10.34723f,4.42361f,-1.41547f,-7.36729f,-13.65814f,-20.45428f,-27.80115f,-35.61613f,-43.73164f,-51.96263f,-60.16701f,-68.28139f,-76.33426f,-84.45291f,-92.88710f,-102.08596f,-112.90534f,-127.14324f,-148.72029f,177.01697f,138.42469f,112.13402f,96.22709f,85.58549f}, + {47.63660f,46.34868f,44.88668f,43.46724f,42.13334f,40.75578f,39.03991f,36.60567f,33.13133f,28.48297f,22.78165f,16.40811f,9.93011f,3.92458f,-1.26916f,-5.73344f,-9.95561f,-14.59663f,-20.17634f,-26.83800f,-34.30473f,-42.04900f,-49.54378f,-56.41737f,-62.46557f,-67.58445f,-71.67147f,-74.48888f,-75.41488f,-72.72854f,-60.65739f,-20.78150f,26.31817f,42.64478f,47.39885f,48.29568f,47.63660f}, + {30.97061f,31.18244f,30.92007f,30.51421f,30.19783f,30.07382f,29.96272f,29.32724f,27.44230f,23.71829f,17.99066f,10.70786f,2.93356f,-4.02123f,-9.25421f,-12.72420f,-15.16719f,-17.68585f,-21.36741f,-26.80526f,-33.62817f,-40.77079f,-47.22749f,-52.37678f,-55.85085f,-57.33789f,-56.38997f,-52.20324f,-43.65581f,-30.23589f,-13.76879f,1.84396f,13.87138f,22.01689f,27.05355f,29.80197f,30.97061f}, + {22.35701f,22.87747f,22.95380f,22.77246f,22.50130f,22.37009f,22.48347f,22.51381f,21.59756f,18.63829f,12.92222f,4.75452f,-4.29892f,-12.13023f,-17.44827f,-20.36076f,-21.71093f,-22.23838f,-22.95576f,-25.55066f,-30.56100f,-36.48539f,-41.58017f,-44.82620f,-45.65465f,-43.72977f,-38.84914f,-30.98997f,-21.09926f,-11.31468f,-3.00404f,3.97652f,9.93802f,14.84528f,18.57977f,21.04644f,22.35701f}, + {16.83374f,17.31701f,17.52984f,17.52512f,17.27497f,16.89826f,16.64631f,16.52197f,15.82359f,13.20018f,7.50544f,-1.01430f,-10.31063f,-17.86218f,-22.52054f,-24.77956f,-25.51811f,-24.67883f,-22.16922f,-20.17352f,-21.46713f,-25.48482f,-29.63408f,-31.91043f,-31.42054f,-28.23519f,-22.87573f,-15.95318f,-8.88587f,-3.41726f,0.43088f,3.87433f,7.44305f,10.85242f,13.73685f,15.75505f,16.83374f}, + {13.16653f,13.42584f,13.56856f,13.64914f,13.49908f,13.04415f,12.54312f,12.17294f,11.33249f,8.62333f,2.85124f,-5.49975f,-14.06214f,-20.49962f,-23.98868f,-24.97282f,-24.13471f,-21.32697f,-16.43179f,-11.33581f,-9.07326f,-10.72561f,-14.43596f,-17.29905f,-17.69300f,-15.77796f,-12.24267f,-7.56947f,-3.00400f,-0.11834f,1.39928f,3.14408f,5.63638f,8.32414f,10.72028f,12.39111f,13.16653f}, + {10.90437f,10.88236f,10.81163f,10.86511f,10.80010f,10.39894f,9.91702f,9.49494f,8.41184f,5.36760f,-0.47690f,-8.26124f,-15.66061f,-20.76090f,-22.78986f,-21.89942f,-18.89020f,-14.52921f,-9.53528f,-4.91597f,-1.91353f,-1.76484f,-4.25201f,-7.17489f,-8.55660f,-8.15241f,-6.39303f,-3.54345f,-0.64444f,0.81130f,1.15035f,2.11074f,4.17898f,6.59237f,8.80142f,10.33532f,10.90437f}, + {9.69156f,9.50282f,9.23338f,9.25605f,9.28344f,8.98186f,8.56848f,8.04846f,6.56771f,3.06569f,-2.75457f,-9.75616f,-15.91853f,-19.62874f,-20.10653f,-17.62100f,-13.38807f,-8.80249f,-4.83206f,-1.62618f,0.85458f,1.72352f,0.35339f,-2.00271f,-3.58526f,-3.93648f,-3.30170f,-1.78891f,-0.13334f,0.41999f,0.14853f,0.72192f,2.61852f,5.02104f,7.34668f,9.06648f,9.69156f}, + {8.99220f,9.02256f,8.80901f,8.93941f,9.15462f,8.99412f,8.49413f,7.54605f,5.38233f,1.29060f,-4.51973f,-10.72800f,-15.61393f,-17.87879f,-17.08037f,-13.87843f,-9.54744f,-5.34026f,-2.15287f,0.11672f,2.02473f,3.04733f,2.31093f,0.47960f,-0.96646f,-1.54925f,-1.53032f,-1.00772f,-0.42112f,-0.63497f,-1.37540f,-1.17754f,0.48492f,2.94585f,5.61932f,7.87900f,8.99220f}, + {8.04368f,8.88487f,9.24731f,9.76871f,10.30385f,10.32830f,9.60849f,7.94603f,4.81731f,-0.08975f,-6.10101f,-11.65411f,-15.25677f,-16.15822f,-14.52060f,-11.22653f,-7.24580f,-3.44558f,-0.61333f,1.25309f,2.76805f,3.73177f,3.36695f,1.97948f,0.74967f,0.13075f,-0.18979f,-0.42444f,-0.85420f,-1.93961f,-3.28866f,-3.62225f,-2.35247f,0.08639f,3.11990f,6.05468f,8.04368f}, + {6.44887f,8.52898f,10.00772f,11.25814f,12.19628f,12.38325f,11.43950f,9.05774f,4.87180f,-1.07081f,-7.58681f,-12.76972f,-15.34699f,-15.21292f,-13.06213f,-9.80318f,-6.10115f,-2.52848f,0.26062f,2.11110f,3.47313f,4.40612f,4.43031f,3.60979f,2.68625f,2.01876f,1.34276f,0.35243f,-1.18272f,-3.35038f,-5.50704f,-6.45531f,-5.60558f,-3.25332f,0.01287f,3.49652f,6.44887f}, + {4.61309f,7.91484f,10.67459f,12.86217f,14.28349f,14.59916f,13.44634f,10.43327f,5.19620f,-1.95110f,-9.25904f,-14.43995f,-16.45824f,-15.72227f,-13.22423f,-9.85034f,-6.14231f,-2.53072f,0.48676f,2.68447f,4.29956f,5.52570f,6.19623f,6.20084f,5.73301f,4.92006f,3.60085f,1.53163f,-1.39438f,-4.87688f,-7.94536f,-9.43097f,-8.85100f,-6.51552f,-3.06017f,0.84306f,4.61309f}, + {3.21875f,7.41586f,11.19354f,14.28261f,16.32057f,16.89633f,15.56524f,11.80300f,5.20653f,-3.55470f,-11.97809f,-17.43029f,-19.22616f,-18.17302f,-15.37952f,-11.69571f,-7.65025f,-3.63286f,-0.01295f,2.99661f,5.46716f,7.56207f,9.25413f,10.31810f,10.49165f,9.54146f,7.26006f,3.54973f,-1.33752f,-6.50635f,-10.55663f,-12.38793f,-11.79132f,-9.25449f,-5.49875f,-1.18292f,3.21875f}, + {2.52051f,7.33138f,11.79037f,15.58977f,18.29190f,19.30865f,17.85567f,12.95067f,3.98293f,-7.53431f,-17.54806f,-23.19596f,-24.60407f,-23.07402f,-19.77569f,-15.49637f,-10.73649f,-5.85097f,-1.12175f,3.26826f,7.26895f,10.88888f,14.03678f,16.41453f,17.52792f,16.76756f,13.54324f,7.61980f,-0.22282f,-7.89770f,-13.19396f,-15.21847f,-14.33135f,-11.37454f,-7.18383f,-2.40863f,2.52051f}, + {2.02983f,7.36989f,12.37081f,16.71056f,19.89761f,21.11515f,18.97400f,11.40232f,-2.70274f,-18.81527f,-29.66002f,-33.88571f,-33.57644f,-30.58814f,-26.04521f,-20.59815f,-14.64262f,-8.44459f,-2.20066f,3.93897f,9.85468f,15.42277f,20.45728f,24.64081f,27.44761f,28.04182f,25.18387f,17.54097f,5.49796f,-6.72811f,-14.58218f,-17.34691f,-16.39748f,-13.14509f,-8.58967f,-3.38880f,2.02983f}, + {0.42359f,5.67280f,10.45897f,14.18719f,15.84273f,13.44387f,3.31185f,-17.06908f,-37.86930f,-48.41039f,-50.74957f,-48.66334f,-44.20871f,-38.40471f,-31.78712f,-24.66267f,-17.22360f,-9.60278f,-1.90231f,5.78964f,13.38658f,20.79088f,27.87654f,34.46122f,40.25514f,44.75763f,47.03469f,45.25758f,36.18138f,17.77476f,-1.60905f,-12.11869f,-14.89059f,-13.37813f,-9.65915f,-4.84192f,0.42359f}, + {-178.76222f,-168.76222f,-158.76222f,-148.76222f,-138.76222f,-128.76222f,-118.76222f,-108.76222f,-98.76222f,-88.76222f,-78.76222f,-68.76222f,-58.76222f,-48.76222f,-38.76222f,-28.76222f,-18.76222f,-8.76222f,1.23778f,11.23778f,21.23778f,31.23778f,41.23778f,51.23778f,61.23778f,71.23778f,81.23778f,91.23778f,101.23778f,111.23778f,121.23778f,131.23778f,141.23778f,151.23778f,161.23778f,171.23778f,-178.76222f} +}; + +#else /* !NAV_AUTO_MAG_DECLINATION_PRECISE */ +#define SAMPLING_RES 10.00000f +#define SAMPLING_MIN_LON -180.00000f +#define SAMPLING_MAX_LON 180.00000f +#define SAMPLING_MIN_LAT -60.00000f +#define SAMPLING_MAX_LAT 60.00000f + +static const int8_t declination_table[13][37] = { + {48,46,45,43,42,41,39,37,33,28,23,16,10,4,-1,-6,-10,-15,-20,-27,-34,-42,-50,-56,-62,-68,-72,-74,-75,-73,-61,-21,26,43,47,48,48}, + {31,31,31,31,30,30,30,29,27,24,18,11,3,-4,-9,-13,-15,-18,-21,-27,-34,-41,-47,-52,-56,-57,-56,-52,-44,-30,-14,2,14,22,27,30,31}, + {22,23,23,23,23,22,22,23,22,19,13,5,-4,-12,-17,-20,-22,-22,-23,-26,-31,-36,-42,-45,-46,-44,-39,-31,-21,-11,-3,4,10,15,19,21,22}, + {17,17,18,18,17,17,17,17,16,13,8,-1,-10,-18,-23,-25,-26,-25,-22,-20,-21,-25,-30,-32,-31,-28,-23,-16,-9,-3,0,4,7,11,14,16,17}, + {13,13,14,14,13,13,13,12,11,9,3,-5,-14,-20,-24,-25,-24,-21,-16,-11,-9,-11,-14,-17,-18,-16,-12,-8,-3,0,1,3,6,8,11,12,13}, + {11,11,11,11,11,10,10,9,8,5,0,-8,-16,-21,-23,-22,-19,-15,-10,-5,-2,-2,-4,-7,-9,-8,-6,-4,-1,1,1,2,4,7,9,10,11}, + {10,10,9,9,9,9,9,8,7,3,-3,-10,-16,-20,-20,-18,-13,-9,-5,-2,1,2,0,-2,-4,-4,-3,-2,0,0,0,1,3,5,7,9,10}, + {9,9,9,9,9,9,8,8,5,1,-5,-11,-16,-18,-17,-14,-10,-5,-2,0,2,3,2,0,-1,-2,-2,-1,0,-1,-1,-1,0,3,6,8,9}, + {8,9,9,10,10,10,10,8,5,0,-6,-12,-15,-16,-15,-11,-7,-3,-1,1,3,4,3,2,1,0,0,0,-1,-2,-3,-4,-2,0,3,6,8}, + {6,9,10,11,12,12,11,9,5,-1,-8,-13,-15,-15,-13,-10,-6,-3,0,2,3,4,4,4,3,2,1,0,-1,-3,-6,-6,-6,-3,0,3,6}, + {5,8,11,13,14,15,13,10,5,-2,-9,-14,-16,-16,-13,-10,-6,-3,0,3,4,6,6,6,6,5,4,2,-1,-5,-8,-9,-9,-7,-3,1,5}, + {3,7,11,14,16,17,16,12,5,-4,-12,-17,-19,-18,-15,-12,-8,-4,0,3,5,8,9,10,10,10,7,4,-1,-7,-11,-12,-12,-9,-5,-1,3}, + {3,7,12,16,18,19,18,13,4,-8,-18,-23,-25,-23,-20,-15,-11,-6,-1,3,7,11,14,16,18,17,14,8,0,-8,-13,-15,-14,-11,-7,-2,3} +}; + +#endif diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 99f058c21c9..ae475fedf18 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -432,6 +432,11 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) return throttleSpeedAdjustment; } +int16_t fixedWingPitchToThrottleCorrection(int16_t pitch) +{ + return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle; +} + void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs) { int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle; @@ -447,7 +452,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat // PITCH >0 dive, <0 climb int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle)); rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]); - int16_t throttleCorrection = DECIDEGREES_TO_DEGREES(pitchCorrection) * navConfig()->fw.pitch_to_throttle; + int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection); #ifdef NAV_FIXED_WING_LANDING if (navStateFlags & NAV_CTL_LAND) { diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 5613808e5cc..1ee58dbbee3 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -70,7 +70,7 @@ typedef struct FixedWingLaunchState_s { bool motorControlAllowed; } FixedWingLaunchState_t; -static FixedWingLaunchState_t launchState; +static EXTENDED_FASTRAM FixedWingLaunchState_t launchState; #define COS_MAX_LAUNCH_ANGLE 0.70710678f // cos(45), just to be safe #define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate @@ -106,7 +106,7 @@ void resetFixedWingLaunchController(timeUs_t currentTimeUs) launchState.motorControlAllowed = false; } -bool isFixedWingLaunchDetected(void) +bool FAST_CODE isFixedWingLaunchDetected(void) { return launchState.launchDetected; } @@ -117,7 +117,7 @@ void enableFixedWingLaunchController(timeUs_t currentTimeUs) launchState.motorControlAllowed = true; } -bool isFixedWingLaunchFinishedOrAborted(void) +bool FAST_CODE isFixedWingLaunchFinishedOrAborted(void) { return launchState.launchFinished; } diff --git a/src/main/navigation/navigation_geo.c b/src/main/navigation/navigation_geo.c index b6e9f3f8447..1c668e1d169 100755 --- a/src/main/navigation/navigation_geo.c +++ b/src/main/navigation/navigation_geo.c @@ -43,32 +43,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" - -#if defined(NAV_AUTO_MAG_DECLINATION) -/* Declination calculation code from PX4 project */ -/* set this always to the sampling in degrees for the table below */ -#define SAMPLING_RES 10.0f -#define SAMPLING_MIN_LAT -60.0f -#define SAMPLING_MAX_LAT 60.0f -#define SAMPLING_MIN_LON -180.0f -#define SAMPLING_MAX_LON 180.0f - -static const int8_t declination_table[13][37] = \ -{ - { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 }, - { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, - { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, - { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, - { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, - { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, - { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, - { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, - { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, - { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, - { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, - { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, - { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, -}; +#include "navigation/navigation_declination_gen.c" static float get_lookup_table_val(unsigned lat_index, unsigned lon_index) { @@ -131,7 +106,6 @@ float geoCalculateMagDeclination(const gpsLocation_t * llh) // degrees units return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min; } -#endif void geoSetOrigin(gpsOrigin_t *origin, const gpsLocation_t *llh, geoOriginResetMode_e resetMode) { diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index eb4ad2da7cc..2d4e4f21f85 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -75,14 +75,24 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) posControl.pids.pos[Z].output_constrained = targetVel; - // limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing RoC or RoD (only if vel is of the same sign) - // if we are decelerating - don't limit (allow better recovery from falling) - const bool isSameDirection = (signbit(targetVel) == signbit(posControl.desiredState.vel.z)) && (targetVel != 0) && (posControl.desiredState.vel.z != 0); - if (isSameDirection && (fabsf(targetVel) > fabsf(posControl.desiredState.vel.z))) { - const float maxVelDifference = US2S(deltaMicros) * (GRAVITY_CMSS / 5.0f); + // Limit max up/down acceleration target + const float smallVelChange = US2S(deltaMicros) * (GRAVITY_CMSS * 0.1f); + const float velTargetChange = targetVel - posControl.desiredState.vel.z; + + if (velTargetChange <= -smallVelChange) { + // Large & Negative - acceleration is _down_. We can't reach more than -1G in any possible condition. Hard limit to 0.8G to stay safe + // This should be safe enough for stability since we only reduce throttle + const float maxVelDifference = US2S(deltaMicros) * (GRAVITY_CMSS * 0.8f); + posControl.desiredState.vel.z = constrainf(targetVel, posControl.desiredState.vel.z - maxVelDifference, posControl.desiredState.vel.z + maxVelDifference); + } + else if (velTargetChange >= smallVelChange) { + // Large and positive - acceleration is _up_. We are limited by thrust/weight ratio which is usually about 2:1 (hover around 50% throttle). + // T/W ratio = 2 means we are able to reach 1G acceleration in "UP" direction. Hard limit to 0.5G to be on a safe side and avoid abrupt throttle changes + const float maxVelDifference = US2S(deltaMicros) * (GRAVITY_CMSS * 0.5f); posControl.desiredState.vel.z = constrainf(targetVel, posControl.desiredState.vel.z - maxVelDifference, posControl.desiredState.vel.z + maxVelDifference); } else { + // Small - desired acceleration is less than 0.1G. We should be safe setting velocity target directly - any platform should be able to satisfy this posControl.desiredState.vel.z = targetVel; } @@ -130,11 +140,11 @@ bool adjustMulticopterAltitudeFromRCInput(void) // Make sure we can satisfy max_manual_climb_rate in both up and down directions if (rcThrottleAdjustment > 0) { // Scaling from altHoldThrottleRCZero to maxthrottle - rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (motorConfig()->maxthrottle - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband); + rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(motorConfig()->maxthrottle - altHoldThrottleRCZero - rcControlsConfig()->alt_hold_deadband); } else { // Scaling from minthrottle to altHoldThrottleRCZero - rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (altHoldThrottleRCZero - motorConfig()->minthrottle - rcControlsConfig()->alt_hold_deadband); + rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - motorConfig()->minthrottle - rcControlsConfig()->alt_hold_deadband); } updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL); @@ -174,8 +184,9 @@ void setupMulticopterAltitudeController(void) motorConfig()->minthrottle + rcControlsConfig()->alt_hold_deadband + 10, motorConfig()->maxthrottle - rcControlsConfig()->alt_hold_deadband - 10); - /* Force AH controller to initialize althold integral for pending takeoff on reset */ - if (throttleStatus == THROTTLE_LOW) { + // Force AH controller to initialize althold integral for pending takeoff on reset + // Signal for that is low throttle _and_ low actual altitude + if (throttleStatus == THROTTLE_LOW && fabsf(navGetCurrentActualPositionAndVelocity()->pos.z) <= 50.0f) { prepareForTakeoffOnReset = true; } } @@ -263,7 +274,6 @@ bool adjustMulticopterHeadingFromRCInput(void) /*----------------------------------------------------------- * XY-position controller for multicopter aircraft *-----------------------------------------------------------*/ -static pt1Filter_t mcPosControllerAccFilterStateX, mcPosControllerAccFilterStateY; static float lastAccelTargetX = 0.0f, lastAccelTargetY = 0.0f; void resetMulticopterBrakingMode(void) @@ -363,8 +373,6 @@ void resetMulticopterPositionController(void) for (int axis = 0; axis < 2; axis++) { navPidReset(&posControl.pids.vel[axis]); posControl.rcAdjustment[axis] = 0; - pt1FilterReset(&mcPosControllerAccFilterStateX, 0.0f); - pt1FilterReset(&mcPosControllerAccFilterStateY, 0.0f); lastAccelTargetX = 0.0f; lastAccelTargetY = 0.0f; } @@ -379,8 +387,8 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR if (rcPitchAdjustment || rcRollAdjustment) { // If mode is GPS_CRUISE, move target position, otherwise POS controller will passthru the RC input to ANGLE PID if (navConfig()->general.flags.user_control_mode == NAV_GPS_CRUISE) { - const float rcVelX = rcPitchAdjustment * navConfig()->general.max_manual_speed / (500 - rcControlsConfig()->pos_hold_deadband); - const float rcVelY = rcRollAdjustment * navConfig()->general.max_manual_speed / (500 - rcControlsConfig()->pos_hold_deadband); + const float rcVelX = rcPitchAdjustment * navConfig()->general.max_manual_speed / (float)(500 - rcControlsConfig()->pos_hold_deadband); + const float rcVelY = rcRollAdjustment * navConfig()->general.max_manual_speed / (float)(500 - rcControlsConfig()->pos_hold_deadband); // Rotate these velocities from body frame to to earth frame const float neuVelX = rcVelX * posControl.actualState.cosYaw - rcVelY * posControl.actualState.sinYaw; @@ -491,7 +499,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA if (STATE(NAV_CRUISE_BRAKING)) { maxAccelChange = maxAccelChange * 2; } -#endif +#endif const float accelLimitXMin = constrainf(lastAccelTargetX - maxAccelChange, -accelLimitX, +accelLimitX); const float accelLimitXMax = constrainf(lastAccelTargetX + maxAccelChange, -accelLimitX, +accelLimitX); @@ -507,23 +515,22 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, navGetCurrentActualPositionAndVelocity()->vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0); int32_t maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle); - uint8_t accCutoffFrequency = NAV_ACCEL_CUTOFF_FREQUENCY_HZ; #ifdef USE_MR_BRAKING_MODE //Boost required accelerations if (STATE(NAV_CRUISE_BRAKING_BOOST) && navConfig()->mc.braking_boost_factor > 0) { const float rawBoostFactor = (float)navConfig()->mc.braking_boost_factor / 100.0f; - + //Scale boost factor according to speed const float boostFactor = constrainf( scaleRangef( - posControl.actualState.velXY, - navConfig()->mc.braking_boost_speed_threshold, - navConfig()->general.max_manual_speed, + posControl.actualState.velXY, + navConfig()->mc.braking_boost_speed_threshold, + navConfig()->general.max_manual_speed, 0.0f, rawBoostFactor ), - 0.0f, + 0.0f, rawBoostFactor ); @@ -532,7 +539,6 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA newAccelY = newAccelY * (1.0f + boostFactor); maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.braking_bank_angle); - accCutoffFrequency = NAV_ACCEL_CUTOFF_FREQUENCY_HZ * 2; } #endif @@ -540,13 +546,9 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA lastAccelTargetX = newAccelX; lastAccelTargetY = newAccelY; - // Apply LPF to jerk limited acceleration target - const float accelN = pt1FilterApply4(&mcPosControllerAccFilterStateX, newAccelX, accCutoffFrequency, US2S(deltaMicros)); - const float accelE = pt1FilterApply4(&mcPosControllerAccFilterStateY, newAccelY, accCutoffFrequency, US2S(deltaMicros)); - // Rotate acceleration target into forward-right frame (aircraft) - const float accelForward = accelN * posControl.actualState.cosYaw + accelE * posControl.actualState.sinYaw; - const float accelRight = -accelN * posControl.actualState.sinYaw + accelE * posControl.actualState.cosYaw; + const float accelForward = newAccelX * posControl.actualState.cosYaw + newAccelY * posControl.actualState.sinYaw; + const float accelRight = -newAccelX * posControl.actualState.sinYaw + newAccelY * posControl.actualState.cosYaw; // Calculate banking angles const float desiredPitch = atan2_approx(accelForward, GRAVITY_CMSS); @@ -727,11 +729,11 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) } else { /* Sensors has gone haywire, attempt to land regardless */ - if (failsafeConfig()) { - rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; + if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) { + rcCommand[THROTTLE] = motorConfig()->minthrottle; } else { - rcCommand[THROTTLE] = motorConfig()->minthrottle; + rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle; } } } diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index cf9445786da..d190c0c041e 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -28,6 +28,7 @@ #include "build/debug.h" #include "common/axis.h" +#include "common/log.h" #include "common/maths.h" #include "config/parameter_group.h" @@ -53,19 +54,21 @@ navigationPosEstimator_t posEstimator; -PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 4); PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, // Inertial position estimator parameters .automatic_mag_declination = 1, .reset_altitude_type = NAV_RESET_ON_FIRST_ARM, - .reset_home_type = NAV_RESET_ON_EACH_ARM, + .reset_home_type = NAV_RESET_ON_FIRST_ARM, .gravity_calibration_tolerance = 5, // 5 cm/s/s calibration error accepted (0.5% of gravity) .use_gps_velned = 1, // "Disabled" is mandatory with gps_dyn_model = Pedestrian .allow_dead_reckoning = 0, .max_surface_altitude = 200, + .w_xyz_acc_p = 1.0f, + .w_z_baro_p = 0.35f, .w_z_surface_p = 3.500f, @@ -206,15 +209,14 @@ void onNewGPSData(void) isFirstGPSUpdate = true; } -#if defined(NAV_AUTO_MAG_DECLINATION) /* Automatic magnetic declination calculation - do this once */ - static bool magDeclinationSet = false; - if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) { - imuSetMagneticDeclination(geoCalculateMagDeclination(&newLLH)); - magDeclinationSet = true; + if(STATE(GPS_FIX_HOME)){ + static bool magDeclinationSet = false; + if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) { + imuSetMagneticDeclination(geoCalculateMagDeclination(&newLLH)); + magDeclinationSet = true; + } } -#endif - /* Process position update if GPS origin is already set, or precision is good enough */ // FIXME: Add HDOP check for acquisition of GPS origin /* Set GPS origin or reset the origin altitude - keep initial pre-arming altitude at zero */ @@ -352,8 +354,37 @@ static bool gravityCalibrationComplete(void) return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); } -static void updateIMUTopic(void) +static void updateIMUEstimationWeight(const float dt) +{ + bool isAccClipped = accIsClipped(); + + // If accelerometer measurement is clipped - drop the acc weight to zero + // and gradually restore weight back to 1.0 over time + if (isAccClipped) { + posEstimator.imu.accWeightFactor = 0.0f; + } + else { + const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT); + posEstimator.imu.accWeightFactor = posEstimator.imu.accWeightFactor * (1.0f - relAlpha) + 1.0f * relAlpha; + } + + // DEBUG_VIBE[0-3] are used in IMU + DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000); +} + +float navGetAccelerometerWeight(void) +{ + const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p; + DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000); + + return accWeightScaled; +} + +static void updateIMUTopic(timeUs_t currentTimeUs) { + const float dt = US2S(currentTimeUs - posEstimator.imu.lastUpdateTime); + posEstimator.imu.lastUpdateTime = currentTimeUs; + if (!isImuReady()) { posEstimator.imu.accelNEU.x = 0; posEstimator.imu.accelNEU.y = 0; @@ -362,6 +393,9 @@ static void updateIMUTopic(void) restartGravityCalibration(); } else { + /* Update acceleration weight based on vibration levels and clipping */ + updateIMUEstimationWeight(dt); + fpVector3_t accelBF; /* Read acceleration data in body frame */ @@ -388,7 +422,7 @@ static void updateIMUTopic(void) if (gravityCalibrationComplete()) { zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS); - DEBUG_TRACE_SYNC("Gravity calibration complete (%d)", lrintf(posEstimator.imu.calibratedGravityCMSS)); + LOG_D(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS)); } } @@ -474,11 +508,13 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) static void estimationPredict(estimationContext_t * ctx) { + const float accWeight = navGetAccelerometerWeight(); + /* Prediction step: Z-axis */ if ((ctx->newFlags & EST_Z_VALID)) { posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt; - posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f; - posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt; + posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight; + posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight); } /* Prediction step: XY-axis */ @@ -489,10 +525,10 @@ static void estimationPredict(estimationContext_t * ctx) // If heading is valid, accelNEU is valid as well. Account for acceleration if (navIsHeadingUsable() && navIsAccelerationUsable()) { - posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f; - posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f; - posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt; - posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt; + posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f * accWeight; + posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f * accWeight; + posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt * sq(accWeight); + posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt * sq(accWeight); } } } @@ -758,6 +794,7 @@ void initializePositionEstimator(void) posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f; posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f; + posEstimator.imu.lastUpdateTime = 0; posEstimator.gps.lastUpdateTime = 0; posEstimator.baro.lastUpdateTime = 0; posEstimator.surface.lastUpdateTime = 0; @@ -768,6 +805,8 @@ void initializePositionEstimator(void) posEstimator.est.flowCoordinates[X] = 0; posEstimator.est.flowCoordinates[Y] = 0; + posEstimator.imu.accWeightFactor = 0; + restartGravityCalibration(); for (axis = 0; axis < 3; axis++) { @@ -784,7 +823,7 @@ void initializePositionEstimator(void) * Update estimator * Update rate: loop rate (>100Hz) */ -void updatePositionEstimator(void) +void FAST_CODE NOINLINE updatePositionEstimator(void) { static bool isInitialized = false; @@ -796,7 +835,7 @@ void updatePositionEstimator(void) const timeUs_t currentTimeUs = micros(); /* Read updates from IMU, preprocess */ - updateIMUTopic(); + updateIMUTopic(currentTimeUs); /* Update estimate */ updateEstimatedTopic(currentTimeUs); diff --git a/src/main/navigation/navigation_pos_estimator_agl.c b/src/main/navigation/navigation_pos_estimator_agl.c index 21c09909dec..0a975072043 100644 --- a/src/main/navigation/navigation_pos_estimator_agl.c +++ b/src/main/navigation/navigation_pos_estimator_agl.c @@ -149,9 +149,10 @@ void estimationCalculateAGL(estimationContext_t * ctx) } // Update estimate + const float accWeight = navGetAccelerometerWeight(); posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt; - posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f; - posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt; + posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight; + posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight); // Apply correction if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) { diff --git a/src/main/navigation/navigation_pos_estimator_flow.c b/src/main/navigation/navigation_pos_estimator_flow.c index c0763d42eb7..60ecaad8658 100644 --- a/src/main/navigation/navigation_pos_estimator_flow.c +++ b/src/main/navigation/navigation_pos_estimator_flow.c @@ -45,7 +45,7 @@ extern navigationPosEstimator_t posEstimator; -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPFLOW /** * Read optical flow topic * Function is called by OPFLOW task as soon as new update is available @@ -63,7 +63,7 @@ void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs) bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) { -#if defined(USE_RANGEFINDER) && defined(USE_OPTICAL_FLOW) +#if defined(USE_RANGEFINDER) && defined(USE_OPFLOW) if (!((ctx->newFlags & EST_FLOW_VALID) && (ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_Z_VALID))) { return false; } @@ -112,8 +112,8 @@ bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, sqrtf(sq(flowResidualX) + sq(flowResidualY)), positionEstimationConfig()->w_xy_flow_p); } - DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[Y])); - DEBUG_SET(DEBUG_FLOW, 1, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X])); + DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X])); + DEBUG_SET(DEBUG_FLOW, 1, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[Y])); DEBUG_SET(DEBUG_FLOW, 2, posEstimator.est.flowCoordinates[X]); DEBUG_SET(DEBUG_FLOW, 3, posEstimator.est.flowCoordinates[Y]); diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 46c15509789..7306936ff6c 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -51,6 +51,8 @@ #define INAV_BARO_AVERAGE_HZ 1.0f #define INAV_SURFACE_AVERAGE_HZ 1.0f +#define INAV_ACC_CLIPPING_RC_CONSTANT (0.010f) // Reduce acc weight for ~10ms after clipping + #define RANGEFINDER_RELIABILITY_RC_CONSTANT (0.47802f) #define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f) #define RANGEFINDER_RELIABILITY_LOW_THRESHOLD (0.33f) @@ -126,9 +128,11 @@ typedef struct { } navPositionEstimatorESTIMATE_t; typedef struct { + timeUs_t lastUpdateTime; fpVector3_t accelNEU; fpVector3_t accelBias; float calibratedGravityCMSS; + float accWeightFactor; zeroCalibrationScalar_t gravityCalibration; } navPosisitonEstimatorIMU_t; @@ -182,5 +186,5 @@ typedef struct { extern float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w); extern void estimationCalculateAGL(estimationContext_t * ctx); extern bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx); - +extern float navGetAccelerometerWeight(void); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 29dc704ca3b..14eaa21a3ab 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -29,11 +29,10 @@ #define MIN_POSITION_UPDATE_RATE_HZ 5 // Minimum position update rate at which XYZ controllers would be applied #define NAV_THROTTLE_CUTOFF_FREQENCY_HZ 4 // low-pass filter on throttle output -#define NAV_ACCEL_CUTOFF_FREQUENCY_HZ 2 // low-pass filter on XY-acceleration target #define NAV_FW_CONTROL_MONITORING_RATE 2 #define NAV_FW_PITCH_CUTOFF_FREQENCY_HZ 2 // low-pass filter on Z (pitch angle) for fixed wing #define NAV_FW_ROLL_CUTOFF_FREQUENCY_HZ 10 // low-pass filter on roll correction for fixed wing -#define NAV_DTERM_CUT_HZ 10 +#define NAV_DTERM_CUT_HZ 10.0f #define NAV_ACCELERATION_XY_MAX 980.0f // cm/s/s // approx 45 deg lean angle #define INAV_SURFACE_MAX_DISTANCE 40 @@ -298,6 +297,23 @@ typedef struct { timeMs_t lastYawAdjustmentTime; } navCruise_t; +typedef struct { + navigationHomeFlags_t homeFlags; + navWaypointPosition_t homePosition; // Original home position and base altitude + float rthInitialAltitude; // Altitude at start of RTH + float rthFinalAltitude; // Altitude at end of RTH approach + float rthInitialDistance; // Distance when starting flight home + fpVector3_t homeTmpWaypoint; // Temporary storage for home target +} rthState_t; + +typedef enum { + RTH_HOME_ENROUTE_INITIAL, // Initial position for RTH approach + RTH_HOME_ENROUTE_PROPORTIONAL, // Prorpotional position for RTH approach + RTH_HOME_ENROUTE_FINAL, // Final position for RTH approach + RTH_HOME_FINAL_HOVER, // Final hover altitude (if rth_home_altitude is set) + RTH_HOME_FINAL_LAND, // Home position and altitude +} rthTargetMode_e; + typedef struct { /* Flags and navigation system state */ navigationFSMState_t navState; @@ -322,10 +338,9 @@ typedef struct { /* Home parameters (NEU coordinated), geodetic position of home (LLH) is stores in GPS_home variable */ rthSanityChecker_t rthSanityChecker; - navWaypointPosition_t homePosition; // Special waypoint, stores original yaw (heading when launched) - navWaypointPosition_t homeWaypointAbove; // NEU-coordinates and initial bearing + desired RTH altitude - navigationHomeFlags_t homeFlags; + rthState_t rthState; + /* Home parameters */ uint32_t homeDistance; // cm int32_t homeDirection; // deg*100 @@ -345,6 +360,10 @@ typedef struct { float totalTripDistance; } navigationPosControl_t; +#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) +PG_DECLARE_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList); +#endif + extern navigationPosControl_t posControl; /* Internally used functions */ @@ -353,8 +372,7 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void); float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags); float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler); void navPidReset(pidController_t *pid); -void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD); -void navPInit(pController_t *p, float _kP); +void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz); bool isThrustFacingDownwards(void); uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos); diff --git a/src/main/platform.h b/src/main/platform.h index 67365d6ba69..657b0674809 100644 --- a/src/main/platform.h +++ b/src/main/platform.h @@ -62,3 +62,10 @@ #include "target/common.h" #include "target.h" #include "target/common_post.h" + +// Remove the unaligned packed structure member pointer access warning +// The compiler guarantees that unaligned access is safe for packed structures. + +#if (__GNUC__ >= 9) +#pragma GCC diagnostic ignored "-Waddress-of-packed-member" +#endif diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index a0e0cf6389a..f3bc7933494 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -43,6 +43,7 @@ enum { enum { CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, + CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. @@ -83,6 +84,7 @@ typedef enum { typedef enum { CRSF_FRAMETYPE_GPS = 0x02, + CRSF_FRAMETYPE_VARIO_SENSOR = 0x07, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index 59418c1e0f8..f8d430a07f8 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -48,7 +48,8 @@ #define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500 #define FPORT_MAX_TELEMETRY_AGE_MS 500 -#define FPORT_TELEMETRY_MAX_CONSECUTIVE_SENSORS 2 // Needed to avoid lost sensors on FPort, see #3198 +#define FPORT_TELEMETRY_MAX_CONSECUTIVE_TELEMETRY_FRAMES 2 + #define FPORT_FRAME_MARKER 0x7E @@ -65,7 +66,7 @@ enum { DEBUG_FPORT_FRAME_INTERVAL = 0, DEBUG_FPORT_FRAME_ERRORS, DEBUG_FPORT_FRAME_LAST_ERROR, - DEBUG_FPORT_TELEMETRY_DELAY, + DEBUG_FPORT_TELEMETRY_INTERVAL, }; enum { @@ -150,7 +151,9 @@ static serialPort_t *fportPort; static void reportFrameError(uint8_t errorReason) { static volatile uint16_t frameErrors = 0; - DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_ERRORS, ++frameErrors); + frameErrors++; + + DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_ERRORS, frameErrors); DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_LAST_ERROR, errorReason); } @@ -251,6 +254,8 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { #if defined(USE_TELEMETRY_SMARTPORT) static smartPortPayload_t payloadBuffer; + static bool rxDrivenFrameRate = false; + static uint8_t consecutiveTelemetryFrameCount = 0; #endif static bool hasTelemetryRequest = false; @@ -272,7 +277,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_CONTROL) { reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE); } else { - result |= sbusChannelsDecode(rxRuntimeConfig, &frame->data.controlData.channels); + result = sbusChannelsDecode(rxRuntimeConfig, &frame->data.controlData.channels); setRSSI(scaleRange(frame->data.controlData.rssi, 0, 100, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_PROTOCOL, false); @@ -290,10 +295,25 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) } switch(frame->data.telemetryData.frameId) { - case FPORT_FRAME_ID_NULL: - case FPORT_FRAME_ID_DATA: // never used + case FPORT_FRAME_ID_DATA: + if (!rxDrivenFrameRate) { + rxDrivenFrameRate = true; + } + hasTelemetryRequest = true; + break; + case FPORT_FRAME_ID_NULL: + if (!rxDrivenFrameRate) { + if (consecutiveTelemetryFrameCount >= FPORT_TELEMETRY_MAX_CONSECUTIVE_TELEMETRY_FRAMES && !(mspPayload && smartPortPayloadContainsMSP(mspPayload))) { + consecutiveTelemetryFrameCount = 0; + } else { + hasTelemetryRequest = true; + + consecutiveTelemetryFrameCount++; + } + } + break; case FPORT_FRAME_ID_READ: case FPORT_FRAME_ID_WRITE: // never used @@ -324,7 +344,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) if ((mspPayload || hasTelemetryRequest) && cmpTimeUs(micros(), lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) { hasTelemetryRequest = false; - result = result | RX_FRAME_PROCESSING_REQUIRED; + result = (result & ~RX_FRAME_PENDING) | RX_FRAME_PROCESSING_REQUIRED; } if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) { @@ -340,31 +360,24 @@ static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) UNUSED(rxRuntimeConfig); #if defined(USE_TELEMETRY_SMARTPORT) + static timeUs_t lastTelemetryFrameSentUs; + timeUs_t currentTimeUs = micros(); if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) { clearToSend = false; } if (clearToSend) { - DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs); - - uint8_t *consecutiveSensorCount = rxRuntimeConfig->frameData; - if (*consecutiveSensorCount >= FPORT_TELEMETRY_MAX_CONSECUTIVE_SENSORS && !smartPortPayloadContainsMSP(mspPayload)) { - // Stop one cycle to avoid saturating the buffer in the RX, since the - // downstream bandwidth doesn't allow sensor sensors on every cycle. - // We allow MSP frames to run over the resting period, expecting that - // the caller won't flood us with requests. - *consecutiveSensorCount = 0; - } else { - (*consecutiveSensorCount)++; - processSmartPortTelemetry(mspPayload, &clearToSend, NULL); - } + processSmartPortTelemetry(mspPayload, &clearToSend, NULL); if (clearToSend) { smartPortWriteFrameFport(&emptySmartPortFrame); clearToSend = false; } + + DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_INTERVAL, currentTimeUs - lastTelemetryFrameSentUs); + lastTelemetryFrameSentUs = currentTimeUs; } mspPayload = NULL; @@ -376,9 +389,7 @@ static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { static uint16_t sbusChannelData[SBUS_MAX_CHANNEL]; - static uint8_t consecutiveSensorCount = 0; rxRuntimeConfig->channelData = sbusChannelData; - rxRuntimeConfig->frameData = &consecutiveSensorCount; sbusChannelsInit(rxRuntimeConfig); rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL; diff --git a/src/main/rx/msp_override.c b/src/main/rx/msp_override.c new file mode 100755 index 00000000000..cf526a7d465 --- /dev/null +++ b/src/main/rx/msp_override.c @@ -0,0 +1,240 @@ +/* + * 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 . + */ + +#include +#include + +#include "platform.h" + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/maths.h" +#include "common/utils.h" + +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/time.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/rc_modes.h" + +#include "flight/failsafe.h" + +#include "rx/rx.h" +#include "rx/msp.h" +#include "rx/msp_override.h" + + +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + +static bool rxDataProcessingRequired = false; + +static bool rxSignalReceived = false; +static bool rxFlightChannelsValid = false; +static bool rxFailsafe = true; + +static timeMs_t rxDataFailurePeriod; +static timeMs_t rxDataRecoveryPeriod; +static timeMs_t validRxDataReceivedAt = 0; +static timeMs_t validRxDataFailedAt = 0; + +static timeUs_t rxNextUpdateAtUs = 0; +static timeUs_t needRxSignalBefore = 0; + +static uint16_t mspOverrideCtrlChannels = 0; // bitmask representing which channels are used to control MSP override +static rcChannel_t mspRcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + +static rxRuntimeConfig_t rxRuntimeConfigMSP; + + +void mspOverrideInit(void) +{ + timeMs_t nowMs = millis(); + + for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { + mspRcChannels[i].raw = PWM_RANGE_MIDDLE; + mspRcChannels[i].data = PWM_RANGE_MIDDLE; + mspRcChannels[i].expiresAt = nowMs + MAX_INVALID_RX_PULSE_TIME; + } + + mspRcChannels[THROTTLE].raw = (feature(FEATURE_3D)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec; + mspRcChannels[THROTTLE].data = mspRcChannels[THROTTLE].raw; + + // Initialize ARM switch to OFF position when arming via switch is defined + for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + if (modeActivationConditions(i)->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationConditions(i)->range)) { + // ARM switch is defined, determine an OFF value + uint16_t value; + if (modeActivationConditions(i)->range.startStep > 0) { + value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.startStep - 1)); + } else { + value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.endStep + 1)); + } + // Initialize ARM AUX channel to OFF value + rcChannel_t *armChannel = &mspRcChannels[modeActivationConditions(i)->auxChannelIndex + NON_AUX_CHANNEL_COUNT]; + armChannel->raw = value; + armChannel->data = value; + } + + // Find which channels are used to control MSP override + if (modeActivationConditions(i)->modeId == BOXMSPRCOVERRIDE && IS_RANGE_USABLE(&modeActivationConditions(i)->range)) { + mspOverrideCtrlChannels |= 1 << (modeActivationConditions(i)->auxChannelIndex + NON_AUX_CHANNEL_COUNT); + } + } + + rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; + rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND; + + rxMspInit(rxConfig(), &rxRuntimeConfigMSP); +} + +bool mspOverrideIsReceivingSignal(void) +{ + return rxSignalReceived; +} + +bool mspOverrideAreFlightChannelsValid(void) +{ + return rxFlightChannelsValid; +} + +bool mspOverrideIsInFailsafe(void) +{ + return rxFailsafe; +} + +bool mspOverrideUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) +{ + UNUSED(currentDeltaTime); + + if (rxSignalReceived) { + if (currentTimeUs >= needRxSignalBefore) { + rxSignalReceived = false; + } + } + + const uint8_t frameStatus = rxRuntimeConfigMSP.rcFrameStatusFn(&rxRuntimeConfigMSP); + if (frameStatus & RX_FRAME_COMPLETE) { + rxDataProcessingRequired = true; + rxSignalReceived = true; + needRxSignalBefore = currentTimeUs + rxRuntimeConfigMSP.rxSignalTimeout; + } + + if (cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) { + rxDataProcessingRequired = true; + } + + return rxDataProcessingRequired; // data driven or 50Hz +} + +bool mspOverrideCalculateChannels(timeUs_t currentTimeUs) +{ + int16_t rcStaging[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + const timeMs_t currentTimeMs = millis(); + + if (!rxDataProcessingRequired) { + return false; + } + + rxDataProcessingRequired = false; + rxNextUpdateAtUs = currentTimeUs + DELAY_50_HZ; + + rxFlightChannelsValid = true; + + // Read and process channel data + for (int channel = 0; channel < rxRuntimeConfigMSP.channelCount; channel++) { + const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); + + // sample the channel + uint16_t sample = (*rxRuntimeConfigMSP.rcReadRawFn)(&rxRuntimeConfigMSP, rawChannel); + + // apply the rx calibration to flight channel + if (channel < NON_AUX_CHANNEL_COUNT && sample != PPM_RCVR_TIMEOUT) { + sample = scaleRange(sample, rxChannelRangeConfigs(channel)->min, rxChannelRangeConfigs(channel)->max, PWM_RANGE_MIN, PWM_RANGE_MAX); + sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX); + } + + // Store as rxRaw + mspRcChannels[channel].raw = sample; + + // Apply invalid pulse value logic + if (!isRxPulseValid(sample)) { + sample = mspRcChannels[channel].data; // hold channel, replace with old value + if ((currentTimeMs > mspRcChannels[channel].expiresAt) && (channel < NON_AUX_CHANNEL_COUNT)) { + rxFlightChannelsValid = false; + } + } else { + mspRcChannels[channel].expiresAt = currentTimeMs + MAX_INVALID_RX_PULSE_TIME; + } + + // Save channel value + rcStaging[channel] = sample; + } + + // Update channel input value if receiver is not in failsafe mode + // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good input values are retained + if (rxFlightChannelsValid && rxSignalReceived) { + for (int channel = 0; channel < rxRuntimeConfigMSP.channelCount; channel++) { + mspRcChannels[channel].data = rcStaging[channel]; + } + } + + // Update failsafe + if (rxFlightChannelsValid && rxSignalReceived) { + validRxDataReceivedAt = millis(); + if ((validRxDataReceivedAt - validRxDataFailedAt) > rxDataRecoveryPeriod) { + rxFailsafe = false; + } + } else { + validRxDataFailedAt = millis(); + if ((validRxDataFailedAt - validRxDataReceivedAt) > rxDataFailurePeriod) { + rxFailsafe = true; + } + } + + return true; +} + +void mspOverrideChannels(rcChannel_t *rcChannels) +{ + for (uint16_t channel = 0, channelMask = 1; channel < rxRuntimeConfigMSP.channelCount; ++channel, channelMask <<= 1) { + if (rxConfig()->mspOverrideChannels & ~mspOverrideCtrlChannels & channelMask) { + rcChannels[channel].raw = rcChannels[channel].data = mspRcChannels[channel].data; + } + } +} + +uint16_t mspOverrideGetRefreshRate(void) +{ + return rxRuntimeConfigMSP.rxRefreshRate; +} + +int16_t mspOverrideGetChannelValue(unsigned channelNumber) +{ + return mspRcChannels[channelNumber].data; +} + +int16_t mspOverrideGetRawChannelValue(unsigned channelNumber) +{ + return mspRcChannels[channelNumber].raw; +} + +#endif // defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) diff --git a/src/main/rx/msp_override.h b/src/main/rx/msp_override.h new file mode 100644 index 00000000000..7929ed4d9a2 --- /dev/null +++ b/src/main/rx/msp_override.h @@ -0,0 +1,29 @@ +/* + * 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 . + */ + +#pragma once + +void mspOverrideInit(void); +bool mspOverrideUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime); +bool mspOverrideCalculateChannels(timeUs_t currentTimeUs); +void mspOverrideChannels(rcChannel_t *rcChannels); +bool mspOverrideIsReceivingSignal(void); +bool mspOverrideIsInFailsafe(void); +bool mspOverrideAreFlightChannelsValid(void); +uint16_t mspOverrideGetRefreshRate(void); +int16_t mspOverrideGetChannelValue(unsigned channelNumber); +int16_t mspOverrideGetRawChannelValue(unsigned channelNumber); diff --git a/src/main/rx/pwm.c b/src/main/rx/pwm.c index 252b55d7bb1..fa90e3b476c 100644 --- a/src/main/rx/pwm.c +++ b/src/main/rx/pwm.c @@ -23,13 +23,14 @@ #include "platform.h" -#if defined(USE_RX_PWM) || defined(USE_RX_PPM) +#if defined(USE_RX_PPM) #include "build/debug.h" #include "common/utils.h" #include "config/feature.h" +#include "drivers/timer.h" #include "drivers/rx_pwm.h" #include "drivers/time.h" @@ -40,14 +41,7 @@ #define RC_PWM_50HZ_UPDATE (20 * 1000) // 50Hz update rate period -#if MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT > MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT - #define PWM_RX_CHANNEL_COUNT MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT -#else - #define PWM_RX_CHANNEL_COUNT MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT -#endif - -static uint16_t channelData[PWM_RX_CHANNEL_COUNT]; -static timeUs_t lastReceivedFrameUs; +static uint16_t channelData[MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT]; static uint16_t readRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { @@ -55,30 +49,6 @@ static uint16_t readRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan return channelData[channel]; } -static uint8_t pwmFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) -{ - UNUSED(rxRuntimeConfig); - - timeUs_t currentTimeUs = micros(); - - // PWM doesn't indicate individual updates, if low level code indicates that we have valid signal - // we mimic the update at 50Hz rate - - if (isPWMDataBeingReceived()) { - if ((currentTimeUs - lastReceivedFrameUs) >= RC_PWM_50HZ_UPDATE) { - lastReceivedFrameUs = currentTimeUs; - - for (int channel = 0; channel < MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT; channel++) { - channelData[channel] = pwmRead(channel); - } - - return RX_FRAME_COMPLETE; - } - } - - return RX_FRAME_PENDING; -} - static uint8_t ppmFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxRuntimeConfig); @@ -96,21 +66,25 @@ static uint8_t ppmFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) return RX_FRAME_PENDING; } -void rxPwmInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +bool rxPpmInit(rxRuntimeConfig_t *rxRuntimeConfig) { + const timerHardware_t * timHw = timerGetByUsageFlag(TIM_USE_PPM); + + if (timHw == NULL) { + return false; + } + + if (!ppmInConfig(timHw)) { + return false; + } + rxRuntimeConfig->rxRefreshRate = RC_PWM_50HZ_UPDATE; rxRuntimeConfig->requireFiltering = true; + rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; + rxRuntimeConfig->rcReadRawFn = readRawRC; + rxRuntimeConfig->rcFrameStatusFn = ppmFrameStatus; - // configure PWM/CPPM read function and max number of channels. serial rx below will override both of these, if enabled - if (rxConfig->receiverType == RX_TYPE_PWM) { - rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT; - rxRuntimeConfig->rcReadRawFn = readRawRC; - rxRuntimeConfig->rcFrameStatusFn = pwmFrameStatus; - } else if (rxConfig->receiverType == RX_TYPE_PPM) { - rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; - rxRuntimeConfig->rcReadRawFn = readRawRC; - rxRuntimeConfig->rcFrameStatusFn = ppmFrameStatus; - } + return true; } #endif diff --git a/src/main/rx/pwm.h b/src/main/rx/pwm.h index 100c2b7014a..6c2ef6ca9c0 100644 --- a/src/main/rx/pwm.h +++ b/src/main/rx/pwm.h @@ -17,4 +17,4 @@ #pragma once -void rxPwmInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +bool rxPpmInit(rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index cc3f1fa3874..8879d111e39 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -49,20 +49,21 @@ #include "io/serial.h" #include "rx/rx.h" +#include "rx/crsf.h" +#include "rx/eleres.h" +#include "rx/ibus.h" +#include "rx/jetiexbus.h" #include "rx/fport.h" +#include "rx/msp.h" +#include "rx/msp_override.h" #include "rx/pwm.h" +#include "rx/rx_spi.h" #include "rx/sbus.h" #include "rx/spektrum.h" #include "rx/sumd.h" #include "rx/sumh.h" -#include "rx/msp.h" -#include "rx/xbus.h" -#include "rx/ibus.h" -#include "rx/jetiexbus.h" -#include "rx/rx_spi.h" -#include "rx/crsf.h" -#include "rx/eleres.h" #include "rx/uib_rx.h" +#include "rx/xbus.h" //#define DEBUG_RX_SIGNAL_LOSS @@ -79,6 +80,10 @@ static rssiSource_e rssiSource; static bool rxDataProcessingRequired = false; static bool auxiliaryProcessingRequired = false; +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) +static bool mspOverrideDataProcessingRequired = false; +#endif + static bool rxSignalReceived = false; static bool rxFlightChannelsValid = false; static bool rxIsInFailsafeMode = true; @@ -88,11 +93,7 @@ static timeUs_t needRxSignalBefore = 0; static timeUs_t suspendRxSignalUntil = 0; static uint8_t skipRxSamples = 0; -int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] -int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] -timeMs_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; - -#define MAX_INVALID_PULS_TIME 300 +static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; #define SKIP_RC_ON_SUSPEND_PERIOD 1500000 // 1.5 second period in usec (call frequency independent) #define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent) @@ -100,7 +101,7 @@ timeMs_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; rxRuntimeConfig_t rxRuntimeConfig; static uint8_t rcSampleIndex = 0; -PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 8); #ifndef RX_SPI_DEFAULT_PROTOCOL #define RX_SPI_DEFAULT_PROTOCOL 0 @@ -131,6 +132,9 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig, .rssiMax = RSSI_VISIBLE_VALUE_MAX, .sbusSyncInterval = SBUS_DEFAULT_INTERFRAME_DELAY_US, .rcFilterFrequency = 50, +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + .mspOverrideChannels = 15, +#endif ); void resetAllRxChannelRangeConfigurations(void) @@ -168,7 +172,7 @@ static uint8_t nullFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) return RX_FRAME_PENDING; } -static bool isPulseValid(uint16_t pulseDuration) +bool isRxPulseValid(uint16_t pulseDuration) { return pulseDuration >= rxConfig()->rx_min_usec && pulseDuration <= rxConfig()->rx_max_usec; @@ -242,12 +246,16 @@ void rxInit(void) rxRuntimeConfig.requireFiltering = false; rcSampleIndex = 0; + timeMs_t nowMs = millis(); + for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { - rcData[i] = PWM_RANGE_MIDDLE; - rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME; + rcChannels[i].raw = PWM_RANGE_MIDDLE; + rcChannels[i].data = PWM_RANGE_MIDDLE; + rcChannels[i].expiresAt = nowMs + MAX_INVALID_RX_PULSE_TIME; } - rcData[THROTTLE] = (feature(FEATURE_3D)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec; + rcChannels[THROTTLE].raw = (feature(FEATURE_3D)) ? PWM_RANGE_MIDDLE : rxConfig()->rx_min_usec; + rcChannels[THROTTLE].data = rcChannels[THROTTLE].raw; // Initialize ARM switch to OFF position when arming via switch is defined for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { @@ -260,15 +268,20 @@ void rxInit(void) value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationConditions(i)->range.endStep + 1)); } // Initialize ARM AUX channel to OFF value - rcData[modeActivationConditions(i)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = value; + rcChannel_t *armChannel = &rcChannels[modeActivationConditions(i)->auxChannelIndex + NON_AUX_CHANNEL_COUNT]; + armChannel->raw = value; + armChannel->data = value; } } switch (rxConfig()->receiverType) { -#if defined(USE_RX_PWM) || defined(USE_RX_PPM) - case RX_TYPE_PWM: +#if defined(USE_RX_PPM) case RX_TYPE_PPM: - rxPwmInit(rxConfig(), &rxRuntimeConfig); + if (!rxPpmInit(&rxRuntimeConfig)) { + rxConfigMutable()->receiverType = RX_TYPE_NONE; + rxRuntimeConfig.rcReadRawFn = nullReadRawRC; + rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; + } break; #endif @@ -304,13 +317,22 @@ void rxInit(void) break; #endif - case RX_TYPE_NONE: default: - // Already configured for NONE + case RX_TYPE_NONE: + case RX_TYPE_PWM: + rxConfigMutable()->receiverType = RX_TYPE_NONE; + rxRuntimeConfig.rcReadRawFn = nullReadRawRC; + rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; break; } rxUpdateRSSISource(); + +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + if (rxConfig()->receiverType != RX_TYPE_MSP) { + mspOverrideInit(); + } +#endif } void rxUpdateRSSISource(void) @@ -329,11 +351,6 @@ void rxUpdateRSSISource(void) return; } - if (rxConfig()->rssi_channel > 0) { - rssiSource = RSSI_SOURCE_RX_CHANNEL; - return; - } - bool serialProtocolSupportsRSSI = false; switch (rxConfig()->receiverType) { #if defined(USE_SERIAL_RX) @@ -370,7 +387,7 @@ void rxUpdateRSSISource(void) } } -static uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap) +uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap) { if (channelToRemap < channelMapEntryCount) { return channelMap[channelToRemap]; @@ -428,7 +445,16 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) rxDataProcessingRequired = true; } - return rxDataProcessingRequired || auxiliaryProcessingRequired; // data driven or 50Hz + bool result = rxDataProcessingRequired || auxiliaryProcessingRequired; + +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + if (rxConfig()->receiverType != RX_TYPE_MSP) { + mspOverrideDataProcessingRequired = mspOverrideUpdateCheck(currentTimeUs, currentDeltaTime); + result = result || mspOverrideDataProcessingRequired; + } +#endif + + return result; } #define FILTERING_SAMPLE_COUNT 5 @@ -466,11 +492,15 @@ static uint16_t applyChannelFiltering(uint8_t chan, uint16_t sample) bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); - int16_t rcStaging[MAX_SUPPORTED_RC_CHANNEL_COUNT]; const timeMs_t currentTimeMs = millis(); +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + if ((rxConfig()->receiverType != RX_TYPE_MSP) && mspOverrideDataProcessingRequired) { + mspOverrideCalculateChannels(currentTimeUs); + } +#endif + if (auxiliaryProcessingRequired) { auxiliaryProcessingRequired = !rxRuntimeConfig.rcProcessFrameFn(&rxRuntimeConfig); } @@ -507,36 +537,42 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } // Store as rxRaw - rcRaw[channel] = sample; + rcChannels[channel].raw = sample; // Apply invalid pulse value logic - if (!isPulseValid(sample)) { - sample = rcData[channel]; // hold channel, replace with old value - if ((currentTimeMs > rcInvalidPulsPeriod[channel]) && (channel < NON_AUX_CHANNEL_COUNT)) { + if (!isRxPulseValid(sample)) { + sample = rcChannels[channel].data; // hold channel, replace with old value + if ((currentTimeMs > rcChannels[channel].expiresAt) && (channel < NON_AUX_CHANNEL_COUNT)) { rxFlightChannelsValid = false; } } else { - rcInvalidPulsPeriod[channel] = currentTimeMs + MAX_INVALID_PULS_TIME; + rcChannels[channel].expiresAt = currentTimeMs + MAX_INVALID_RX_PULSE_TIME; } // Save channel value rcStaging[channel] = sample; } - // Update rcData channel value if receiver is not in failsafe mode - // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good rcData values are retained + // Update channel input value if receiver is not in failsafe mode + // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good input values are retained if (rxFlightChannelsValid && rxSignalReceived) { if (rxRuntimeConfig.requireFiltering) { for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { - rcData[channel] = applyChannelFiltering(channel, rcStaging[channel]); + rcChannels[channel].data = applyChannelFiltering(channel, rcStaging[channel]); } } else { for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { - rcData[channel] = rcStaging[channel]; + rcChannels[channel].data = rcStaging[channel]; } } } +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && !mspOverrideIsInFailsafe()) { + mspOverrideChannels(rcChannels); + } +#endif + // Update failsafe if (rxFlightChannelsValid && rxSignalReceived) { failsafeOnValidDataReceived(); @@ -621,8 +657,9 @@ static void updateRSSIPWM(void) { int16_t pwmRssi = 0; // Read value of AUX channel as rssi - if (rxConfig()->rssi_channel > 0) { - pwmRssi = rcRaw[rxConfig()->rssi_channel - 1]; + unsigned rssiChannel = rxConfig()->rssi_channel; + if (rssiChannel > 0) { + pwmRssi = rcChannels[rssiChannel - 1].raw; // Range of rawPwmRssi is [1000;2000]. rssi should be in [0;1023]; uint16_t rawRSSI = (uint16_t)((constrain(pwmRssi - 1000, 0, 1000) / 1000.0f) * (RSSI_MAX_VALUE * 1.0f)); @@ -677,3 +714,13 @@ uint16_t rxGetRefreshRate(void) { return rxRuntimeConfig.rxRefreshRate; } + +int16_t rxGetChannelValue(unsigned channelNumber) +{ + return rcChannels[channelNumber].data; +} + +int16_t rxGetRawChannelValue(unsigned channelNumber) +{ + return rcChannels[channelNumber].raw; +} diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 7ebb8fa412c..b5d6a987bfa 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -81,25 +81,17 @@ typedef enum { } rxSerialReceiverType_e; #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16 -#define MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT 8 #define MAX_SUPPORTED_RC_CHANNEL_COUNT 18 #define NON_AUX_CHANNEL_COUNT 4 #define MAX_AUX_CHANNEL_COUNT (MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT) -#if MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT > MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT -#define MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT -#else -#define MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT -#endif - extern const char rcChannelLetters[]; -extern int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] -extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] - #define MAX_MAPPABLE_RX_INPUTS 4 +#define MAX_INVALID_RX_PULSE_TIME 300 + #define RSSI_VISIBLE_VALUE_MIN 0 #define RSSI_VISIBLE_VALUE_MAX 100 #define RSSI_VISIBLE_FACTOR (RSSI_MAX_VALUE/(float)RSSI_VISIBLE_VALUE_MAX) @@ -130,6 +122,7 @@ typedef struct rxConfig_s { uint16_t rx_min_usec; uint16_t rx_max_usec; uint8_t rcFilterFrequency; // RC filter cutoff frequency (smoothness vs response sharpness) + uint16_t mspOverrideChannels; // Channels to override with MSP RC when BOXMSPRCOVERRIDE is active } rxConfig_t; PG_DECLARE(rxConfig_t, rxConfig); @@ -153,6 +146,12 @@ typedef struct rxRuntimeConfig_s { void *frameData; } rxRuntimeConfig_t; +typedef struct rcChannel_s { + int16_t raw; // Value received via RX - [1000;2000] + int16_t data; // Value after processing - [1000;2000] + timeMs_t expiresAt; // Time when this value becomes too old and it's discarded +} rcChannel_t; + typedef enum { RSSI_SOURCE_NONE = 0, RSSI_SOURCE_ADC, @@ -169,7 +168,9 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime); bool rxIsReceivingSignal(void); bool rxAreFlightChannelsValid(void); bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs); +bool isRxPulseValid(uint16_t pulseDuration); +uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap); void parseRcChannels(const char *input); // filtered = true indicates that newRssi comes from a source which already does @@ -187,3 +188,13 @@ void suspendRxSignal(void); void resumeRxSignal(void); uint16_t rxGetRefreshRate(void); + +// Processed RC channel value. These values might include +// filtering and some extra processing like value holding +// during failsafe. Most callers should use this instead +// of rxGetRawChannelValue() +int16_t rxGetChannelValue(unsigned channelNumber); + +// Raw RC channel data as received by the RX. Should only +// be used by very low level subsystems, like blackbox. +int16_t rxGetRawChannelValue(unsigned channelNumber); diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index cdf9ffcd488..ee83dd5fa06 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -178,7 +178,7 @@ void setTaskEnabled(cfTaskId_e taskId, bool enabled) } } -timeDelta_t getTaskDeltaTime(cfTaskId_e taskId) +timeDelta_t FAST_CODE NOINLINE getTaskDeltaTime(cfTaskId_e taskId) { if (taskId == TASK_SELF) { return currentTask->taskLatestDeltaTime; @@ -212,7 +212,7 @@ void schedulerInit(void) queueAdd(&cfTasks[TASK_SYSTEM]); } -void scheduler(void) +void FAST_CODE NOINLINE scheduler(void) { // Cache currentTime const timeUs_t currentTimeUs = micros(); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 4ca38bc08dd..cf42a7ea5fe 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -98,7 +98,7 @@ typedef enum { #ifdef USE_CMS TASK_CMS, #endif -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPFLOW TASK_OPFLOW, #endif #ifdef USE_UAV_INTERCONNECT @@ -110,7 +110,9 @@ typedef enum { #ifdef USE_VTX_CONTROL TASK_VTXCTRL, #endif - +#ifdef USE_LOGIC_CONDITIONS + TASK_LOGIC_CONDITIONS, +#endif /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 1b776d362d9..f2c3f8b27b5 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -50,7 +50,6 @@ #include "drivers/accgyro/accgyro_bmi160.h" #include "drivers/accgyro/accgyro_icm20689.h" #include "drivers/accgyro/accgyro_fake.h" -#include "drivers/logging.h" #include "drivers/sensor.h" #include "fc/config.h" @@ -75,7 +74,9 @@ STATIC_FASTRAM zeroCalibrationVector_t zeroCalibration; STATIC_FASTRAM int32_t accADC[XYZ_AXIS_COUNT]; -STATIC_FASTRAM biquadFilter_t accFilter[XYZ_AXIS_COUNT]; +STATIC_FASTRAM filter_t accFilter[XYZ_AXIS_COUNT]; +STATIC_FASTRAM filterApplyFnPtr accSoftLpfFilterApplyFn; +STATIC_FASTRAM void *accSoftLpfFilter[XYZ_AXIS_COUNT]; STATIC_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT]; STATIC_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT]; @@ -85,7 +86,7 @@ STATIC_FASTRAM filterApplyFnPtr accNotchFilterApplyFn; STATIC_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT]; #endif -PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 2); +PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 3); void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) { @@ -94,7 +95,8 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) .acc_hardware = ACC_AUTODETECT, .acc_lpf_hz = 15, .acc_notch_hz = 0, - .acc_notch_cutoff = 1 + .acc_notch_cutoff = 1, + .acc_soft_lpf_type = FILTER_BIQUAD ); RESET_CONFIG_2(flightDynamicsTrims_t, &instance->accZero, .raw[X] = 0, @@ -304,8 +306,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) break; } - addBootlogEvent6(BOOT_EVENT_ACC_DETECTION, BOOT_EVENT_FLAGS_NONE, accHardware, 0, 0, 0); - if (accHardware == ACC_NONE) { return false; } @@ -336,6 +336,11 @@ bool accInit(uint32_t targetLooptime) acc.accClipCount = 0; accInitFilters(); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + acc.extremes[axis].min = 100; + acc.extremes[axis].max = -100; + } + if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) { acc.dev.accAlign = accelerometerConfig()->acc_align; } @@ -501,7 +506,7 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f } /* - * Calculate measured acceleration in body frame in g + * Calculate measured acceleration in body frame in m/s^2 */ void accGetMeasuredAcceleration(fpVector3_t *measuredAcc) { @@ -510,6 +515,19 @@ void accGetMeasuredAcceleration(fpVector3_t *measuredAcc) } } +/* + * Return g's + */ +const acc_extremes_t* accGetMeasuredExtremes(void) +{ + return (const acc_extremes_t *)&acc.extremes; +} + +float accGetMeasuredMaxG(void) +{ + return acc.maxG; +} + void accUpdate(void) { if (!acc.dev.readFn(&acc.dev)) { @@ -538,8 +556,12 @@ void accUpdate(void) // Before filtering check for clipping and vibration levels if (fabsf(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) { + acc.isClipped = true; acc.accClipCount++; } + else { + acc.isClipped = false; + } // Calculate vibration levels for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { @@ -552,10 +574,8 @@ void accUpdate(void) } // Filter acceleration - if (accelerometerConfig()->acc_lpf_hz) { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - acc.accADCf[axis] = biquadFilterApply(&accFilter[axis], acc.accADCf[axis]); - } + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]); } #ifdef USE_ACC_NOTCH @@ -568,6 +588,18 @@ void accUpdate(void) } +// Record extremes: min/max for each axis and acceleration vector modulus +void updateAccExtremes(void) +{ + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + if (acc.accADCf[axis] < acc.extremes[axis].min) acc.extremes[axis].min = acc.accADCf[axis]; + if (acc.accADCf[axis] > acc.extremes[axis].max) acc.extremes[axis].max = acc.accADCf[axis]; + } + + float gforce = sqrtf(sq(acc.accADCf[0]) + sq(acc.accADCf[1]) + sq(acc.accADCf[2])); + if (gforce > acc.maxG) acc.maxG = gforce; +} + void accGetVibrationLevels(fpVector3_t *accVibeLevels) { accVibeLevels->x = sqrtf(acc.accVibeSq[X]); @@ -585,6 +617,11 @@ uint32_t accGetClipCount(void) return acc.accClipCount; } +bool accIsClipped(void) +{ + return acc.isClipped; +} + void accSetCalibrationValues(void) { if ((accelerometerConfig()->accZero.raw[X] == 0) && (accelerometerConfig()->accZero.raw[Y] == 0) && (accelerometerConfig()->accZero.raw[Z] == 0) && @@ -597,11 +634,29 @@ void accSetCalibrationValues(void) } void accInitFilters(void) -{ +{ + accSoftLpfFilterApplyFn = nullFilterApply; + if (acc.accTargetLooptime && accelerometerConfig()->acc_lpf_hz) { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInitLPF(&accFilter[axis], accelerometerConfig()->acc_lpf_hz, acc.accTargetLooptime); + + switch (accelerometerConfig()->acc_soft_lpf_type) + { + case FILTER_PT1: + accSoftLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + accSoftLpfFilter[axis] = &accFilter[axis].pt1; + pt1FilterInit(accSoftLpfFilter[axis], accelerometerConfig()->acc_lpf_hz, acc.accTargetLooptime * 1e-6f); + } + break; + case FILTER_BIQUAD: + accSoftLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + accSoftLpfFilter[axis] = &accFilter[axis].biquad; + biquadFilterInitLPF(accSoftLpfFilter[axis], accelerometerConfig()->acc_lpf_hz, acc.accTargetLooptime); + } + break; } + } const float accDt = acc.accTargetLooptime * 1e-6f; diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 3dc5941f416..47f669b83a5 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -49,12 +49,20 @@ typedef enum { ACC_MAX = ACC_FAKE } accelerationSensor_e; +typedef struct { + float min; + float max; +} acc_extremes_t; + typedef struct acc_s { accDev_t dev; uint32_t accTargetLooptime; float accADCf[XYZ_AXIS_COUNT]; // acceleration in g float accVibeSq[XYZ_AXIS_COUNT]; uint32_t accClipCount; + bool isClipped; + acc_extremes_t extremes[XYZ_AXIS_COUNT]; + float maxG; } acc_t; extern acc_t acc; @@ -67,6 +75,7 @@ typedef struct accelerometerConfig_s { flightDynamicsTrims_t accGain; // Accelerometer gain to read exactly 1G uint8_t acc_notch_hz; // Accelerometer notch filter frequency uint8_t acc_notch_cutoff; // Accelerometer notch filter cutoff frequency + uint8_t acc_soft_lpf_type; // Accelerometer LPF type } accelerometerConfig_t; PG_DECLARE(accelerometerConfig_t, accelerometerConfig); @@ -75,9 +84,13 @@ bool accInit(uint32_t accTargetLooptime); bool accIsCalibrationComplete(void); void accStartCalibration(void); void accGetMeasuredAcceleration(fpVector3_t *measuredAcc); +const acc_extremes_t* accGetMeasuredExtremes(void); +float accGetMeasuredMaxG(void); +void updateAccExtremes(void); void accGetVibrationLevels(fpVector3_t *accVibeLevels); float accGetVibrationLevel(void); uint32_t accGetClipCount(void); +bool accIsClipped(void); void accUpdate(void); void accSetCalibrationValues(void); void accInitFilters(void); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index b37d17113bf..58e3a64cd31 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -21,12 +21,11 @@ #include "platform.h" -#include "build/debug.h" - +#include "common/calibration.h" +#include "common/log.h" #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" @@ -37,7 +36,6 @@ #include "drivers/barometer/barometer_lps25h.h" #include "drivers/barometer/barometer_fake.h" #include "drivers/barometer/barometer_ms56xx.h" -#include "drivers/logging.h" #include "drivers/time.h" #include "fc/runtime_config.h" @@ -164,8 +162,6 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse) break; } - addBootlogEvent6(BOOT_EVENT_BARO_DETECTION, BOOT_EVENT_FLAGS_NONE, baroHardware, 0, 0, 0); - if (baroHardware == BARO_NONE) { sensorsClear(SENSOR_BARO); return false; @@ -289,7 +285,7 @@ int32_t baroCalculateAltitude(void) if (zeroCalibrationIsCompleteS(&zeroCalibration)) { zeroCalibrationGetZeroS(&zeroCalibration, &baroGroundPressure); baroGroundAltitude = pressureToAltitude(baroGroundPressure); - DEBUG_TRACE_SYNC("Barometer calibration complete (%d)", lrintf(baroGroundAltitude)); + LOG_D(BARO, "Barometer calibration complete (%d)", (int)lrintf(baroGroundAltitude)); } baro.BaroAlt = 0; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index d9ef1cb4478..2fa663fd46e 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -157,7 +157,7 @@ static int profile_compare(profile_comp_t *a, profile_comp_t *b) { } // Find profile matching plugged battery for profile_autoselect -static int8_t profileDetect() { +static int8_t profileDetect(void) { profile_comp_t profile_comp_array[MAX_BATTERY_PROFILE_COUNT]; // Prepare profile sort diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 2ce03700749..45b9affad3b 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -148,6 +148,6 @@ void powerMeterUpdate(timeUs_t timeDelta); uint8_t calculateBatteryPercentage(void); float calculateThrottleCompensationFactor(void); -int32_t calculateAveragePower(); -int32_t calculateAverageEfficiency(); +int32_t calculateAveragePower(void); +int32_t calculateAverageEfficiency(void); int32_t heatLossesCompensatedPower(int32_t power); diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index 63bcd49d5e3..b23edb7af7b 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -20,6 +20,8 @@ #include #include +#include "platform.h" + #include "common/maths.h" #include "common/vector.h" #include "common/axis.h" @@ -29,6 +31,16 @@ #include "drivers/sensor.h" +#if defined(UNIT_TEST) +// Unit tests can't include settings. Provide some dummy limits. +#define SETTING_ALIGN_BOARD_ROLL_MIN -1800 +#define SETTING_ALIGN_BOARD_ROLL_MAX 3600 +#define SETTING_ALIGN_BOARD_PITCH_MIN -1800 +#define SETTING_ALIGN_BOARD_PITCH_MAX 3600 +#else +#include "fc/settings.h" +#endif + #include "boardalignment.h" static bool standardBoardAlignment = true; // board orientation correction @@ -64,13 +76,16 @@ void updateBoardAlignment(int16_t roll, int16_t pitch) const float sinAlignYaw = sin_approx(DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees)); const float cosAlignYaw = cos_approx(DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees)); - boardAlignmentMutable()->rollDeciDegrees += -sinAlignYaw * pitch + cosAlignYaw * roll; - boardAlignmentMutable()->pitchDeciDegrees += cosAlignYaw * pitch + sinAlignYaw * roll; + int16_t rollDeciDegrees = boardAlignment()->rollDeciDegrees + -sinAlignYaw * pitch + cosAlignYaw * roll; + int16_t pitchDeciDegrees = boardAlignment()->pitchDeciDegrees + cosAlignYaw * pitch + sinAlignYaw * roll; + + boardAlignmentMutable()->rollDeciDegrees = constrain(rollDeciDegrees, SETTING_ALIGN_BOARD_ROLL_MIN, SETTING_ALIGN_BOARD_ROLL_MAX); + boardAlignmentMutable()->pitchDeciDegrees = constrain(pitchDeciDegrees, SETTING_ALIGN_BOARD_PITCH_MIN, SETTING_ALIGN_BOARD_PITCH_MAX); initBoardAlignment(); } -void applyBoardAlignment(int32_t *vec) +void FAST_CODE applyBoardAlignment(int32_t *vec) { if (standardBoardAlignment) { return; @@ -84,7 +99,7 @@ void applyBoardAlignment(int32_t *vec) vec[Z] = lrintf(fpVec.z); } -void applySensorAlignment(int32_t * dest, int32_t * src, uint8_t rotation) +void FAST_CODE applySensorAlignment(int32_t * dest, int32_t * src, uint8_t rotation) { // Create a copy so we could use the same buffer for src & dest const int32_t x = src[X]; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 62b1540d243..0a0173dce82 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -42,7 +42,6 @@ #include "drivers/compass/compass_lis3mdl.h" #include "drivers/io.h" #include "drivers/light_led.h" -#include "drivers/logging.h" #include "drivers/time.h" #include "fc/config.h" @@ -267,8 +266,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) break; } - addBootlogEvent6(BOOT_EVENT_MAG_DETECTION, BOOT_EVENT_FLAGS_NONE, magHardware, 0, 0, 0); - if (magHardware == MAG_NONE) { sensorsClear(SENSOR_MAG); return false; @@ -296,7 +293,6 @@ bool compassInit(void) LED1_OFF; if (!ret) { - addBootlogEvent2(BOOT_EVENT_MAG_INIT_FAILED, BOOT_EVENT_FLAGS_ERROR); sensorsClear(SENSOR_MAG); } diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index 360d1d7230b..ca1c4c67f05 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -192,7 +192,7 @@ hardwareSensorStatus_e getHwGPSStatus(void) hardwareSensorStatus_e getHwOpticalFlowStatus(void) { -#if defined(USE_OPTICAL_FLOW) +#if defined(USE_OPFLOW) if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) { if (opflowIsHealthy()) { return HW_SENSOR_OK; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 6c26de7e04e..e650f7feb18 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -26,9 +26,10 @@ #include "build/debug.h" #include "common/axis.h" -#include "common/maths.h" #include "common/calibration.h" #include "common/filter.h" +#include "common/log.h" +#include "common/maths.h" #include "common/utils.h" #include "config/parameter_group.h" @@ -52,7 +53,6 @@ #include "drivers/accgyro/accgyro_icm20689.h" #include "drivers/accgyro/accgyro_fake.h" #include "drivers/io.h" -#include "drivers/logging.h" #include "fc/config.h" #include "fc/runtime_config.h" @@ -96,11 +96,12 @@ STATIC_FASTRAM filterApplyFnPtr gyroFilterStage2ApplyFn; STATIC_FASTRAM void *stage2Filter[XYZ_AXIS_COUNT]; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = GYRO_LPF_42HZ, // 42HZ value is defined for Invensense/TDK gyros .gyro_soft_lpf_hz = 60, + .gyro_soft_lpf_type = FILTER_BIQUAD, .gyro_align = ALIGN_DEFAULT, .gyroMovementCalibrationThreshold = 32, .looptime = 1000, @@ -243,8 +244,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard gyroHardware = GYRO_NONE; } - addBootlogEvent6(BOOT_EVENT_GYRO_DETECTION, BOOT_EVENT_FLAGS_NONE, gyroHardware, 0, 0, 0); - if (gyroHardware != GYRO_NONE) { detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; sensorsSet(SENSOR_GYRO); @@ -286,7 +285,7 @@ bool gyroInit(void) void gyroInitFilters(void) { - STATIC_FASTRAM biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; + STATIC_FASTRAM filter_t gyroFilterLPF[XYZ_AXIS_COUNT]; softLpfFilterApplyFn = nullFilterApply; #ifdef USE_GYRO_NOTCH_1 STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; @@ -311,10 +310,23 @@ void gyroInitFilters(void) #endif if (gyroConfig()->gyro_soft_lpf_hz) { - softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; - for (int axis = 0; axis < 3; axis++) { - softLpfFilter[axis] = &gyroFilterLPF[axis]; - biquadFilterInitLPF(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, getLooptime()); + + switch (gyroConfig()->gyro_soft_lpf_type) + { + case FILTER_PT1: + softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; + for (int axis = 0; axis < 3; axis++) { + softLpfFilter[axis] = &gyroFilterLPF[axis].pt1; + pt1FilterInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, getLooptime()* 1e-6f); + } + break; + case FILTER_BIQUAD: + softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; + for (int axis = 0; axis < 3; axis++) { + softLpfFilter[axis] = &gyroFilterLPF[axis].biquad; + biquadFilterInitLPF(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, getLooptime()); + } + break; } } @@ -344,7 +356,7 @@ void gyroStartCalibration(void) zeroCalibrationStartV(&gyroCalibration, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); } -bool gyroIsCalibrationComplete(void) +bool FAST_CODE NOINLINE gyroIsCalibrationComplete(void) { return zeroCalibrationIsCompleteV(&gyroCalibration) && zeroCalibrationIsSuccessfulV(&gyroCalibration); } @@ -367,7 +379,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe dev->gyroZero[1] = v.v[1]; dev->gyroZero[2] = v.v[2]; - DEBUG_TRACE_SYNC("Gyro calibration complete (%d, %d, %d)", dev->gyroZero[0], dev->gyroZero[1], dev->gyroZero[2]); + LOG_D(GYRO, "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 } else { @@ -387,7 +399,7 @@ void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate) } } -void gyroUpdate() +void FAST_CODE NOINLINE gyroUpdate() { // range: +/- 8192; +/- 2000 deg/sec if (gyroDev0.readFn(&gyroDev0)) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 61d65e62373..b6802b011cb 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -49,11 +49,11 @@ extern gyro_t gyro; typedef struct gyroConfig_s { sensor_align_e gyro_align; // gyro alignment uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. - uint8_t __deprecated_0; // Was: gyro sync denominator uint8_t gyroSync; // Enable interrupt based loop uint16_t looptime; // imu loop time in us uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_soft_lpf_hz; + uint8_t gyro_soft_lpf_type; uint8_t gyro_to_use; uint16_t gyro_soft_notch_hz_1; uint16_t gyro_soft_notch_cutoff_1; @@ -67,7 +67,7 @@ PG_DECLARE(gyroConfig_t, gyroConfig); bool gyroInit(void); void gyroInitFilters(void); void gyroGetMeasuredRotationRate(fpVector3_t *imuMeasuredRotationBF); -void gyroUpdate(); +void gyroUpdate(void); void gyroStartCalibration(void); bool gyroIsCalibrationComplete(void); bool gyroReadTemperature(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index e4990a14a62..394c627c69d 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,8 +24,6 @@ #include "config/config_eeprom.h" -#include "drivers/logging.h" - #include "fc/config.h" #include "fc/runtime_config.h" @@ -74,7 +72,7 @@ bool sensorsAutodetect(void) rangefinderInit(); #endif -#ifdef USE_OPTICAL_FLOW +#ifdef USE_OPFLOW opflowInit(); #endif diff --git a/src/main/sensors/opflow.c b/src/main/sensors/opflow.c index 6b5a417c811..942bc4ff843 100755 --- a/src/main/sensors/opflow.c +++ b/src/main/sensors/opflow.c @@ -41,7 +41,7 @@ #include "config/parameter_group_ids.h" #include "drivers/io.h" -#include "drivers/logging.h" +#include "drivers/light_led.h" #include "drivers/time.h" #include "drivers/opflow/opflow.h" @@ -64,17 +64,23 @@ opflow_t opflow; +#ifdef USE_OPFLOW +static bool opflowIsCalibrating = false; +static timeMs_t opflowCalibrationStartedAt; +static float opflowCalibrationBodyAcc; +static float opflowCalibrationFlowAcc; + #define OPFLOW_SQUAL_THRESHOLD_HIGH 35 // TBD #define OPFLOW_SQUAL_THRESHOLD_LOW 10 // TBD #define OPFLOW_UPDATE_TIMEOUT_US 200000 // At least 5Hz updates required +#define OPFLOW_CALIBRATE_TIME_MS 30000 // 30 second calibration time -#ifdef USE_OPTICAL_FLOW PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 1); PG_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, .opflow_hardware = OPFLOW_NONE, .opflow_align = CW0_DEG_FLIP, - .opflow_scale = 1.0f, + .opflow_scale = 10.5f, ); static bool opflowDetect(opflowDev_t * dev, uint8_t opflowHardwareToUse) @@ -112,8 +118,6 @@ static bool opflowDetect(opflowDev_t * dev, uint8_t opflowHardwareToUse) break; } - addBootlogEvent6(BOOT_EVENT_OPFLOW_DETECTION, BOOT_EVENT_FLAGS_NONE, opflowHardware, 0, 0, 0); - if (opflowHardware == OPFLOW_NONE) { sensorsClear(SENSOR_OPFLOW); return false; @@ -147,6 +151,14 @@ bool opflowInit(void) return true; } +void opflowStartCalibration(void) +{ + opflowCalibrationStartedAt = millis(); + opflowIsCalibrating = true; + opflowCalibrationBodyAcc = 0; + opflowCalibrationFlowAcc = 0; +} + /* * This is called periodically by the scheduler */ @@ -157,8 +169,8 @@ void opflowUpdate(timeUs_t currentTimeUs) if (opflow.dev.updateFn(&opflow.dev)) { // Indicate valid update - opflow.lastValidUpdate = currentTimeUs; opflow.isHwHealty = true; + opflow.lastValidUpdate = currentTimeUs; opflow.rawQuality = opflow.dev.rawData.quality; // Handle state switching @@ -176,33 +188,65 @@ void opflowUpdate(timeUs_t currentTimeUs) break; } - if ((opflow.flowQuality == OPFLOW_QUALITY_VALID) && (opflow.gyroBodyRateTimeUs > 0)) { - const float integralToRateScaler = (1.0e6 / opflow.dev.rawData.deltaTime) / (float)opticalFlowConfig()->opflow_scale; + // Opflow updated. Assume zero valus unless further processing sets otherwise + opflow.flowRate[X] = 0; + opflow.flowRate[Y] = 0; + opflow.bodyRate[X] = 0; + opflow.bodyRate[Y] = 0; + + // In the following code we operate deg/s and do conversion to rad/s in the last step + // Calculate body rates + if (opflow.gyroBodyRateTimeUs > 0) { + opflow.bodyRate[X] = opflow.gyroBodyRateAcc[X] / opflow.gyroBodyRateTimeUs; + opflow.bodyRate[Y] = opflow.gyroBodyRateAcc[Y] / opflow.gyroBodyRateTimeUs; + } + + // If quality of the flow from the sensor is good - process further + if (opflow.flowQuality == OPFLOW_QUALITY_VALID) { + const float integralToRateScaler = (opticalFlowConfig()->opflow_scale > 0.01f) ? (1.0e6 / opflow.dev.rawData.deltaTime) / (float)opticalFlowConfig()->opflow_scale : 0.0f; // Apply sensor alignment applySensorAlignment(opflow.dev.rawData.flowRateRaw, opflow.dev.rawData.flowRateRaw, opticalFlowConfig()->opflow_align); - //applyBoardAlignment(opflow.dev.rawData.flowRateRaw); - - opflow.flowRate[X] = DEGREES_TO_RADIANS(opflow.dev.rawData.flowRateRaw[X] * integralToRateScaler); - opflow.flowRate[Y] = DEGREES_TO_RADIANS(opflow.dev.rawData.flowRateRaw[Y] * integralToRateScaler); - opflow.bodyRate[X] = DEGREES_TO_RADIANS(opflow.gyroBodyRateAcc[X] / opflow.gyroBodyRateTimeUs); - opflow.bodyRate[Y] = DEGREES_TO_RADIANS(opflow.gyroBodyRateAcc[Y] / opflow.gyroBodyRateTimeUs); + // Calculate flow rate and accumulated body rate + opflow.flowRate[X] = opflow.dev.rawData.flowRateRaw[X] * integralToRateScaler; + opflow.flowRate[Y] = opflow.dev.rawData.flowRateRaw[Y] * integralToRateScaler; - DEBUG_SET(DEBUG_FLOW_RAW, 0, RADIANS_TO_DEGREES(opflow.dev.rawData.flowRateRaw[X])); - DEBUG_SET(DEBUG_FLOW_RAW, 1, RADIANS_TO_DEGREES(opflow.dev.rawData.flowRateRaw[Y])); - DEBUG_SET(DEBUG_FLOW_RAW, 2, RADIANS_TO_DEGREES(opflow.bodyRate[X])); - DEBUG_SET(DEBUG_FLOW_RAW, 3, RADIANS_TO_DEGREES(opflow.bodyRate[Y])); + // Only update DEBUG_FLOW_RAW if flow is good + DEBUG_SET(DEBUG_FLOW_RAW, 0, (opflow.flowRate[X])); + DEBUG_SET(DEBUG_FLOW_RAW, 1, (opflow.flowRate[Y])); + DEBUG_SET(DEBUG_FLOW_RAW, 2, (opflow.bodyRate[X])); + DEBUG_SET(DEBUG_FLOW_RAW, 3, (opflow.bodyRate[Y])); } - else { - // Opflow updated but invalid - zero out flow rates and body - opflow.flowRate[X] = 0; - opflow.flowRate[Y] = 0; - opflow.bodyRate[X] = 0; - opflow.bodyRate[Y] = 0; + // Process calibration + if (opflowIsCalibrating) { + // Blink LED + LED0_TOGGLE; + + if ((millis() - opflowCalibrationStartedAt) > OPFLOW_CALIBRATE_TIME_MS) { + // Finish calibration if we accumulated more than 3600 deg of rotation over 30 seconds + if (opflowCalibrationBodyAcc > 3600.0f) { + opticalFlowConfigMutable()->opflow_scale = opflowCalibrationFlowAcc / opflowCalibrationBodyAcc; + saveConfigAndNotify(); + } + + opflowIsCalibrating = 0; + } + else if (opflow.flowQuality == OPFLOW_QUALITY_VALID) { + // Ongoing calibration - accumulate body and flow rotation magniture if opflow quality is good enough + const float invDt = 1.0e6 / opflow.dev.rawData.deltaTime; + opflowCalibrationBodyAcc += sqrtf(sq(opflow.bodyRate[X]) + sq(opflow.bodyRate[Y])); + opflowCalibrationFlowAcc += sqrtf(sq(opflow.dev.rawData.flowRateRaw[X]) + sq(opflow.dev.rawData.flowRateRaw[Y])) * invDt; + } } + // Convert to radians so NAV doesn't have to do the conversion + opflow.bodyRate[X] = DEGREES_TO_RADIANS(opflow.bodyRate[X]); + opflow.bodyRate[Y] = DEGREES_TO_RADIANS(opflow.bodyRate[Y]); + opflow.flowRate[X] = DEGREES_TO_RADIANS(opflow.flowRate[X]); + opflow.flowRate[Y] = DEGREES_TO_RADIANS(opflow.flowRate[Y]); + // Zero out gyro accumulators to calculate rotation per flow update opflowZeroBodyGyroAcc(); } diff --git a/src/main/sensors/opflow.h b/src/main/sensors/opflow.h index c48b5dc1692..52fc487f7ed 100755 --- a/src/main/sensors/opflow.h +++ b/src/main/sensors/opflow.h @@ -69,4 +69,5 @@ extern opflow_t opflow; void opflowGyroUpdateCallback(timeUs_t gyroUpdateDeltaUs); bool opflowInit(void); void opflowUpdate(timeUs_t currentTimeUs); -bool opflowIsHealthy(void); \ No newline at end of file +bool opflowIsHealthy(void); +void opflowStartCalibration(void); diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 385cac4b088..b86f2c280bf 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -21,8 +21,7 @@ #include "platform.h" -#include "build/debug.h" - +#include "common/log.h" #include "common/maths.h" #include "common/time.h" #include "common/utils.h" @@ -30,10 +29,10 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" -#include "drivers/logging.h" #include "drivers/pitotmeter.h" #include "drivers/pitotmeter_ms4525.h" #include "drivers/pitotmeter_adc.h" +#include "drivers/pitotmeter_virtual.h" #include "drivers/time.h" #include "fc/config.h" @@ -51,8 +50,6 @@ pitot_t pitot; PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 2); #define PITOT_HARDWARE_TIMEOUT_MS 500 // Accept 500ms of non-responsive sensor, report HW failure otherwise -#define AIR_DENSITY_SEA_LEVEL_15C 1.225f // Air density at sea level and 15 degrees Celsius -#define P0 101325.0f // standard pressure [Pa] #ifdef USE_PITOT #define PITOT_HARDWARE_DEFAULT PITOT_AUTODETECT @@ -99,13 +96,11 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse) FALLTHROUGH; case PITOT_VIRTUAL: -#if defined(USE_PITOT_VIRTUAL) - /* - if (adcPitotDetect(&pitot)) { - pitotHardware = PITOT_ADC; +#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) + if ((pitotHardwareToUse != PITOT_AUTODETECT) && virtualPitotDetect(dev)) { + pitotHardware = PITOT_VIRTUAL; break; } - */ #endif /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ if (pitotHardwareToUse != PITOT_AUTODETECT) { @@ -131,8 +126,6 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse) break; } - addBootlogEvent6(BOOT_EVENT_PITOT_DETECTION, BOOT_EVENT_FLAGS_NONE, pitotHardware, 0, 0, 0); - if (pitotHardware == PITOT_NONE) { sensorsClear(SENSOR_PITOT); return false; @@ -167,7 +160,7 @@ static void performPitotCalibrationCycle(void) if (zeroCalibrationIsCompleteS(&pitot.zeroCalibration)) { zeroCalibrationGetZeroS(&pitot.zeroCalibration, &pitot.pressureZero); - DEBUG_TRACE_SYNC("Pitot calibration complete (%d)", lrintf(pitot.pressureZero)); + LOG_D(PITOT, "Pitot calibration complete (%d)", (int)lrintf(pitot.pressureZero)); } } diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index 548b23be02f..5bdd955ea22 100644 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -58,6 +58,9 @@ typedef struct pito_s { #ifdef USE_PITOT +#define AIR_DENSITY_SEA_LEVEL_15C 1.225f // Air density at sea level and 15 degrees Celsius +#define P0 101325.0f // standard pressure [Pa] + extern pitot_t pitot; bool pitotInit(void); diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 4db94d89a32..aec1e065aa7 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -34,7 +34,6 @@ #include "config/parameter_group_ids.h" #include "drivers/io.h" -#include "drivers/logging.h" #include "drivers/time.h" #include "drivers/rangefinder/rangefinder.h" #include "drivers/rangefinder/rangefinder_hcsr04.h" @@ -166,8 +165,6 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar break; } - addBootlogEvent6(BOOT_EVENT_RANGEFINDER_DETECTION, BOOT_EVENT_FLAGS_NONE, rangefinderHardware, 0, 0, 0); - if (rangefinderHardware == RANGEFINDER_NONE) { sensorsClear(SENSOR_RANGEFINDER); return false; diff --git a/src/main/sensors/temperature.c b/src/main/sensors/temperature.c index 3f6b61aab54..28cf60802d0 100644 --- a/src/main/sensors/temperature.c +++ b/src/main/sensors/temperature.c @@ -21,8 +21,6 @@ #include "platform.h" -#include "build/debug.h" - #include "common/maths.h" #include "common/printf.h" @@ -30,7 +28,6 @@ #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" @@ -45,7 +42,7 @@ #include "scheduler/protothreads.h" -PG_REGISTER_ARRAY(tempSensorConfig_t, MAX_TEMP_SENSORS, tempSensorConfig, PG_TEMP_SENSOR_CONFIG, 1); +PG_REGISTER_ARRAY(tempSensorConfig_t, MAX_TEMP_SENSORS, tempSensorConfig, PG_TEMP_SENSOR_CONFIG, 2); #define MPU_TEMP_VALID_BIT 0 #define BARO_TEMP_VALID_BIT 1 @@ -102,7 +99,6 @@ static void newSensorCheckAndEnter(uint8_t type, uint64_t addr) void temperatureInit(void) { memset(sensorStatus, 0, sizeof(sensorStatus) * sizeof(*sensorStatus)); - addBootlogEvent2(BOOT_EVENT_TEMP_SENSOR_DETECTION, BOOT_EVENT_FLAGS_NONE); sensorsSet(SENSOR_TEMP); @@ -164,7 +160,7 @@ bool getSensorTemperature(uint8_t temperatureUpdateSensorIndex, int16_t *tempera void tempSensorAddressToString(uint64_t address, char *hex_address) { if (address < 8) - tfp_sprintf(hex_address, "%d", address); + tfp_sprintf(hex_address, "%d", (int)address); else { uint32_t *address32 = (uint32_t *)&address; tfp_sprintf(hex_address, "%08lx%08lx", address32[1], address32[0]); diff --git a/src/main/sensors/temperature.h b/src/main/sensors/temperature.h index 9f6f8be0041..401a140f889 100644 --- a/src/main/sensors/temperature.h +++ b/src/main/sensors/temperature.h @@ -33,10 +33,10 @@ typedef enum { typedef struct { tempSensorType_e type; uint64_t address; - uint8_t osdSymbol; - char label[TEMPERATURE_LABEL_LEN]; int16_t alarm_min; int16_t alarm_max; + uint8_t osdSymbol; + char label[TEMPERATURE_LABEL_LEN]; } tempSensorConfig_t; PG_DECLARE_ARRAY(tempSensorConfig_t, MAX_TEMP_SENSORS, tempSensorConfig); diff --git a/src/main/startup/startup_stm32f722xx.s b/src/main/startup/startup_stm32f722xx.s index 69d6b475ec1..f856d04e3b2 100644 --- a/src/main/startup/startup_stm32f722xx.s +++ b/src/main/startup/startup_stm32f722xx.s @@ -134,6 +134,9 @@ LoopMarkHeapStack: cmp r2, r3 bcc MarkHeapStack + //If there was code addressed into ITCM, copy from flash to ITCM_RAM for execution + bl CopyFastCode + /* Call the clock system intitialization function.*/ bl SystemInit /* Call static constructors */ diff --git a/src/main/startup/startup_stm32f745xx.s b/src/main/startup/startup_stm32f745xx.s index f2b5950d7df..eb6aa25659a 100644 --- a/src/main/startup/startup_stm32f745xx.s +++ b/src/main/startup/startup_stm32f745xx.s @@ -138,6 +138,9 @@ LoopMarkHeapStack: orr r1,r1,#(0xF << 20) str r1,[r0] + //If there was code addressed into ITCM, copy from flash to ITCM_RAM for execution + bl CopyFastCode + /* Call the clock system initialization function.*/ bl SystemInit /* Call the application's entry point.*/ diff --git a/src/main/startup/startup_stm32f746xx.s b/src/main/startup/startup_stm32f746xx.s index 23f9cc2e8d0..8ef14a877ff 100644 --- a/src/main/startup/startup_stm32f746xx.s +++ b/src/main/startup/startup_stm32f746xx.s @@ -135,6 +135,9 @@ LoopMarkHeapStack: cmp r2, r3 bcc MarkHeapStack + //If there was code addressed into ITCM, copy from flash to ITCM_RAM for execution + bl CopyFastCode + /* Call the clock system initialization function.*/ bl SystemInit /* Call static constructors */ diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 21518419ab9..a7ab64be382 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -23,6 +23,7 @@ #include "common/axis.h" #include "drivers/pwm_esc_detect.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "fc/controlrate_profile.h" @@ -68,12 +69,12 @@ void targetConfiguration(void) controlRateProfilesMutable(0)->stabilized.rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT; parseRcChannels("TAER1234"); - *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R - *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R - *customMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L - *customMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L - *customMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R - *customMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L - *customMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R - *customMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L + *primaryMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + *primaryMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + *primaryMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + *primaryMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + *primaryMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + *primaryMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + *primaryMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + *primaryMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L } diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 11be0fcc7fd..36f871324a7 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -27,6 +27,7 @@ #include "drivers/sensor.h" #include "drivers/pwm_esc_detect.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "drivers/serial.h" @@ -83,12 +84,12 @@ void targetConfiguration(void) pidProfileMutable()->bank_mc.pid[YAW].P = 64; pidProfileMutable()->bank_mc.pid[YAW].D = 18; - *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R - *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R - *customMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L - *customMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L - *customMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R - *customMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L - *customMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R - *customMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L + *primaryMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + *primaryMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + *primaryMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + *primaryMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + *primaryMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + *primaryMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + *primaryMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + *primaryMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L } diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 9bdb5a14393..39bb130d0ce 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -126,7 +126,6 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PC2 #define SPI2_MOSI_PIN PC3 -#define SPI2_CLOCK_LEADING_EDGE #define SPI3_NSS_PIN PA15 #define SPI3_SCK_PIN PB3 diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index e15c1b7f229..e11cce5b2fe 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -24,6 +24,7 @@ #include "drivers/sensor.h" #include "drivers/pwm_esc_detect.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "drivers/serial.h" @@ -85,12 +86,12 @@ void targetConfiguration(void) pidProfileMutable()->bank_mc.pid[YAW].P = 64; pidProfileMutable()->bank_mc.pid[YAW].D = 18; - *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R - *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R - *customMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L - *customMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L - *customMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R - *customMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L - *customMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R - *customMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L + *primaryMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + *primaryMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + *primaryMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + *primaryMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + *primaryMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + *primaryMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + *primaryMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + *primaryMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L } diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 8eef1ed8ebb..116cf92d5f6 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -134,7 +134,6 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PC2 #define SPI2_MOSI_PIN PC3 -#define SPI2_CLOCK_LEADING_EDGE #define SPI3_NSS_PIN PA15 #define SPI3_SCK_PIN PB3 diff --git a/src/main/target/ANYFCF7/target.c b/src/main/target/ANYFCF7/target.c index 1a06c3b3eda..63038f96fb1 100644 --- a/src/main/target/ANYFCF7/target.c +++ b/src/main/target/ANYFCF7/target.c @@ -23,12 +23,12 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN - DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN - DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN DMA2_ST7 + DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN + DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM | TIM_USE_MC_SERVO, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM | TIM_USE_MC_SERVO, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM | TIM_USE_MC_SERVO, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM | TIM_USE_MC_SERVO, 0, 0 ), // S6_IN DMA2_ST7 DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S10_OUT 1 DMA1_ST7 DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S6_OUT 2 DMA1_ST0 diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index b174105df9c..288c8136f27 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -129,7 +129,6 @@ #define SPI4_SCK_PIN PE12 #define SPI4_MISO_PIN PE13 #define SPI4_MOSI_PIN PE14 -#define SPI4_CLOCK_LEADING_EDGE #define USE_OSD #define USE_MAX7456 @@ -152,7 +151,6 @@ #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) #define USE_NAV -#define NAV_AUTO_MAG_DECLINATION #define NAV_GPS_GLITCH_DETECTION #define USE_ADC diff --git a/src/main/target/ASGARD32F4/config.c b/src/main/target/ASGARD32F4/config.c index 8929c9ce7b4..44ba43c0cd5 100644 --- a/src/main/target/ASGARD32F4/config.c +++ b/src/main/target/ASGARD32F4/config.c @@ -27,6 +27,7 @@ #include "drivers/io.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" #include "flight/mixer.h" #include "rx/rx.h" #include "io/serial.h" diff --git a/src/main/target/ASGARD32F7/config.c b/src/main/target/ASGARD32F7/config.c index 8929c9ce7b4..44ba43c0cd5 100644 --- a/src/main/target/ASGARD32F7/config.c +++ b/src/main/target/ASGARD32F7/config.c @@ -27,6 +27,7 @@ #include "drivers/io.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" #include "flight/mixer.h" #include "rx/rx.h" #include "io/serial.h" diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index 20a6963cb01..1c11634ba83 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -117,7 +117,6 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define SPI2_CLOCK_LEADING_EDGE //MAX7456 / SPI RX #define USE_SPI_DEVICE_3 diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 978ef55dc1a..3b70e722fc0 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -83,7 +83,6 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define SPI2_CLOCK_LEADING_EDGE #define USE_OSD #define USE_MAX7456 diff --git a/src/main/target/BETAFLIGHTF4/target.c b/src/main/target/BETAFLIGHTF4/target.c index 18d56e7e8bc..fd5026f669d 100755 --- a/src/main/target/BETAFLIGHTF4/target.c +++ b/src/main/target/BETAFLIGHTF4/target.c @@ -27,10 +27,10 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM // Motors - DEF_TIM(TIM1, CH2, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D2_ST6 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2_OUT D1_ST2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT D1_ST6 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1 + DEF_TIM(TIM1, CH2N, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D2_ST6 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT D1_ST6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT D1_ST1 // LED strip DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // D1_ST0 diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index bf5dc37144a..92184c09309 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -121,7 +121,6 @@ #define SPI3_SCK_PIN PC10 #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 -#define SPI3_CLOCK_LEADING_EDGE #define USE_I2C #define USE_I2C_DEVICE_1 diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index ade4be0ccb8..bcdf88a1e8a 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -108,7 +108,6 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define SPI2_CLOCK_LEADING_EDGE #define USE_SPI_DEVICE_3 #define SPI3_NSS_PIN PA15 diff --git a/src/main/target/FALCORE/config.c b/src/main/target/FALCORE/config.c index 7d4379a9ddc..231f0dd6b1b 100755 --- a/src/main/target/FALCORE/config.c +++ b/src/main/target/FALCORE/config.c @@ -19,6 +19,7 @@ #include "platform.h" #include "config/feature.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" #include "blackbox/blackbox.h" #include "fc/config.h" #include "fc/controlrate_profile.h" diff --git a/src/main/target/FF_F35_LIGHTNING/config.c b/src/main/target/FF_F35_LIGHTNING/config.c index d55462b72c8..14c033fb23c 100644 --- a/src/main/target/FF_F35_LIGHTNING/config.c +++ b/src/main/target/FF_F35_LIGHTNING/config.c @@ -48,7 +48,7 @@ void targetConfiguration(void) rxConfigMutable()->serialrx_provider = SERIALRX_CRSF; serialConfigMutable()->portConfigs[6].functionMask = FUNCTION_TELEMETRY_SMARTPORT; - telemetryConfigMutable()->smartportUartUnidirectional = 1; + telemetryConfigMutable()->uartUnidirectional = 1; mixerConfigMutable()->platformType = PLATFORM_AIRPLANE; } diff --git a/src/main/target/FF_F35_LIGHTNING/target.c b/src/main/target/FF_F35_LIGHTNING/target.c index 0c0bb591e8d..3a9692f5fe5 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.c +++ b/src/main/target/FF_F35_LIGHTNING/target.c @@ -30,6 +30,9 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), + DEF_TIM(TIM5, CH4, PA3, TIM_USE_ANY, 0, 0), + #ifdef WINGFC DEF_TIM(TIM2, CH4, PB11, TIM_USE_PPM, 0, 0), // UART3 RX #else diff --git a/src/main/target/FF_F35_LIGHTNING/target.h b/src/main/target/FF_F35_LIGHTNING/target.h index d7364d68af7..e50a43485d1 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.h +++ b/src/main/target/FF_F35_LIGHTNING/target.h @@ -87,7 +87,11 @@ #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 -#define SERIAL_PORT_COUNT 7 //VCP, UART1, UART2, UART3, UART4, UART5, UART6 +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_RX_PIN PA3 // shared with UART2 RX +#define SOFTSERIAL_1_TX_PIN PA2 // shared with UART2 TX + +#define SERIAL_PORT_COUNT 8 //VCP, UART1, UART2, UART3, UART4, UART5, UART6 #define USE_SPI #define USE_SPI_DEVICE_1 diff --git a/src/main/target/FF_F35_LIGHTNING/target.mk b/src/main/target/FF_F35_LIGHTNING/target.mk index 167cd1789d3..8162b1f3a9c 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.mk +++ b/src/main/target/FF_F35_LIGHTNING/target.mk @@ -11,6 +11,7 @@ TARGET_SRC = \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_mag3110.c \ drivers/rangefinder/rangefinder_hcsr04.c \ + drivers/serial_softserial.c \ drivers/serial_usb_vcp.c \ drivers/max7456.c diff --git a/src/main/target/FF_PIKOF4/config.c b/src/main/target/FF_PIKOF4/config.c index 14e365618ab..6f00b0fccb7 100644 --- a/src/main/target/FF_PIKOF4/config.c +++ b/src/main/target/FF_PIKOF4/config.c @@ -28,6 +28,6 @@ void targetConfiguration(void) { rxConfigMutable()->halfDuplex = false; - telemetryConfigMutable()->smartportUartUnidirectional = true; + telemetryConfigMutable()->uartUnidirectional = true; batteryMetersConfigMutable()->current.scale = CURRENTSCALE; } diff --git a/src/main/target/FIREWORKSV2/target.c b/src/main/target/FIREWORKSV2/target.c index a232dd5881d..1eb592444df 100644 --- a/src/main/target/FIREWORKSV2/target.c +++ b/src/main/target/FIREWORKSV2/target.c @@ -48,7 +48,9 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S3_OUT D(1,6) DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4_OUT D(1,5) - + DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5_OUT D(1,7) + DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6_OUT D(1,8) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip D(1,0) DEF_TIM(TIM1, CH2, PA9, TIM_USE_ANY, 0, 0), // SS1 diff --git a/src/main/target/FIREWORKSV2/target.h b/src/main/target/FIREWORKSV2/target.h index f5aab377b24..3ea92fbd50e 100644 --- a/src/main/target/FIREWORKSV2/target.h +++ b/src/main/target/FIREWORKSV2/target.h @@ -182,11 +182,11 @@ #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 -#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_BUS BUS_SPI2 #define USE_FLASHFS #define USE_FLASH_M25P16 +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_BUS BUS_SPI2 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define USE_ADC #define ADC_CHANNEL_1_PIN PC1 @@ -223,8 +223,8 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE // Number of available PWM outputs -#define MAX_PWM_OUTPUT_PORTS 4 -#define TARGET_MOTOR_COUNT 4 +#define MAX_PWM_OUTPUT_PORTS 8 +#define TARGET_MOTOR_COUNT 6 #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index 9e8a1921cfc..bbea1613015 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -94,7 +94,6 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 #define SPI3_NSS_PIN PB6 -#define SPI3_CLOCK_LEADING_EDGE #define USE_SDCARD #define USE_SDCARD_SPI @@ -161,7 +160,6 @@ // *************** NAV ***************************** #define USE_NAV -#define NAV_AUTO_MAG_DECLINATION #define NAV_GPS_GLITCH_DETECTION #define NAV_MAX_WAYPOINTS 60 diff --git a/src/main/target/FRSKYF3/target.h b/src/main/target/FRSKYF3/target.h index 13a2bfad993..6fe5507157f 100644 --- a/src/main/target/FRSKYF3/target.h +++ b/src/main/target/FRSKYF3/target.h @@ -82,7 +82,6 @@ #define SPI1_SCK_PIN PA5 #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 -#define SPI1_CLOCK_LEADING_EDGE #define USE_SDCARD #define USE_SDCARD_SPI diff --git a/src/main/target/FRSKYF4/target.h b/src/main/target/FRSKYF4/target.h index cefdc61f3ad..b727701acee 100755 --- a/src/main/target/FRSKYF4/target.h +++ b/src/main/target/FRSKYF4/target.h @@ -84,7 +84,6 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define SPI2_CLOCK_LEADING_EDGE #define USE_SPI_DEVICE_3 #define SPI3_NSS_PIN PA15 diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 41e0a224854..114ee507c3c 100755 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -92,7 +92,6 @@ #ifdef USE_SDCARD #define USE_SDCARD_SPI -#define SPI2_CLOCK_LEADING_EDGE #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PB2 diff --git a/src/main/target/FURYF4OSD/MAMBAF405.mk b/src/main/target/FURYF4OSD/MAMBAF405.mk new file mode 100644 index 00000000000..50a56f6187e --- /dev/null +++ b/src/main/target/FURYF4OSD/MAMBAF405.mk @@ -0,0 +1 @@ +# MAMBAF405 diff --git a/src/main/target/FURYF4OSD/target.c b/src/main/target/FURYF4OSD/target.c new file mode 100644 index 00000000000..56881d7c814 --- /dev/null +++ b/src/main/target/FURYF4OSD/target.c @@ -0,0 +1,36 @@ +/* +* 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 . +*/ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const timerHardware_t timerHardware[] = { + + DEF_TIM(TIM8, CH4, PC9, TIM_USE_PPM, 0, 0 ), // PPM IN + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S1_OUT - DMA1_ST6_CH3 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S2_OUT - DMA1_ST7_CH5 + DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3_OUT - DMA2_ST6_CH0 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0 ), // S4_OUT - DMA1_ST1_CH3 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_ANY, 0, 0), + DEF_TIM(TIM2, CH4, PB11, TIM_USE_ANY, 0, 0), + DEF_TIM(TIM5, CH1, PA0, TIM_USE_LED, 0, 0 ), // LED_STRIP - DMA1_ST2_CH6 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FURYF4OSD/target.h b/src/main/target/FURYF4OSD/target.h new file mode 100644 index 00000000000..294686f22f4 --- /dev/null +++ b/src/main/target/FURYF4OSD/target.h @@ -0,0 +1,180 @@ +/* + * 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 . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "FYF4" + +#define USBD_PRODUCT_STRING "FuryF4" + +#define LED0 PB5 +#define LED1 PB4 + +#define BEEPER PA8 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_BUS BUS_SPI1 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_EXTI +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + +#define USE_GYRO +#define USE_GYRO_MPU6500 +#define GYRO_MPU6500_ALIGN CW180_DEG + +#define USE_GYRO_MPU6000 +#define GYRO_MPU6000_ALIGN CW180_DEG + +#define USE_ACC +#define USE_ACC_MPU6500 +#define ACC_MPU6500_ALIGN CW180_DEG + +#define USE_ACC_MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +// *************** M25P256 flash ******************** +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PB3 + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** UART ***************************** +#define USE_VCP +#define VBUS_SENSING_PIN PC5 +#define VBUS_SENSING_ENABLED + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_RX_PIN PA1 +#define SOFTSERIAL_1_TX_PIN PC9 // PPM + +//#define USE_SOFTSERIAL2 +//#define SOFTSERIAL_2_RX_PIN PA2 +//#define SOFTSERIAL_2_TX_PIN PA2 + +#define SERIAL_PORT_COUNT 5 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_RANGEFINDER_HCSR04_I2C +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC1 +#ifdef MAMBAF405 +#define ADC_CHANNEL_2_PIN PC3 +#define ADC_CHANNEL_3_PIN PC2 +#else +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC3 +#endif +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) +#define CURRENT_METER_SCALE 179 + +#define USE_LED_STRIP +#define WS2811_PIN PA0 + +//#define USE_SPEKTRUM_BIND +//#define BIND_PIN PA3 // RX2 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define USE_DSHOT + +#define MAX_PWM_OUTPUT_PORTS 6 + +#define PCA9685_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/FURYF4OSD/target.mk b/src/main/target/FURYF4OSD/target.mk new file mode 100644 index 00000000000..654b3888b0a --- /dev/null +++ b/src/main/target/FURYF4OSD/target.mk @@ -0,0 +1,20 @@ +F405_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_mpu6000.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms56xx.c \ + drivers/compass/compass_ak8963.c \ + drivers/compass/compass_ak8975.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_ist8310.c \ + drivers/compass/compass_ist8308.c \ + drivers/compass/compass_mag3110.c \ + drivers/compass/compass_lis3mdl.c \ + drivers/pitotmeter_adc.c \ + drivers/light_ws2811strip.c \ + drivers/max7456.c diff --git a/src/main/target/KAKUTEF7/KAKUTEF7MINI.mk b/src/main/target/KAKUTEF7/KAKUTEF7MINI.mk new file mode 100644 index 00000000000..f5763af08d9 --- /dev/null +++ b/src/main/target/KAKUTEF7/KAKUTEF7MINI.mk @@ -0,0 +1 @@ +#KAKUTEF7MINI.mk file diff --git a/src/main/target/KAKUTEF7/target.c b/src/main/target/KAKUTEF7/target.c index f0451514875..75e2a1c6392 100755 --- a/src/main/target/KAKUTEF7/target.c +++ b/src/main/target/KAKUTEF7/target.c @@ -31,16 +31,16 @@ BUSDEV_REGISTER_SPI_TAG(busdev_icm20689, DEVHW_ICM20689, ICM20689_SPI_BUS, ICM20689_CS_PIN, ICM20689_EXTI_PIN, 0, DEVFLAGS_NONE); const timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 + DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M1 , DMA1_ST7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M2 , DMA1_ST2 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // M3 , DMA2_ST2 - DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // M4 , DMA2_ST4 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // M5 , DMA2_ST7 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // M6 , DMA1_ST1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M1 , DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M2 , DMA1_ST2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // M3 , DMA2_ST2 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // M4 , DMA2_ST4 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M5 , DMA2_ST7 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M6 , DMA1_ST1 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_TRIP, DMA1_ST0 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_TRIP, DMA1_ST0 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index 9d0b44791e4..7c7ce5b8fc4 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -20,8 +20,13 @@ #pragma once +#ifdef KAKUTEF7MINI +#define TARGET_BOARD_IDENTIFIER "KF7M" +#define USBD_PRODUCT_STRING "KakuteF7-Mini" +#else #define TARGET_BOARD_IDENTIFIER "KTF7" #define USBD_PRODUCT_STRING "KakuteF7" +#endif #define LED0 PA2 @@ -84,7 +89,6 @@ #define SPI1_SCK_PIN PA5 #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 -#define SPI1_CLOCK_LEADING_EDGE #define SPI2_NSS_PIN PB12 #define SPI2_SCK_PIN PB13 @@ -101,12 +105,20 @@ #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN SPI2_NSS_PIN +#if defined(KAKUTEF7MINI) +#define M25P16_CS_PIN SPI1_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI1 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#else #define USE_SDCARD #define USE_SDCARD_SPI #define SDCARD_SPI_BUS BUS_SPI1 #define SDCARD_CS_PIN SPI1_NSS_PIN #define SDCARD_DETECT_INVERTED #define SDCARD_DETECT_PIN PD8 +#endif #define USE_I2C #define USE_I2C_DEVICE_1 diff --git a/src/main/target/KAKUTEF7/target.mk b/src/main/target/KAKUTEF7/target.mk index f22a7f7d6fa..ae3033bf2a4 100755 --- a/src/main/target/KAKUTEF7/target.mk +++ b/src/main/target/KAKUTEF7/target.mk @@ -1,5 +1,9 @@ F7X5XG_TARGETS += $(TARGET) +ifeq ($(TARGET), KAKUTEF7MINI) +FEATURES += VCP ONBOARDFLASH +else FEATURES += SDCARD VCP +endif TARGET_SRC = \ drivers/accgyro/accgyro_icm20689.c \ diff --git a/src/main/target/KFC32F3_INAV/target.c b/src/main/target/KFC32F3_INAV/target.c index 83ad48dfaf3..a32e395332c 100755 --- a/src/main/target/KFC32F3_INAV/target.c +++ b/src/main/target/KFC32F3_INAV/target.c @@ -25,7 +25,7 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH1, PA0, TIM_USE_PPM, 0), // PPM - + DEF_TIM(TIM8, CH3, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM2 DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 @@ -35,7 +35,7 @@ const timerHardware_t timerHardware[] = DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM7 DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM8 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_LED, 0), // S1_out + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // S1_out DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // S2_out }; diff --git a/src/main/target/KFC32F3_INAV/target.h b/src/main/target/KFC32F3_INAV/target.h index 6fb2a9dd79b..40808156df2 100755 --- a/src/main/target/KFC32F3_INAV/target.h +++ b/src/main/target/KFC32F3_INAV/target.h @@ -127,10 +127,10 @@ #define RSSI_ADC_CHANNEL ADC_CHN_2 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 -#define USE_LED_STRIP -#define WS2811_PIN PA8 -#define WS2811_DMA_STREAM DMA1_Channel2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER +//#define USE_LED_STRIP +//#define WS2811_PIN PA8 +//#define WS2811_DMA_STREAM DMA1_Channel2 +//#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_OSD | FEATURE_VBAT) #define DEFAULT_RX_TYPE RX_TYPE_PPM diff --git a/src/main/target/KROOZX/target.h b/src/main/target/KROOZX/target.h index acbbb08865c..924b619277c 100755 --- a/src/main/target/KROOZX/target.h +++ b/src/main/target/KROOZX/target.h @@ -136,7 +136,6 @@ #define SPI3_SCK_PIN PB3 #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define SPI3_CLOCK_LEADING_EDGE #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index 6d635a92fd3..811fbeb73c9 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -69,7 +69,6 @@ #define SPI3_SCK_PIN PB3 #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define SPI3_CLOCK_LEADING_EDGE #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -170,9 +169,6 @@ #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS -#define USE_OPTICAL_FLOW -#define USE_OPFLOW_MSP - #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP #define USE_RANGEFINDER_HCSR04_I2C @@ -208,6 +204,7 @@ #define TARGET_IO_PORTD (BIT(2)) #define USE_DSHOT +#define USE_SERIALSHOT #define MAX_PWM_OUTPUT_PORTS 6 diff --git a/src/main/target/MATEKF405SE/config.c b/src/main/target/MATEKF405SE/config.c index 324138e9e7f..c6176712f92 100644 --- a/src/main/target/MATEKF405SE/config.c +++ b/src/main/target/MATEKF405SE/config.c @@ -26,9 +26,7 @@ // alternative defaults settings for MATEKF405SE targets void targetConfiguration(void) { - motorConfigMutable()->minthrottle = 1050; - motorConfigMutable()->maxthrottle = 1950; - + serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; serialConfigMutable()->portConfigs[1].msp_baudrateIndex = BAUD_57600; @@ -37,4 +35,4 @@ void targetConfiguration(void) mixerConfigMutable()->platformType = PLATFORM_AIRPLANE; // default mixer to Airplane serialConfigMutable()->portConfigs[7].functionMask = FUNCTION_TELEMETRY_SMARTPORT; -} \ No newline at end of file +} diff --git a/src/main/target/MATEKF405SE/target.c b/src/main/target/MATEKF405SE/target.c index 6e77baa43f1..1ab3ec5c702 100644 --- a/src/main/target/MATEKF405SE/target.c +++ b/src/main/target/MATEKF405SE/target.c @@ -22,18 +22,18 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S1 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S2 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S1 D(1,3,2) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S2 D(1,0,2) - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S3 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S5 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S3 D(1,7,5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4 D(1,2,5) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S5 D(2,4,7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S6 D(2,7,7) DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S7 DEF_TIM(TIM12, CH2, PB15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S8 DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // S9 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), //2812LED + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h index 39ec76c8e65..abb6d48b77a 100644 --- a/src/main/target/MATEKF405SE/target.h +++ b/src/main/target/MATEKF405SE/target.h @@ -104,7 +104,6 @@ #define SPI3_SCK_PIN PB3 #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define SPI3_CLOCK_LEADING_EDGE #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -172,6 +171,8 @@ #define USE_SPEKTRUM_BIND #define BIND_PIN PA3 // RX2 +#define USE_DSHOT + #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/MATEKF411/MATEKF411_FD_SFTSRL.mk b/src/main/target/MATEKF411/MATEKF411_FD_SFTSRL.mk new file mode 100644 index 00000000000..2f32a7c73b4 --- /dev/null +++ b/src/main/target/MATEKF411/MATEKF411_FD_SFTSRL.mk @@ -0,0 +1 @@ +# MATEKF411_FD_SFTSRL diff --git a/src/main/target/MATEKF411/target.c b/src/main/target/MATEKF411/target.c index 553718340ac..25fea5b85d1 100755 --- a/src/main/target/MATEKF411/target.c +++ b/src/main/target/MATEKF411/target.c @@ -25,16 +25,16 @@ const timerHardware_t timerHardware[] = { // DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM IN - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT - - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 - - DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), //softserial_tx2 - 2812LED TIM_USE_LED + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,4,5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,5,5) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(1,0,2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(1,3,2) + + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 D(1,6,3) + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,1,3) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,5,3) - clash with S2 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), //softserial_tx2 - 2812LED TIM_USE_LED D(2,1,6) DEF_TIM(TIM5, CH1, PA0, TIM_USE_PPM, 0, 0), //use rssi pad for PPM/softserial_tx1 //DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), // TX2 diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h index 79bd5d2b72d..f6a2de733f8 100755 --- a/src/main/target/MATEKF411/target.h +++ b/src/main/target/MATEKF411/target.h @@ -32,8 +32,8 @@ #define USE_SPI_DEVICE_1 #define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_BUS BUS_SPI1 @@ -53,8 +53,8 @@ // *************** SPI2 OSD ***************************** #define USE_SPI_DEVICE_2 #define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 #define USE_OSD #define USE_MAX7456 @@ -83,6 +83,12 @@ #define SOFTSERIAL_2_RX_PIN PA8 #define SERIAL_PORT_COUNT 5 +#elif defined(MATEKF411_FD_SFTSRL) +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA0 // ST1 pad +#define SOFTSERIAL_1_RX_PIN PA8 // LED pad +#define SERIAL_PORT_COUNT 4 + #elif defined(MATEKF411_RSSI) #define USE_SOFTSERIAL1 #define SOFTSERIAL_1_TX_PIN PA8 // LED pad @@ -141,7 +147,7 @@ #endif // *************** LED2812 ************************ -#if !defined(MATEKF411_SFTSRL2) && !defined(MATEKF411_RSSI) +#if !defined(MATEKF411_SFTSRL2) && !defined(MATEKF411_RSSI) && !defined(MATEKF411_FD_SFTSRL) #define USE_LED_STRIP #define WS2811_PIN PA8 #endif @@ -151,6 +157,9 @@ #define USE_SPEKTRUM_BIND #define BIND_PIN PA10 // RX1 +#define USE_DSHOT + +#define USE_SERIALSHOT #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h index 6318091d00a..7f43c4f1697 100755 --- a/src/main/target/MATEKF722/target.h +++ b/src/main/target/MATEKF722/target.h @@ -85,7 +85,6 @@ #define SPI3_SCK_PIN PB3 #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define SPI3_CLOCK_LEADING_EDGE #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -162,3 +161,4 @@ #define MAX_PWM_OUTPUT_PORTS 7 #define USE_DSHOT +#define USE_SERIALSHOT diff --git a/src/main/target/MATEKF722SE/target.h b/src/main/target/MATEKF722SE/target.h index 7d98e23a3b5..79d0038a1b3 100644 --- a/src/main/target/MATEKF722SE/target.h +++ b/src/main/target/MATEKF722SE/target.h @@ -122,7 +122,6 @@ # define SDCARD_SPI_BUS BUS_SPI3 # define SDCARD_CS_PIN PD2 # define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -# define SPI3_CLOCK_LEADING_EDGE // TODO(digitalentity): implement DEVFLAGS_SPI_MODE_0 flag in SPI drivers #endif // *************** UART ***************************** @@ -197,3 +196,4 @@ #define MAX_PWM_OUTPUT_PORTS 8 #define USE_DSHOT +#define USE_SERIALSHOT diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index cccbce0120d..11270303791 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -96,7 +96,6 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define SPI2_CLOCK_LEADING_EDGE //#define USE_RX_SPI #define RX_SPI_INSTANCE SPI2 diff --git a/src/main/target/OMNIBUSF4/OMNIBUSF4V3_S5S6_SS.mk b/src/main/target/OMNIBUSF4/OMNIBUSF4V3_S5S6_SS.mk new file mode 100644 index 00000000000..b455b4036bd --- /dev/null +++ b/src/main/target/OMNIBUSF4/OMNIBUSF4V3_S5S6_SS.mk @@ -0,0 +1 @@ +# OMNIBUSF4V3_S5S6_SS diff --git a/src/main/target/OMNIBUSF4/OMNIBUSF4V3_S5_S6_2SS.mk b/src/main/target/OMNIBUSF4/OMNIBUSF4V3_S5_S6_2SS.mk new file mode 100644 index 00000000000..f07faf414ce --- /dev/null +++ b/src/main/target/OMNIBUSF4/OMNIBUSF4V3_S5_S6_2SS.mk @@ -0,0 +1 @@ +# OMNIBUSF4V3_S5_S6_2SS diff --git a/src/main/target/OMNIBUSF4/OMNIBUSF4V3_S6_SS.mk b/src/main/target/OMNIBUSF4/OMNIBUSF4V3_S6_SS.mk new file mode 100644 index 00000000000..207070ba7f5 --- /dev/null +++ b/src/main/target/OMNIBUSF4/OMNIBUSF4V3_S6_SS.mk @@ -0,0 +1 @@ +# OMNIBUSF4V3_S6_SS diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index c32af130e93..01c6b313a03 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -33,8 +33,8 @@ const timerHardware_t timerHardware[] = { #endif DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // S3_IN, UART6_TX DEF_TIM(TIM8, CH2, PC7, TIM_USE_ANY, 0, 0), // S4_IN, UART6_RX - DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN - DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_ANY, 0, 0), // S5_IN // pad labelled CH5 on OMNIBUSF4PRO + DEF_TIM(TIM8, CH4, PC9, TIM_USE_ANY, 0, 0), // S6_IN // pad labelled CH6 on OMNIBUSF4PRO DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT D1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT D1_ST2 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 6ab62f25500..47988469aba 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -22,7 +22,7 @@ #endif #ifdef OMNIBUSF4PRO #define TARGET_BOARD_IDENTIFIER "OBSD" -#elif defined(OMNIBUSF4V3) +#elif defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS) #define TARGET_BOARD_IDENTIFIER "OB43" #elif defined(DYSF4PRO) #define TARGET_BOARD_IDENTIFIER "DYS4" @@ -35,7 +35,7 @@ #if defined(DYSF4PRO) #define USBD_PRODUCT_STRING "DysF4Pro" #else -#define USBD_PRODUCT_STRING "Omnibus F4" +#define USBD_PRODUCT_STRING "Omnibus F4" #endif #define LED0 PB5 @@ -69,22 +69,22 @@ #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_BUS BUS_SPI1 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) +#if defined(DYSF4PRO) || defined(DYSF4PROV2) #define USE_GYRO_MPU6000 - #define GYRO_MPU6000_ALIGN CW270_DEG + #define GYRO_MPU6000_ALIGN CW180_DEG #define USE_ACC_MPU6000 - #define ACC_MPU6000_ALIGN CW270_DEG + #define ACC_MPU6000_ALIGN CW180_DEG #else #define USE_GYRO_MPU6000 - #define GYRO_MPU6000_ALIGN CW180_DEG + #define GYRO_MPU6000_ALIGN CW270_DEG #define USE_ACC_MPU6000 - #define ACC_MPU6000_ALIGN CW180_DEG + #define ACC_MPU6000_ALIGN CW270_DEG #endif // Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000 -#if defined (OMNIBUSF4PRO) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4PRO_LEDSTRIPM5) +#if !defined(DYSF4PRO) && !defined(DYSF4PROV2) #define MPU6500_CS_PIN MPU6000_CS_PIN #define MPU6500_SPI_BUS MPU6000_SPI_BUS @@ -110,7 +110,12 @@ #define USE_BARO -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) +#if defined(DYSF4PRO) || defined(DYSF4PROV2) + #define BARO_I2C_BUS I2C_EXT_BUS + #define USE_BARO_BMP085 + #define USE_BARO_BMP280 + #define USE_BARO_MS5611 +#else #define USE_BARO_BMP280 #define BMP280_SPI_BUS BUS_SPI3 #define BMP280_CS_PIN PB3 // v1 @@ -119,11 +124,6 @@ #define BARO_I2C_BUS I2C_EXT_BUS #define USE_BARO_BMP085 #define USE_BARO_MS5611 -#else - #define BARO_I2C_BUS I2C_EXT_BUS - #define USE_BARO_BMP085 - #define USE_BARO_BMP280 - #define USE_BARO_MS5611 #endif #define PITOT_I2C_BUS I2C_EXT_BUS @@ -131,9 +131,6 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS I2C_EXT_BUS -#define USE_OPTICAL_FLOW -#define USE_OPFLOW_CXOF - #define USE_VCP #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED @@ -144,7 +141,7 @@ #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 #define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 -#if !defined(OMNIBUSF4V3) +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4PRO_LEDSTRIPM5) #define INVERTER_PIN_UART1_RX PC0 // PC0 has never been used as inverter control on genuine OMNIBUS F4 variants, but leave it as is since some clones actually implement it. #endif @@ -155,32 +152,57 @@ #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 -#if defined(OMNIBUSF4V3) +#if defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS) #define INVERTER_PIN_UART6_RX PC8 #define INVERTER_PIN_UART6_TX PC9 #endif #if defined(OMNIBUSF4V3) #define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_RX_PIN PC6 //shared with UART6_TX -#define SOFTSERIAL_1_TX_PIN PC6 //shared with UART6_TX +#define SOFTSERIAL_1_RX_PIN PC6 // shared with UART6 TX +#define SOFTSERIAL_1_TX_PIN PC6 // shared with UART6 TX -#define SERIAL_PORT_COUNT 5 //VCP, USART1, USART3, USART6, SOFTSERIAL1 -#else +#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 + +#elif defined(OMNIBUSF4V3_S6_SS) // one softserial on S6 #define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_RX_PIN PC8 -#define SOFTSERIAL_1_TX_PIN PC9 +#define SOFTSERIAL_1_RX_PIN PA8 // shared with S6 output +#define SOFTSERIAL_1_TX_PIN PA8 // shared with S6 output -#define SERIAL_PORT_COUNT 5 //VCP, USART1, USART3, USART6, SOFTSERIAL1 +#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 + +#elif defined(OMNIBUSF4V3_S5S6_SS) // one softserial on S5/RX S6/TX +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_RX_PIN PA1 // shared with S5 output +#define SOFTSERIAL_1_TX_PIN PA8 // shared with S6 output + +#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 + +#elif defined(OMNIBUSF4V3_S5_S6_2SS) // two softserials, one on S5 and one on S6 +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_RX_PIN PA1 // shared with S5 output +#define SOFTSERIAL_1_TX_PIN PA1 // shared with S5 output + +#define USE_SOFTSERIAL2 +#define SOFTSERIAL_2_RX_PIN PA8 // shared with S6 output +#define SOFTSERIAL_2_TX_PIN PA8 // shared with S6 output + +#define SERIAL_PORT_COUNT 6 // VCP, USART1, USART3, USART6, SOFTSERIAL1, SOFTSERIAL2 + +#else // One softserial on versions other than OMNIBUSF4V3 +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_RX_PIN PC8 // pad labelled CH5 on OMNIBUSF4PRO +#define SOFTSERIAL_1_TX_PIN PC9 // pad labelled CH6 on OMNIBUSF4PRO + +#define SERIAL_PORT_COUNT 5 // VCP, USART1, USART3, USART6, SOFTSERIAL1 #endif #define USE_SPI #define USE_SPI_DEVICE_1 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) +#if !defined(DYSF4PRO) && !defined(DYSF4PROV2) #define USE_SPI_DEVICE_2 - #define SPI2_CLOCK_LEADING_EDGE #define SPI2_NSS_PIN PB12 #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 @@ -188,7 +210,7 @@ #endif #define USE_SPI_DEVICE_3 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) +#if !defined(DYSF4PRO) && !defined(DYSF4PROV2) #define SPI3_NSS_PIN PA15 #else #define SPI3_NSS_PIN PB3 @@ -202,7 +224,13 @@ #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) +#if defined(DYSF4PRO) || defined(DYSF4PROV2) + #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + #define M25P16_CS_PIN SPI3_NSS_PIN + #define M25P16_SPI_BUS BUS_SPI3 + #define USE_FLASHFS + #define USE_FLASH_M25P16 +#else #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define USE_SDCARD #define USE_SDCARD_SPI @@ -212,12 +240,6 @@ #define SDCARD_DETECT_PIN PB7 #define SDCARD_DETECT_INVERTED -#else - #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT - #define M25P16_CS_PIN SPI3_NSS_PIN - #define M25P16_SPI_BUS BUS_SPI3 - #define USE_FLASHFS - #define USE_FLASH_M25P16 #endif #define USE_ADC diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk index 1dbfc623fac..fa363a0f651 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH +FEATURES += VCP ONBOARDFLASH SDCARD TARGET_SRC = \ drivers/accgyro/accgyro_mpu6000.c \ diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index 9492dd2bd6b..c3a72620783 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -136,8 +136,6 @@ #define SPI4_SCK_PIN PE2 #define SPI4_MISO_PIN PE5 #define SPI4_MOSI_PIN PE6 -#define SPI4_CLOCK_LEADING_EDGE - #define USE_OSD #define USE_MAX7456 @@ -211,4 +209,4 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff -#define PCA9685_I2C_BUS BUS_I2C2 +#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file diff --git a/src/main/target/OMNIBUSF7NXT/target.h b/src/main/target/OMNIBUSF7NXT/target.h index fab05054a83..83577dcda52 100644 --- a/src/main/target/OMNIBUSF7NXT/target.h +++ b/src/main/target/OMNIBUSF7NXT/target.h @@ -174,6 +174,7 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_DSHOT +#define USE_SERIALSHOT // Number of available PWM outputs #define MAX_PWM_OUTPUT_PORTS 6 diff --git a/src/main/target/RADIX/target.h b/src/main/target/RADIX/target.h index 15f9cfe5228..fbf2339a888 100644 --- a/src/main/target/RADIX/target.h +++ b/src/main/target/RADIX/target.h @@ -56,12 +56,6 @@ //#define USE_PITOT_MS4525 //#define PITOT_I2C_BUS BUS_I2C2 -// #define USE_OPTICAL_FLOW -// #define USE_OPFLOW_CXOF - -// #define USE_RANGEFINDER -// #define RANGEFINDER_I2C_BUS BUS_I2C2 - #define USE_VCP #define VBUS_SENSING_PIN PA9 #define VBUS_SENSING_ENABLED diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index c1fb22d5d29..43eaa95b27d 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -33,6 +33,7 @@ #define BEEPER PB4 #define USE_DSHOT +#define USE_SERIALSHOT // MPU6000 interrupts #define USE_EXTI @@ -70,8 +71,6 @@ #define PITOT_I2C_BUS BUS_I2C2 -#define USE_OPTICAL_FLOW - #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C2 diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 8ed846eb7f4..adff656ca7b 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -126,9 +126,6 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C2 -#define USE_OPTICAL_FLOW -#define USE_OPFLOW_CXOF - #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 0259638054c..7a4f57490a4 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -109,7 +109,6 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define SPI2_CLOCK_LEADING_EDGE #define USE_SDCARD #define USE_SDCARD_SPI diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 6cd241afd9d..758bd30577f 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -101,7 +101,6 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define SPI2_CLOCK_LEADING_EDGE #define USE_SDCARD #define USE_SDCARD_SPI diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index 6df547c8f02..465a829b4f6 100755 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -101,7 +101,6 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define SPI2_CLOCK_LEADING_EDGE #define SPI3_NSS_PIN PA15 #define SPI3_SCK_PIN PB3 @@ -180,3 +179,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) + +#undef USE_TELEMETRY_FRSKY diff --git a/src/main/target/SPRACINGF3NEO/target.mk b/src/main/target/SPRACINGF3NEO/target.mk_ similarity index 100% rename from src/main/target/SPRACINGF3NEO/target.mk rename to src/main/target/SPRACINGF3NEO/target.mk_ diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index f48b9a24a48..b3a9b6c2a54 100755 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -120,7 +120,6 @@ #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -#define SPI2_CLOCK_LEADING_EDGE #define SPI3_NSS_PIN PA15 // NC #define SPI3_SCK_PIN PB3 // NC diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h index d174add3097..14b5d8726a6 100644 --- a/src/main/target/SPRACINGF7DUAL/target.h +++ b/src/main/target/SPRACINGF7DUAL/target.h @@ -153,7 +153,6 @@ #define SPI3_SCK_PIN PB3 #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define SPI3_CLOCK_LEADING_EDGE #define SDCARD_SPI_BUS BUS_SPI3 #define SDCARD_CS_PIN PC3 diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c deleted file mode 100644 index 66450827ff9..00000000000 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ /dev/null @@ -1,42 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include - -#include -#include "drivers/io.h" -#include "drivers/pwm_mapping.h" -#include "drivers/timer.h" - -const timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM | TIM_USE_PPM, 0), - DEF_TIM(TIM16, CH1, PB8, TIM_USE_PWM | TIM_USE_LED, 0), - DEF_TIM(TIM17, CH1, PB9, TIM_USE_PWM, 0), - DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0), - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0), - DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0), - DEF_TIM(TIM3, CH2, PA4, TIM_USE_PWM, 0), - DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), - DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), - DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), - DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), -}; - -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h deleted file mode 100644 index c0d97b45006..00000000000 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ /dev/null @@ -1,160 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "SDF3" // STM Discovery F3 - -#define LED0 PE8 // Blue LEDs - PE8/PE12 -#define LED0_INVERTED -#define LED1 PE10 // Orange LEDs - PE10/PE14 -#define LED1_INVERTED - -#define BEEPER PE9 // Red LEDs - PE9/PE13 -#define BEEPER_INVERTED - -#define USE_SPI -#define USE_SPI_DEVICE_1 -#define USE_SPI_DEVICE_2 - -#define SPI2_NSS_PIN PB12 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -#define USE_GYRO -#define USE_FAKE_GYRO -#define USE_GYRO_L3GD20 -#define L3GD20_SPI_BUS BUS_SPI1 -#define L3GD20_CS_PIN PE3 -#define GYRO_L3GD20_ALIGN CW270_DEG -#define USE_GYRO_L3G4200D -#define USE_GYRO_MPU3050 -#define USE_GYRO_MPU6050 -#define MPU6050_I2C_BUS BUS_I2C1 - -#define USE_GYRO_MPU6000 -#define MPU6000_CS_PIN SPI2_NSS_PIN -#define MPU6000_SPI_BUS BUS_SPI2 - -#define USE_GYRO_MPU6500 -#define MPU6500_CS_PIN SPI2_NSS_PIN -#define MPU6500_SPI_BUS BUS_SPI2 -#define GYRO_MPU6500_ALIGN CW270_DEG_FLIP - -#define USE_GYRO_MPU9250 -#define MPU9250_CS_PIN SPI2_NSS_PIN -#define MPU9250_SPI_BUS BUS_SPI2 -#define GYRO_MPU9250_ALIGN CW270_DEG_FLIP - -#define USE_ACC -#define USE_FAKE_ACC -#define USE_ACC_ADXL345 -#define USE_ACC_BMA280 -#define USE_ACC_MMA8452 -#define USE_ACC_MPU6050 -#define USE_ACC_LSM303DLHC -#define LSM303DLHC_I2C_BUS BUS_I2C1 -#define USE_ACC_MPU6000 -#define USE_ACC_MPU6500 -#define USE_ACC_MPU9250 -#define ACC_MPU6500_ALIGN CW270_DEG_FLIP - -#define USE_BARO -#define BARO_I2C_BUS BUS_I2C1 -#define USE_FAKE_BARO -#define USE_BARO_BMP085 -#define USE_BARO_BMP280 -#define USE_BARO_MS5611 - -#define USE_MAG -#define MAG_I2C_BUS BUS_I2C1 -#define USE_FAKE_MAG -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_MPU9250 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 - -#define USE_PITOT_MS4525 -#define PITOT_I2C_BUS BUS_I2C1 - -#define USE_VCP -#define USE_UART1 -#define USE_UART2 -#define USE_UART3 -#define USE_UART4 -#define USE_UART5 -#define SERIAL_PORT_COUNT 6 - -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) - -#define USE_RX_NRF24 -#define NRF24_SPI_INSTANCE SPI2 -#define USE_RX_CX10 -#define USE_RX_H8_3D -#define USE_RX_INAV -#define USE_RX_SYMA -#define USE_RX_V202 -#define RX_DEFAULT_PROTOCOL NRF24RX_V202_1M -#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define RX_NSS_PIN PA0 -#define RX_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define RX_CE_PIN PA1 - -#define USE_I2C -#define USE_I2C_DEVICE_1 - -#define USE_ADC -#define ADC_INSTANCE ADC1 -#define ADC_CHANNEL_1_PIN PC0 -#define ADC_CHANNEL_2_PIN PC1 -#define ADC_CHANNEL_3_PIN PC2 -#define ADC_CHANNEL_4_PIN PC3 -#define VBAT_ADC_CHANNEL ADC_CHN_1 -#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 -#define RSSI_ADC_CHANNEL ADC_CHN_3 - -#define USE_LED_STRIP -#define WS2811_PIN PB8 // TIM16_CH1 - -#define USE_SPEKTRUM_BIND -#define BIND_PIN PA3 // USART2, PA3 - -#define USE_RANGEFINDER -#define RANGEFINDER_I2C_BUS BUS_I2C1 -#define USE_RANGEFINDER_HCSR04 -#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0 -#define RANGEFINDER_HCSR04_ECHO_PIN PB1 -#define USE_RANGEFINDER_SRF10 - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - -// Number of available PWM outputs -#define MAX_PWM_OUTPUT_PORTS 12 - -// IO - 303 in 100pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTE 0xffff -#define TARGET_IO_PORTF 0x00ff diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk deleted file mode 100644 index ebb458b8383..00000000000 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ /dev/null @@ -1,33 +0,0 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD - -TARGET_SRC = \ - drivers/accgyro/accgyro_adxl345.c \ - drivers/accgyro/accgyro_bma280.c \ - drivers/accgyro/accgyro_fake.c \ - drivers/accgyro/accgyro_l3gd20.c \ - drivers/accgyro/accgyro_l3g4200d.c \ - drivers/accgyro/accgyro_lsm303dlhc.c \ - drivers/accgyro/accgyro_mma845x.c \ - drivers/accgyro/accgyro_mpu.c \ - drivers/accgyro/accgyro_mpu3050.c \ - drivers/accgyro/accgyro_mpu6000.c \ - drivers/accgyro/accgyro_mpu6050.c \ - drivers/accgyro/accgyro_mpu6500.c \ - drivers/accgyro/accgyro_mpu9250.c \ - drivers/barometer/barometer_bmp085.c \ - drivers/barometer/barometer_bmp280.c \ - drivers/barometer/barometer_fake.c \ - drivers/barometer/barometer_ms56xx.c \ - drivers/compass/compass_ak8963.c \ - drivers/compass/compass_ak8975.c \ - drivers/compass/compass_mpu9250.c \ - drivers/compass/compass_hmc5883l.c \ - drivers/compass/compass_fake.c \ - drivers/compass/compass_mag3110.c \ - drivers/compass/compass_qmc5883l.c \ - drivers/compass/compass_ist8310.c \ - drivers/compass/compass_ist8308.c \ - drivers/flash_m25p16.c \ - drivers/light_ws2811strip.c \ - diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 45202fd71d0..edfe12c243f 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -139,7 +139,6 @@ #define SPI3_SCK_PIN PC10 #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 -#define SPI3_CLOCK_LEADING_EDGE // ADC inputs #define BOARD_HAS_VOLTAGE_DIVIDER diff --git a/src/main/target/YUPIF7/README.md b/src/main/target/YUPIF7/README.md index 26ed01f66ad..85260ffcc5b 100644 --- a/src/main/target/YUPIF7/README.md +++ b/src/main/target/YUPIF7/README.md @@ -34,9 +34,12 @@ The YuPiF7 is a 36x36mm (30.5x30.5 mounting holes) board with an F7 microcontrol ## Manufacturers and Distributors -This board is available in the shop FR +This board is available in the shop -Website : https://www.yupif7.com +Website : https://www.studiosport.fr +Website : https://www.drone-fpv-racer.com +Website : https://www.fpv4drone.com +Website : https://www.yupifc.com ## Hardware Designs (if available) diff --git a/src/main/target/YUPIF7/target.h b/src/main/target/YUPIF7/target.h index 5b7516522a2..1df0e181b1e 100644 --- a/src/main/target/YUPIF7/target.h +++ b/src/main/target/YUPIF7/target.h @@ -95,6 +95,14 @@ #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 +#define UG2864_I2C_BUS BUS_I2C1 +#define USE_DASHBOARD +#define USE_OLED_UG2864 +#define DASHBOARD_ARMED_BITMAP + +#define USE_PITOT +#define PITOT_I2C_BUS BUS_I2C1 + //SPI ports #define USE_SPI diff --git a/src/main/target/YUPIF7/target.mk b/src/main/target/YUPIF7/target.mk index daa1a78f0fa..1ad0bac85eb 100644 --- a/src/main/target/YUPIF7/target.mk +++ b/src/main/target/YUPIF7/target.mk @@ -11,5 +11,7 @@ TARGET_SRC = \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_mag3110.c \ drivers/max7456.c \ + drivers/display_ug2864hsweg01.c \ + drivers/pitotmeter_ms4525.c \ drivers/light_ws2811strip.c \ drivers/serial_softserial.c diff --git a/src/main/target/common.h b/src/main/target/common.h index 047cf767275..6b1a29ec005 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -27,7 +27,6 @@ #define I2C2_OVERCLOCK false #define USE_I2C_PULLUP // Enable built-in pullups on all boards in case external ones are too week -#define USE_RX_PWM #define USE_RX_PPM #define USE_SERIAL_RX #define USE_SERIALRX_SPEKTRUM // Cheap and fairly common protocol @@ -68,12 +67,14 @@ #define USE_EXTENDED_CMS_MENUS #define USE_UAV_INTERCONNECT #define USE_RX_UIB +#define USE_HOTT_TEXTMODE // Allow default rangefinders #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP #define USE_RANGEFINDER_BENEWAKE #define USE_RANGEFINDER_VL53L0X +#define USE_RANGEFINDER_HCSR04_I2C // Allow default optic flow boards #define USE_OPFLOW @@ -97,14 +98,23 @@ #define USE_PWM_DRIVER_PCA9685 -#define USE_BOOTLOG -#define BOOTLOG_DESCRIPTIONS +#define USE_TELEMETRY_SIM + +#define NAV_NON_VOLATILE_WAYPOINT_CLI + +#define NAV_AUTO_MAG_DECLINATION_PRECISE + +#define USE_D_BOOST + + +#else // FLASH_SIZE < 256 +#define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR #endif #if (FLASH_SIZE > 128) #define NAV_FIXED_WING_LANDING #define USE_AUTOTUNE_FIXED_WING -#define USE_DEBUG_TRACE +#define USE_LOG #define USE_STATS #define USE_GYRO_NOTCH_1 #define USE_GYRO_NOTCH_2 @@ -115,7 +125,6 @@ #define USE_GPS_PROTO_NMEA #define USE_GPS_PROTO_NAZA #define USE_GPS_PROTO_MTK -#define NAV_AUTO_MAG_DECLINATION #define NAV_GPS_GLITCH_DETECTION #define NAV_NON_VOLATILE_WAYPOINT_STORAGE #define USE_TELEMETRY_HOTT @@ -126,6 +135,7 @@ #define USE_MSP_OVER_TELEMETRY // These are rather exotic serial protocols #define USE_RX_MSP +//#define USE_MSP_RC_OVERRIDE #define USE_SERIALRX_SUMD #define USE_SERIALRX_SUMH #define USE_SERIALRX_XBUS @@ -134,10 +144,10 @@ #define USE_PWM_SERVO_DRIVER #define USE_SERIAL_PASSTHROUGH #define NAV_MAX_WAYPOINTS 60 -#define MAX_BOOTLOG_ENTRIES 64 #define USE_RCDEVICE #define USE_PITOT #define USE_PITOT_ADC +#define USE_PITOT_VIRTUAL //Enable VTX control #define USE_VTX_CONTROL @@ -145,6 +155,10 @@ #define USE_VTX_TRAMP #define USE_VTX_FFPV +#ifndef STM32F3 //F3 series does not have enoug RAM to support logic conditions +#define USE_LOGIC_CONDITIONS +#endif + //Enable DST calculations #define RTC_AUTOMATIC_DST // Wind estimator @@ -155,3 +169,7 @@ #define SKIP_TASK_STATISTICS #endif + +#ifdef STM32F7 +#define USE_ITCM_RAM +#endif diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index b338074bf85..4aa252c5e54 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -175,7 +175,8 @@ #if !defined(IST8310_I2C_BUS) #define IST8310_I2C_BUS MAG_I2C_BUS #endif - BUSDEV_REGISTER_I2C(busdev_ist8310, DEVHW_IST8310, IST8310_I2C_BUS, 0x0E, NONE, DEVFLAGS_NONE); + BUSDEV_REGISTER_I2C(busdev_ist8310_0, DEVHW_IST8310_0, IST8310_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE); + BUSDEV_REGISTER_I2C(busdev_ist8310_1, DEVHW_IST8310_1, IST8310_I2C_BUS, 0x0E, NONE, DEVFLAGS_NONE); #endif #if defined(USE_MAG_IST8308) @@ -231,7 +232,7 @@ #endif #endif -#if defined(USE_RANGEFINDER_HCSR04_I2C) +#if defined(USE_RANGEFINDER_HCSR04_I2C) && (defined(HCSR04_I2C_BUS) || defined(RANGEFINDER_I2C_BUS)) #if !defined(HCSR04_I2C_BUS) #define HCSR04_I2C_BUS RANGEFINDER_I2C_BUS #endif diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 6cef361b998..7f0b645222a 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -35,3 +35,19 @@ #if !defined(USE_MSP_DISPLAYPORT) && (FLASH_SIZE > 128) && !defined(USE_OSD) #define USE_MSP_DISPLAYPORT #endif + +#ifdef USE_ITCM_RAM +#define FAST_CODE __attribute__((section(".tcm_code"))) +#define NOINLINE __NOINLINE +#else +#define FAST_CODE +#define NOINLINE +#endif + +#ifdef STM32F3 +#undef USE_WIND_ESTIMATOR +#undef USE_SERIALRX_SUMD +#undef USE_SERIALRX_SUMH +#undef USE_SERIALRX_XBUS +#undef USE_SERIALRX_JETIEXBUS +#endif \ No newline at end of file diff --git a/src/main/target/link/stm32_flash_f722.ld b/src/main/target/link/stm32_flash_f722.ld index 5253d594428..296708678dc 100644 --- a/src/main/target/link/stm32_flash_f722.ld +++ b/src/main/target/link/stm32_flash_f722.ld @@ -26,17 +26,23 @@ ENTRY(Reset_Handler) /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K - FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K - FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 480K + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 16K - TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K - RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 192K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 16K + /* config occupies the entire flash sector 1 for the ease of erasure, 16K on F72x */ + ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00204000, LENGTH = 16K + ITCM_FLASH1 (rx) : ORIGIN = 0x00208000, LENGTH = 480K + + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K + FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K + FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 480K + + TCM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 192K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } -/* note TCM could be used for stack */ REGION_ALIAS("STACKRAM", TCM) REGION_ALIAS("FASTRAM", TCM) -INCLUDE "stm32_flash_split.ld" +INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/link/stm32_flash_f745.ld b/src/main/target/link/stm32_flash_f745.ld index f9d8e26f9e6..a83374e4e41 100644 --- a/src/main/target/link/stm32_flash_f745.ld +++ b/src/main/target/link/stm32_flash_f745.ld @@ -17,6 +17,7 @@ _Min_Stack_Size = 0x1800; ENTRY(Reset_Handler) /* +0x00000000 to 0x00003FFF 16K ITCM RAM, 0x08000000 to 0x080FFFFF 1024K full flash, 0x08000000 to 0x08007FFF 32K isr vector, startup code, 0x08008000 to 0x0800FFFF 32K config, // FLASH_Sector_1 @@ -26,6 +27,12 @@ ENTRY(Reset_Handler) /* Specify the memory areas */ MEMORY { + ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K + ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 32K + /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */ + ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00208000, LENGTH = 32K + ITCM_FLASH1 (rx) : ORIGIN = 0x00210000, LENGTH = 960K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K FLASH_CONFIG (r) : ORIGIN = 0x08008000, LENGTH = 32K FLASH1 (rx) : ORIGIN = 0x08010000, LENGTH = 960K @@ -38,4 +45,4 @@ MEMORY REGION_ALIAS("STACKRAM", TCM) REGION_ALIAS("FASTRAM", TCM) -INCLUDE "stm32_flash_split.ld" +INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/link/stm32_flash_f746.ld b/src/main/target/link/stm32_flash_f746.ld index 589745c5715..0c9041676f6 100644 --- a/src/main/target/link/stm32_flash_f746.ld +++ b/src/main/target/link/stm32_flash_f746.ld @@ -26,6 +26,12 @@ ENTRY(Reset_Handler) /* Specify the memory areas */ MEMORY { + ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K + ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 32K + /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */ + ITCM_FLASH_CONFIG (r) : ORIGIN = 0x00208000, LENGTH = 32K + ITCM_FLASH1 (rx) : ORIGIN = 0x00210000, LENGTH = 960K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K FLASH_CONFIG (r) : ORIGIN = 0x08008000, LENGTH = 32K FLASH1 (rx) : ORIGIN = 0x08010000, LENGTH = 960K @@ -38,4 +44,4 @@ MEMORY REGION_ALIAS("STACKRAM", TCM) REGION_ALIAS("FASTRAM", TCM) -INCLUDE "stm32_flash_split.ld" +INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/link/stm32_flash_f7_split.ld b/src/main/target/link/stm32_flash_f7_split.ld new file mode 100644 index 00000000000..42d8f0205ca --- /dev/null +++ b/src/main/target/link/stm32_flash_f7_split.ld @@ -0,0 +1,185 @@ +/* +***************************************************************************** +** +** File : stm32_flash_split.ld +** +** Abstract : Common linker script for STM32 devices. +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = ORIGIN(STACKRAM) + LENGTH(STACKRAM); /* end of RAM */ + +/* Base address where the config is stored. */ +__config_start = ORIGIN(FLASH_CONFIG); +__config_end = ORIGIN(FLASH_CONFIG) + LENGTH(FLASH_CONFIG); + +/* Define output sections */ +SECTIONS +{ + /* The startup code goes first into FLASH */ + .isr_vector : + { + . = ALIGN(4); + PROVIDE (isr_vector_table_base = .); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH1 + + tcm_code = LOADADDR(.tcm_code); + .tcm_code (NOLOAD) : + { + . = ALIGN(4); + tcm_code_start = .; + *(.tcm_code) + *(.tcm_code*) + . = ALIGN(4); + tcm_code_end = .; + } >ITCM_RAM AT >FLASH1 + + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH + .ARM : { + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + } >FLASH + + .busdev_registry : + { + PROVIDE_HIDDEN (__busdev_registry_start = .); + KEEP (*(.busdev_registry)) + KEEP (*(SORT(.busdev_registry.*))) + PROVIDE_HIDDEN (__busdev_registry_end = .); + } >FLASH + + .preinit_array : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >FLASH1 + .init_array : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + } >FLASH1 + .fini_array : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(.fini_array*)) + KEEP (*(SORT(.fini_array.*))) + PROVIDE_HIDDEN (__fini_array_end = .); + } >FLASH1 + .pg_registry : + { + PROVIDE_HIDDEN (__pg_registry_start = .); + KEEP (*(.pg_registry)) + KEEP (*(SORT(.pg_registry.*))) + PROVIDE_HIDDEN (__pg_registry_end = .); + } >FLASH1 + .pg_resetdata : + { + PROVIDE_HIDDEN (__pg_resetdata_start = .); + KEEP (*(.pg_resetdata)) + PROVIDE_HIDDEN (__pg_resetdata_end = .); + } >FLASH1 + + /* used by the startup to initialize data */ + _sidata = .; + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >RAM AT> FLASH1 + + /* Uninitialized data section */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(SORT_BY_ALIGNMENT(.bss*)) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + .fastram_bss (NOLOAD) : + { + __fastram_bss_start__ = .; + *(.fastram_bss) + *(SORT_BY_ALIGNMENT(.fastram_bss*)) + . = ALIGN(4); + __fastram_bss_end__ = .; + } >FASTRAM + + /* User_heap_stack section, used to check that there is enough RAM left */ + _heap_stack_end = ORIGIN(STACKRAM)+LENGTH(STACKRAM) - 8; /* 8 bytes to allow for alignment */ + _heap_stack_begin = _heap_stack_end - _Min_Stack_Size - _Min_Heap_Size; + . = _heap_stack_begin; + ._user_heap_stack : + { + . = ALIGN(4); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(4); + } >STACKRAM = 0xa5 + + /* MEMORY_bank1 section, code must be located here explicitly */ + /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ + .memory_b1_text : + { + *(.mb1text) /* .mb1text sections (code) */ + *(.mb1text*) /* .mb1text* sections (code) */ + *(.mb1rodata) /* read-only data (constants) */ + *(.mb1rodata*) + } >MEMORY_B1 + + /* Remove information from the standard libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/src/main/target/link/stm32_flash_split.ld b/src/main/target/link/stm32_flash_split.ld index 35de5ba017d..d0f746d1da3 100644 --- a/src/main/target/link/stm32_flash_split.ld +++ b/src/main/target/link/stm32_flash_split.ld @@ -49,7 +49,6 @@ SECTIONS _etext = .; /* define a global symbols at end of code */ } >FLASH1 - .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH .ARM : { __exidx_start = .; diff --git a/src/main/target/stm32f7xx_hal_conf.h b/src/main/target/stm32f7xx_hal_conf.h index 26afa82fa6e..ebd30750b47 100644 --- a/src/main/target/stm32f7xx_hal_conf.h +++ b/src/main/target/stm32f7xx_hal_conf.h @@ -158,8 +158,10 @@ #define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */ #define TICK_INT_PRIORITY ((uint32_t)0U) /*!< tick interrupt priority */ #define USE_RTOS 0U -#define PREFETCH_ENABLE 0U -#define ART_ACCLERATOR_ENABLE 0U /* To enable instruction cache and prefetch */ +#define PREFETCH_ENABLE 1U +#define ART_ACCLERATOR_ENABLE 1U /* To enable instruction cache and prefetch */ +#define INSTRUCTION_CACHE_ENABLE 1U +#define DATA_CACHE_ENABLE 1U /* ########################## Assert Selection ############################## */ /** diff --git a/src/main/target/system_stm32f7xx.c b/src/main/target/system_stm32f7xx.c index e0b094bd4b3..2ed92c6bbb2 100644 --- a/src/main/target/system_stm32f7xx.c +++ b/src/main/target/system_stm32f7xx.c @@ -63,6 +63,7 @@ * @{ */ +#include #include "stm32f7xx.h" #if !defined (HSE_VALUE) @@ -240,6 +241,15 @@ * @{ */ +void CopyFastCode(void) +{ + /* Load functions into ITCM RAM */ + extern uint8_t tcm_code_start; + extern uint8_t tcm_code_end; + extern uint8_t tcm_code; + memcpy(&tcm_code_start, &tcm_code, (size_t) (&tcm_code_end - &tcm_code_start)); +} + /** * @brief Setup the microcontroller system * Initialize the Embedded Flash Interface, the PLL and update the diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index f4188e19a7f..449e92f625a 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -56,6 +56,7 @@ #include "rx/rx.h" #include "sensors/battery.h" +#include "sensors/sensors.h" #include "telemetry/crsf.h" #include "telemetry/telemetry.h" @@ -204,7 +205,7 @@ uint16_t GPS heading ( degree / 100 ) uint16 Altitude ( meter ­1000m offset ) uint8_t Satellites in use ( counter ) */ -void crsfFrameGps(sbuf_t *dst) +static void crsfFrameGps(sbuf_t *dst) { // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); @@ -218,6 +219,19 @@ void crsfFrameGps(sbuf_t *dst) crsfSerialize8(dst, gpsSol.numSat); } +/* +0x07 Vario sensor +Payload: +int16 Vertical speed ( cm/s ) +*/ +static void crsfFrameVarioSensor(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_VARIO_SENSOR); + crsfSerialize16(dst, lrintf(getEstimatedActualVelocity(Z))); +} + /* 0x08 Battery sensor Payload: @@ -226,7 +240,7 @@ uint16_t Current ( mA * 100 ) uint24_t Capacity ( mAh ) uint8_t Battery remaining ( percent ) */ -void crsfFrameBatterySensor(sbuf_t *dst) +static void crsfFrameBatterySensor(sbuf_t *dst) { // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); @@ -271,7 +285,7 @@ int16_t Yaw angle ( rad / 10000 ) #define DECIDEGREES_TO_RADIANS10000(angle) ((int16_t)(1000.0f * (angle) * RAD)) -void crsfFrameAttitude(sbuf_t *dst) +static void crsfFrameAttitude(sbuf_t *dst) { sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_ATTITUDE); @@ -285,7 +299,7 @@ void crsfFrameAttitude(sbuf_t *dst) Payload: char[] Flight mode ( Null­terminated string ) */ -void crsfFrameFlightMode(sbuf_t *dst) +static void crsfFrameFlightMode(sbuf_t *dst) { // just do "OK" for the moment as a placeholder // write zero for frame length, since we don't know it yet @@ -296,7 +310,7 @@ void crsfFrameFlightMode(sbuf_t *dst) // use same logic as OSD, so telemetry displays same flight text as OSD when armed const char *flightMode = "OK"; if (ARMING_FLAG(ARMED)) { - if (isAirmodeActive()) { + if (STATE(AIRMODE_ACTIVE)) { flightMode = "AIR"; } else { flightMode = "ACRO"; @@ -350,8 +364,8 @@ uint32_t Null Bytes uint8_t 255 (Max MSP Parameter) uint8_t 0x01 (Parameter version 1) */ -void crsfFrameDeviceInfo(sbuf_t *dst) { - +static void crsfFrameDeviceInfo(sbuf_t *dst) +{ char buff[30]; tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, TARGET_BOARD_IDENTIFIER); @@ -379,6 +393,7 @@ typedef enum { CRSF_FRAME_BATTERY_SENSOR_INDEX, CRSF_FRAME_FLIGHT_MODE_INDEX, CRSF_FRAME_GPS_INDEX, + CRSF_FRAME_VARIO_SENSOR_INDEX, CRSF_SCHEDULE_COUNT_MAX } crsfFrameTypeIndex_e; @@ -438,6 +453,13 @@ static void processCrsf(void) crsfFrameGps(dst); crsfFinalize(dst); } +#endif +#if defined(USE_BARO) || defined(USE_GPS) + if (currentSchedule & BV(CRSF_FRAME_VARIO_SENSOR_INDEX)) { + crsfInitializeFrame(dst); + crsfFrameVarioSensor(dst); + crsfFinalize(dst); + } #endif crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } @@ -462,12 +484,18 @@ void initCrsfTelemetry(void) crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX); crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX); +#ifdef USE_GPS if (feature(FEATURE_GPS)) { crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX); } +#endif +#if defined(USE_BARO) || defined(USE_GPS) + if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) { + crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX); + } +#endif crsfScheduleCount = (uint8_t)index; - - } +} bool checkCrsfTelemetryState(void) { @@ -489,7 +517,7 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs) // in between the RX frames. crsfRxSendTelemetryData(); - // Send ad-hoc response frames as soon as possible + // 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); @@ -510,7 +538,7 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs) } // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz - // Spread out scheduled frames evenly so each frame is sent at the same frequency. + // Spread out scheduled frames evenly so each frame is sent at the same frequency. if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) { crsfLastCycleTime = currentTimeUs; processCrsf(); @@ -539,6 +567,9 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) crsfFrameGps(sbuf); break; #endif + case CRSF_FRAMETYPE_VARIO_SENSOR: + crsfFrameVarioSensor(sbuf); + break; } const int frameSize = crsfFinalizeBuf(sbuf, frame); return frameSize; diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index b4e2ca5bc63..d9e5d99babe 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -24,6 +24,7 @@ * Carsten Giesen - cGiesen - Baseflight port * Oliver Bayer - oBayer - MultiWii-HoTT, HoTT reverse engineering * Adam Majerczyk - HoTT-for-ardupilot from which some information and ideas are borrowed. + * Scavanger & Ziege-One: CMS Textmode addon * * https://github.com/obayer/MultiWii-HoTT * https://github.com/oBayer/MultiHoTT-Module @@ -86,6 +87,15 @@ #include "telemetry/hott.h" #include "telemetry/telemetry.h" +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) +#include "scheduler/scheduler.h" +#include "io/displayport_hott.h" + +#define HOTT_TEXTMODE_TASK_PERIOD 1000 +#define HOTT_TEXTMODE_RX_SCHEDULE 5000 +#define HOTT_TEXTMODE_TX_DELAY_US 1000 +#endif + //#define HOTT_DEBUG typedef enum { @@ -102,6 +112,9 @@ typedef enum { #define HOTT_TX_DELAY_US 2000 #define MILLISECONDS_IN_A_SECOND 1000 +static uint32_t rxSchedule = HOTT_RX_SCHEDULE; +static uint32_t txDelayUs = HOTT_TX_DELAY_US; + static hottState_e hottState = HOTT_WAITING_FOR_REQUEST; static timeUs_t hottStateChangeUs = 0; @@ -121,6 +134,20 @@ static portSharing_e hottPortSharing; static HOTT_GPS_MSG_t hottGPSMessage; static HOTT_EAM_MSG_t hottEAMMessage; +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) +static hottTextModeMsg_t hottTextModeMessage; +static bool textmodeIsAlive = false; +static int32_t telemetryTaskPeriod = 0; + +static void initialiseTextmodeMessage(hottTextModeMsg_t *msg) +{ + msg->start = HOTT_TEXTMODE_START; + msg->esc = HOTT_EAM_SENSOR_TEXT_ID; + msg->warning = 0; + msg->stop = HOTT_TEXTMODE_STOP; +} +#endif + static void hottSwitchState(hottState_e newState, timeUs_t currentTimeUs) { if (hottState != newState) { @@ -162,6 +189,9 @@ static void initialiseMessages(void) #ifdef USE_GPS initialiseGPSMessage(&hottGPSMessage, sizeof(hottGPSMessage)); #endif +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) + initialiseTextmodeMessage(&hottTextModeMessage); +#endif } #ifdef USE_GPS @@ -321,6 +351,14 @@ void initHoTTTelemetry(void) portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_HOTT); hottPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_HOTT); + if (!portConfig) { + return; + } + +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) + hottDisplayportRegister(); +#endif + initialiseMessages(); } @@ -330,7 +368,9 @@ void configureHoTTTelemetryPort(void) return; } - hottPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_HOTT, NULL, NULL, HOTT_BAUDRATE, HOTT_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); + portOptions_t portOptions = (telemetryConfig()->uartUnidirectional ? SERIAL_UNIDIR : SERIAL_BIDIR) | (SERIAL_NOT_INVERTED); + + hottPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_HOTT, NULL, NULL, HOTT_BAUDRATE, HOTT_INITIAL_PORT_MODE, portOptions); if (!hottPort) { return; @@ -345,8 +385,94 @@ static void hottQueueSendResponse(uint8_t *buffer, int length) hottTxMsgSize = length; } +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) +static void hottTextmodeStart(void) +{ + // Increase menu speed + cfTaskInfo_t taskInfo; + getTaskInfo(TASK_TELEMETRY, &taskInfo); + telemetryTaskPeriod = taskInfo.desiredPeriod; + rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(HOTT_TEXTMODE_TASK_PERIOD)); + + rxSchedule = HOTT_TEXTMODE_RX_SCHEDULE; + txDelayUs = HOTT_TEXTMODE_TX_DELAY_US; +} + +static void hottTextmodeStop(void) +{ + // Set back to avoid slow down of the FC + if (telemetryTaskPeriod > 0) { + rescheduleTask(TASK_TELEMETRY, telemetryTaskPeriod); + telemetryTaskPeriod = 0; + } + + rxSchedule = HOTT_RX_SCHEDULE; + txDelayUs = HOTT_TX_DELAY_US; +} + +bool hottTextmodeIsAlive(void) +{ + return textmodeIsAlive; +} + +void hottTextmodeGrab(void) +{ + hottTextModeMessage.esc = HOTT_EAM_SENSOR_TEXT_ID; +} + +void hottTextmodeExit(void) +{ + hottTextModeMessage.esc = HOTT_TEXTMODE_ESC; +} + +void hottTextmodeWriteChar(uint8_t column, uint8_t row, char c) +{ + if (column < HOTT_TEXTMODE_DISPLAY_COLUMNS && row < HOTT_TEXTMODE_DISPLAY_ROWS) { + if (hottTextModeMessage.txt[row][column] != c) + hottTextModeMessage.txt[row][column] = c; + } +} + +static bool processHottTextModeRequest(const uint8_t cmd) +{ + static bool setEscBack = false; + + if (!textmodeIsAlive) { + hottTextmodeStart(); + textmodeIsAlive = true; + } + + if ((cmd & 0xF0) != HOTT_EAM_SENSOR_TEXT_ID) { + return false; + } + + if (setEscBack) { + hottTextModeMessage.esc = HOTT_EAM_SENSOR_TEXT_ID; + setEscBack = false; + } + + if (hottTextModeMessage.esc != HOTT_TEXTMODE_ESC) { + hottCmsOpen(); + } else { + setEscBack = true; + } + + hottSetCmsKey(cmd & 0x0f, hottTextModeMessage.esc == HOTT_TEXTMODE_ESC); + hottQueueSendResponse((uint8_t *)&hottTextModeMessage, sizeof(hottTextModeMessage)); + + return true; +} +#endif + static bool processBinaryModeRequest(uint8_t address) { +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) + if (textmodeIsAlive) { + hottTextmodeStop(); + textmodeIsAlive = false; + } +#endif + switch (address) { #ifdef USE_GPS case 0x8A: @@ -434,7 +560,7 @@ void handleHoTTTelemetry(timeUs_t currentTimeUs) break; case HOTT_RECEIVING_REQUEST: - if ((currentTimeUs - hottStateChangeUs) >= HOTT_RX_SCHEDULE) { + if ((currentTimeUs - hottStateChangeUs) >= rxSchedule) { // Waiting for too long - resync flushHottRxBuffer(); hottSwitchState(HOTT_WAITING_FOR_REQUEST, currentTimeUs); @@ -461,9 +587,17 @@ void handleHoTTTelemetry(timeUs_t currentTimeUs) } } else if (hottRequestBuffer[0] == HOTT_TEXT_MODE_REQUEST_ID) { - // FIXME Text mode +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) + if (processHottTextModeRequest(hottRequestBuffer[1])) { + hottSwitchState(HOTT_WAITING_FOR_TX_WINDOW, currentTimeUs); + } + else { hottSwitchState(HOTT_WAITING_FOR_REQUEST, currentTimeUs); } +#else + hottSwitchState(HOTT_WAITING_FOR_REQUEST, currentTimeUs); +#endif + } else { // Received garbage - resync flushHottRxBuffer(); @@ -489,7 +623,7 @@ void handleHoTTTelemetry(timeUs_t currentTimeUs) break; case HOTT_ENDING_TRANSMISSION: - if ((currentTimeUs - hottStateChangeUs) >= HOTT_TX_DELAY_US) { + if ((currentTimeUs - hottStateChangeUs) >= txDelayUs) { flushHottRxBuffer(); hottSwitchState(HOTT_WAITING_FOR_REQUEST, currentTimeUs); reprocessState = true; diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h index 970ce93649b..c9eecedb7ab 100644 --- a/src/main/telemetry/hott.h +++ b/src/main/telemetry/hott.h @@ -21,6 +21,7 @@ * Carsten Giesen * Adam Majerczyk (majerczyk.adam@gmail.com) * Texmode add-on by Michi (mamaretti32@gmail.com) + * Scavanger & Ziege-One: CMS Textmode addon */ #pragma once @@ -30,12 +31,12 @@ #define HOTTV4_TEXT_MODE_REQUEST_ID 0x7f #define HOTTV4_BINARY_MODE_REQUEST_ID 0x80 -#define HOTTV4_BUTTON_DEC 0xEB -#define HOTTV4_BUTTON_INC 0xED -#define HOTTV4_BUTTON_SET 0xE9 -#define HOTTV4_BUTTON_NIL 0x0F -#define HOTTV4_BUTTON_NEXT 0xEE -#define HOTTV4_BUTTON_PREV 0xE7 +#define HOTTV4_BUTTON_DEC 0xB +#define HOTTV4_BUTTON_INC 0xD +#define HOTTV4_BUTTON_SET 0x9 +#define HOTTV4_BUTTON_NIL 0xF +#define HOTTV4_BUTTON_NEXT 0xE +#define HOTTV4_BUTTON_PREV 0x7 #define HOTT_EAM_OFFSET_HEIGHT 500 #define HOTT_EAM_OFFSET_M2S 72 @@ -101,17 +102,21 @@ typedef enum { #define HOTT_GPS_SENSOR_TEXT_ID 0xA0 // GPS Module ID -#define HOTT_TEXTMODE_MSG_TEXT_LEN 168 +#define HOTT_TEXTMODE_DISPLAY_ROWS 8 +#define HOTT_TEXTMODE_DISPLAY_COLUMNS 21 +#define HOTT_TEXTMODE_START 0x7B +#define HOTT_TEXTMODE_STOP 0x7D +#define HOTT_TEXTMODE_ESC 0x01 //Text mode msgs type -struct HOTT_TEXTMODE_MSG { - uint8_t start_byte; //#01 constant value 0x7b - uint8_t fill1; //#02 constant value 0x00 - uint8_t warning_beeps;//#03 1=A 2=B ... - uint8_t msg_txt[HOTT_TEXTMODE_MSG_TEXT_LEN]; //#04 ASCII text to display to +typedef struct hottTextModeMsg_s { + uint8_t start; //#01 constant value 0x7b + uint8_t esc; //#02 Low byte: Sensor ID or 0x01 for escape + uint8_t warning; //#03 1=A 2=B ... + uint8_t txt[HOTT_TEXTMODE_DISPLAY_ROWS][HOTT_TEXTMODE_DISPLAY_COLUMNS]; //#04 ASCII text to display to // Bit 7 = 1 -> Inverse character display // Display 21x8 - uint8_t stop_byte; //#171 constant value 0x7d -}; + uint8_t stop; //#171 constant value 0x7d +} hottTextModeMsg_t; typedef struct HOTT_GAM_MSG_s { uint8_t start_byte; //#01 start uint8_t constant value 0x7c @@ -492,6 +497,13 @@ void initHoTTTelemetry(void); void configureHoTTTelemetryPort(void); void freeHoTTTelemetryPort(void); +#if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS) +bool hottTextmodeIsAlive(void); +void hottTextmodeGrab(void); +void hottTextmodeExit(void); +void hottTextmodeWriteChar(uint8_t column, uint8_t row, char c); +#endif + uint32_t getHoTTTelemetryProviderBaudRate(void); void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage); diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 644ea8e46a9..0f5345cda3f 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -144,7 +144,7 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { 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)); + return sendIbusMeasurement2(address, (uint16_t)(temperature + 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 diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 9c75370d4bd..8a9a5c85902 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -158,28 +158,32 @@ void ltm_gframe(sbuf_t *dst) void ltm_sframe(sbuf_t *dst) { - uint8_t lt_flightmode; + ltm_modes_e lt_flightmode; if (FLIGHT_MODE(MANUAL_MODE)) - lt_flightmode = 0; + lt_flightmode = LTM_MODE_MANUAL; else if (FLIGHT_MODE(NAV_WP_MODE)) - lt_flightmode = 10; + lt_flightmode = LTM_MODE_WAYPOINTS; else if (FLIGHT_MODE(NAV_RTH_MODE)) - lt_flightmode = 13; + lt_flightmode = LTM_MODE_RTH; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) - lt_flightmode = 9; + lt_flightmode = LTM_MODE_GPSHOLD; else if (FLIGHT_MODE(NAV_CRUISE_MODE)) - lt_flightmode = 18; + lt_flightmode = LTM_MODE_CRUISE; + else if (FLIGHT_MODE(NAV_LAUNCH_MODE)) + lt_flightmode = LTM_MODE_LAUNCH; + else if (FLIGHT_MODE(AUTO_TUNE)) + lt_flightmode = LTM_MODE_AUTOTUNE; else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) - lt_flightmode = 8; + lt_flightmode = LTM_MODE_ALTHOLD; else if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(HEADING_MODE)) - lt_flightmode = 11; + lt_flightmode = LTM_MODE_HEADHOLD; else if (FLIGHT_MODE(ANGLE_MODE)) - lt_flightmode = 2; + lt_flightmode = LTM_MODE_ANGLE; else if (FLIGHT_MODE(HORIZON_MODE)) - lt_flightmode = 3; + lt_flightmode = LTM_MODE_HORIZON; else - lt_flightmode = 1; // Rate mode + lt_flightmode = LTM_MODE_RATE; // Rate mode uint8_t lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0; if (failsafeIsActive()) diff --git a/src/main/telemetry/ltm.h b/src/main/telemetry/ltm.h index f00d8d2e199..36bdee32aa3 100644 --- a/src/main/telemetry/ltm.h +++ b/src/main/telemetry/ltm.h @@ -45,6 +45,33 @@ typedef enum { #define LTM_MAX_PAYLOAD_SIZE 14 #define LTM_MAX_MESSAGE_SIZE (LTM_MAX_PAYLOAD_SIZE+4) + +typedef enum { + LTM_MODE_MANUAL = 0, + LTM_MODE_RATE, + LTM_MODE_ANGLE, + LTM_MODE_HORIZON, + LTM_MODE_ACRO, + LTM_MODE_STABALIZED1, + LTM_MODE_STABALIZED2, + LTM_MODE_STABILIZED3, + LTM_MODE_ALTHOLD, + LTM_MODE_GPSHOLD, + LTM_MODE_WAYPOINTS, + LTM_MODE_HEADHOLD, + LTM_MODE_CIRCLE, + LTM_MODE_RTH, + LTM_MODE_FOLLOWWME, + LTM_MODE_LAND, + LTM_MODE_FLYBYWIRE1, + LTM_MODE_FLYBYWIRE2, + LTM_MODE_CRUISE, + LTM_MODE_UNKNOWN, + // iNav specific extensions + LTM_MODE_LAUNCH, + LTM_MODE_AUTOTUNE +} ltm_modes_e; + void initLtmTelemetry(void); void handleLtmTelemetry(void); void checkLtmTelemetryState(void); @@ -53,4 +80,3 @@ void freeLtmTelemetryPort(void); void configureLtmTelemetryPort(void); int getLtmFrame(uint8_t *frame, ltm_frame_e ltmFrameType); - diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 17e23c8e0ae..3ab3a5dd39e 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -89,12 +89,12 @@ static bool mavlinkTelemetryEnabled = false; static portSharing_e mavlinkPortSharing; /* MAVLink datastream rates in Hz */ -static const uint8_t mavRates[] = { - [MAV_DATA_STREAM_EXTENDED_STATUS] = 2, //2Hz - [MAV_DATA_STREAM_RC_CHANNELS] = 5, //5Hz - [MAV_DATA_STREAM_POSITION] = 2, //2Hz - [MAV_DATA_STREAM_EXTRA1] = 10, //10Hz - [MAV_DATA_STREAM_EXTRA2] = 10 //2Hz +static uint8_t mavRates[] = { + [MAV_DATA_STREAM_EXTENDED_STATUS] = 2, // 2Hz + [MAV_DATA_STREAM_RC_CHANNELS] = 5, // 5Hz + [MAV_DATA_STREAM_POSITION] = 2, // 2Hz + [MAV_DATA_STREAM_EXTRA1] = 10, // 10Hz + [MAV_DATA_STREAM_EXTRA2] = 2 // 2Hz }; #define MAXSTREAMS (sizeof(mavRates) / sizeof(mavRates[0])) @@ -168,6 +168,15 @@ void configureMAVLinkTelemetryPort(void) mavlinkTelemetryEnabled = true; } +static void configureMAVLinkStreamRates(void) +{ + mavRates[MAV_DATA_STREAM_EXTENDED_STATUS] = telemetryConfig()->mavlink.extended_status_rate; + mavRates[MAV_DATA_STREAM_RC_CHANNELS] = telemetryConfig()->mavlink.rc_channels_rate; + mavRates[MAV_DATA_STREAM_POSITION] = telemetryConfig()->mavlink.position_rate; + mavRates[MAV_DATA_STREAM_EXTRA1] = telemetryConfig()->mavlink.extra1_rate; + mavRates[MAV_DATA_STREAM_EXTRA2] = telemetryConfig()->mavlink.extra2_rate; +} + void checkMAVLinkTelemetryState(void) { bool newTelemetryEnabledValue = telemetryDetermineEnabledState(mavlinkPortSharing); @@ -176,9 +185,10 @@ void checkMAVLinkTelemetryState(void) return; } - if (newTelemetryEnabledValue) + if (newTelemetryEnabledValue) { configureMAVLinkTelemetryPort(); - else + configureMAVLinkStreamRates(); + } else freeMAVLinkTelemetryPort(); } @@ -247,29 +257,31 @@ void mavlinkSendSystemStatus(void) void mavlinkSendRCChannelsAndRSSI(void) { +#define GET_CHANNEL_VALUE(x) ((rxRuntimeConfig.channelCount >= (x + 1)) ? rxGetChannelValue(x) : 0) mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg, // time_boot_ms Timestamp (milliseconds since system boot) millis(), // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. 0, // chan1_raw RC channel 1 value, in microseconds - (rxRuntimeConfig.channelCount >= 1) ? rcData[0] : 0, + GET_CHANNEL_VALUE(0), // chan2_raw RC channel 2 value, in microseconds - (rxRuntimeConfig.channelCount >= 2) ? rcData[1] : 0, + GET_CHANNEL_VALUE(1), // chan3_raw RC channel 3 value, in microseconds - (rxRuntimeConfig.channelCount >= 3) ? rcData[2] : 0, + GET_CHANNEL_VALUE(2), // chan4_raw RC channel 4 value, in microseconds - (rxRuntimeConfig.channelCount >= 4) ? rcData[3] : 0, + GET_CHANNEL_VALUE(3), // chan5_raw RC channel 5 value, in microseconds - (rxRuntimeConfig.channelCount >= 5) ? rcData[4] : 0, + GET_CHANNEL_VALUE(4), // chan6_raw RC channel 6 value, in microseconds - (rxRuntimeConfig.channelCount >= 6) ? rcData[5] : 0, + GET_CHANNEL_VALUE(5), // chan7_raw RC channel 7 value, in microseconds - (rxRuntimeConfig.channelCount >= 7) ? rcData[6] : 0, + GET_CHANNEL_VALUE(6), // chan8_raw RC channel 8 value, in microseconds - (rxRuntimeConfig.channelCount >= 8) ? rcData[7] : 0, + GET_CHANNEL_VALUE(7), // rssi Receive signal strength indicator, 0: 0%, 255: 100% scaleRange(getRSSI(), 0, 1023, 0, 255)); +#undef GET_CHANNEL_VALUE mavlinkSendMessage(); } @@ -413,7 +425,7 @@ void mavlinkSendHUDAndHeartbeat(void) // heading Current heading in degrees, in compass units (0..360, 0=north) DECIDEGREES_TO_DEGREES(attitude.values.yaw), // throttle Current throttle setting in integer percent, 0 to 100 - scaleRange(constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100), + scaleRange(constrain(rxGetChannelValue(THROTTLE), PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100), // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate) mavAltitude, // climb Current climb rate in meters/second diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c new file mode 100644 index 00000000000..36b67ab3601 --- /dev/null +++ b/src/main/telemetry/sim.c @@ -0,0 +1,523 @@ +/* + * 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 . + */ + +#include "platform.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SIM) + +#include + +#include "common/printf.h" +#include "common/olc.h" + +#include "drivers/time.h" + +#include "fc/fc_core.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" + +#include "io/gps.h" +#include "io/serial.h" + +#include "navigation/navigation.h" + +#include "sensors/battery.h" +#include "sensors/sensors.h" + +#include "common/string_light.h" +#include "common/typeconversion.h" + +#include "telemetry/sim.h" +#include "telemetry/telemetry.h" + + +#define SIM_AT_COMMAND_MAX_SIZE 255 +#define SIM_RESPONSE_BUFFER_SIZE 255 +#define SIM_CYCLE_MS 5000 // wait between sim command cycles +#define SIM_AT_COMMAND_DELAY_MS 3000 +#define SIM_AT_COMMAND_DELAY_MIN_MS 500 +#define SIM_STARTUP_DELAY_MS 10000 +#define SIM_SMS_COMMAND_RTH "RTH" +#define SIM_PIN "0000" +#define SIM_LOW_ALT_WARNING_MODES (NAV_ALTHOLD_MODE || NAV_RTH_MODE || NAV_WP_MODE || FAILSAFE_MODE) + +#define SIM_RESPONSE_CODE_OK ('O' << 24 | 'K' << 16) +#define SIM_RESPONSE_CODE_ERROR ('E' << 24 | 'R' << 16 | 'R' << 8 | 'O') +#define SIM_RESPONSE_CODE_RING ('R' << 24 | 'I' << 16 | 'N' << 8 | 'G') +#define SIM_RESPONSE_CODE_CLIP ('C' << 24 | 'L' << 16 | 'I' << 8 | 'P') +#define SIM_RESPONSE_CODE_CREG ('C' << 24 | 'R' << 16 | 'E' << 8 | 'G') +#define SIM_RESPONSE_CODE_CSQ ('C' << 24 | 'S' << 16 | 'Q' << 8 | ':') +#define SIM_RESPONSE_CODE_CMT ('C' << 24 | 'M' << 16 | 'T' << 8 | ':') + + +typedef enum { + SIM_TX_FLAG = (1 << 0), + SIM_TX_FLAG_FAILSAFE = (1 << 1), + SIM_TX_FLAG_GPS = (1 << 2), + SIM_TX_FLAG_ACC = (1 << 3), + SIM_TX_FLAG_LOW_ALT = (1 << 4), + SIM_TX_FLAG_RESPONSE = (1 << 5) +} simTxFlags_e; + +typedef enum { + SIM_MODULE_NOT_DETECTED = 0, + SIM_MODULE_NOT_REGISTERED, + SIM_MODULE_REGISTERED, +} simModuleState_e; + +typedef enum { + SIM_STATE_INIT = 0, + SIM_STATE_INIT2, + SIM_STATE_INIT_ENTER_PIN, + SIM_STATE_SET_MODES, + SIM_STATE_SEND_SMS, + SIM_STATE_SEND_SMS_ENTER_MESSAGE +} simTelemetryState_e; + +typedef enum { + SIM_AT_OK = 0, + SIM_AT_ERROR, + SIM_AT_WAITING_FOR_RESPONSE +} simATCommandState_e; + +typedef enum { + SIM_READSTATE_RESPONSE = 0, + SIM_READSTATE_SMS, + SIM_READSTATE_SKIP +} simReadState_e; + +typedef enum { + SIM_TX_NO = 0, + SIM_TX_FS, + SIM_TX +} simTransmissionState_e; + +typedef enum { + ACC_EVENT_NONE = 0, + ACC_EVENT_HIGH, + ACC_EVENT_LOW, + ACC_EVENT_NEG_X +} accEvent_t; + + +static serialPort_t *simPort; +static serialPortConfig_t *portConfig; +static bool simEnabled = false; + +static uint8_t atCommand[SIM_AT_COMMAND_MAX_SIZE]; +static int simTelemetryState = SIM_STATE_INIT; +static timeMs_t sim_t_stateChange = 0; +static uint8_t simResponse[SIM_RESPONSE_BUFFER_SIZE + 1]; +static int atCommandStatus = SIM_AT_OK; +static bool simWaitAfterResponse = false; +static uint8_t readState = SIM_READSTATE_RESPONSE; +static uint8_t transmitFlags = 0; +static timeMs_t t_lastMessageSent = 0; +static uint8_t lastMessageTriggeredBy = 0; +static uint8_t simModuleState = SIM_MODULE_NOT_DETECTED; + +static int simRssi; +static uint8_t accEvent = ACC_EVENT_NONE; +static char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " }; +static char* modeDescriptions[] = { "MAN", "ACR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "LAU", "FS" }; +static const char gpsFixIndicators[] = { '!', '*', ' ' }; + + +// XXX UNUSED +#if 0 +static bool isGroundStationNumberDefined(void) { + return telemetryConfig()->simGroundStationNumber[0] != '\0'; +} +#endif + +static bool checkGroundStationNumber(uint8_t* rv) +{ + int i; + const uint8_t* gsn = telemetryConfig()->simGroundStationNumber; + + int digitsToCheck = strlen((char*)gsn); + if (gsn[0] == '+') { + digitsToCheck -= 5; // ignore country code (max 4 digits) + } else if (gsn[0] == '0') { // ignore trunk prefixes: '0', '8', 01', '02', '06' + digitsToCheck--; + if (gsn[1] == '1' || gsn[1] == '2' || gsn[1] == '6') { + digitsToCheck--; + } + } else if (gsn[0] == '8') { + digitsToCheck--; + } + + for (i = 0; i < 16 && *gsn != '\0'; i++) gsn++; + if (i == 0) + return false; + for (i = 0; i < 16 && *rv != '\"'; i++) rv++; + + gsn--; rv--; + for (i = 0; i < digitsToCheck; i++) { + if (*rv != *gsn) return false; + gsn--; rv--; + } + return true; +} + + +static void readOriginatingNumber(uint8_t* rv) +{ + int i; + uint8_t* gsn = telemetryConfigMutable()->simGroundStationNumber; + if (gsn[0] != '\0') + return; + for (i = 0; i < 15 && rv[i] != '\"'; i++) + gsn[i] = rv[i]; + gsn[i] = '\0'; +} + +static void readTransmitFlags(const uint8_t* fs) +{ + int i; + + transmitFlags = 0; + for (i = 0; i < SIM_N_TX_FLAGS && fs[i] != '\0'; i++) { + switch (fs[i]) { + case 'T': case 't': + transmitFlags |= SIM_TX_FLAG; + break; + case 'F': case 'f': + transmitFlags |= SIM_TX_FLAG_FAILSAFE; + break; + case 'G': case 'g': + transmitFlags |= SIM_TX_FLAG_GPS; + break; + case 'L': case 'l': + transmitFlags |= SIM_TX_FLAG_LOW_ALT; + break; + case 'A': case 'a': + transmitFlags |= SIM_TX_FLAG_ACC; + break; + } + } +} + +static void requestSendSMS(uint8_t trigger) +{ + lastMessageTriggeredBy = trigger; + if (simTelemetryState == SIM_STATE_SEND_SMS_ENTER_MESSAGE) + return; // sending right now, don't reissue AT command + simTelemetryState = SIM_STATE_SEND_SMS; + if (atCommandStatus != SIM_AT_WAITING_FOR_RESPONSE) + sim_t_stateChange = 0; // send immediately +} + +static void readSMS(void) +{ + if (sl_strcasecmp((char*)simResponse, SIM_SMS_COMMAND_RTH) == 0) { + if (getStateOfForcedRTH() == RTH_IDLE) { + activateForcedRTH(); + } else { + abortForcedRTH(); + } + } else { + readTransmitFlags(simResponse); + } + requestSendSMS(SIM_TX_FLAG_RESPONSE); +} + +static void readSimResponse(void) +{ + if (readState == SIM_READSTATE_SKIP) { + readState = SIM_READSTATE_RESPONSE; + return; + } else if (readState == SIM_READSTATE_SMS) { + readSMS(); + readState = SIM_READSTATE_RESPONSE; + return; + } + + uint8_t* resp = simResponse; + uint32_t responseCode = 0; + if (simResponse[0] == '+') { + resp++; + } + responseCode = *resp++; + responseCode <<= 8; responseCode |= *resp++; + responseCode <<= 8; responseCode |= *resp++; + responseCode <<= 8; responseCode |= *resp++; + + if (responseCode == SIM_RESPONSE_CODE_OK) { + // OK + atCommandStatus = SIM_AT_OK; + if (!simWaitAfterResponse) { + sim_t_stateChange = millis() + SIM_AT_COMMAND_DELAY_MIN_MS; + } + return; + } else if (responseCode == SIM_RESPONSE_CODE_ERROR) { + // ERROR + atCommandStatus = SIM_AT_ERROR; + if (!simWaitAfterResponse) { + sim_t_stateChange = millis() + SIM_AT_COMMAND_DELAY_MIN_MS; + } + return; + } else if (responseCode == SIM_RESPONSE_CODE_RING) { + // RING + } else if (responseCode == SIM_RESPONSE_CODE_CSQ) { + // +CSQ: 26,0 + simRssi = fastA2I((char*)&simResponse[6]); + } else if (responseCode == SIM_RESPONSE_CODE_CLIP) { + // we always get this after a RING when a call is incoming + // +CLIP: "+3581234567" + readOriginatingNumber(&simResponse[8]); + if (checkGroundStationNumber(&simResponse[8])) { + requestSendSMS(SIM_TX_FLAG_RESPONSE); + } + } else if (responseCode == SIM_RESPONSE_CODE_CREG) { + // +CREG: 0,1 + if (simResponse[9] == '1' || simResponse[9] == '5') { + simModuleState = SIM_MODULE_REGISTERED; + } else { + simModuleState = SIM_MODULE_NOT_REGISTERED; + } + } else if (responseCode == SIM_RESPONSE_CODE_CMT) { + // +CMT: ,[],[,,,,,,,] + // +CMT: "+3581234567","","19/02/12,14:57:24+08" + readOriginatingNumber(&simResponse[7]); + if (checkGroundStationNumber(&simResponse[7])) { + readState = SIM_READSTATE_SMS; // next simResponse line will be SMS content + } else { + readState = SIM_READSTATE_SKIP; // skip SMS content + } + } +} + +static int16_t getAltitudeMeters(void) +{ +#if defined(USE_NAV) + return getEstimatedActualPosition(Z) / 100; +#else + return sensors(SENSOR_GPS) ? gpsSol.llh.alt / 100 : 0; +#endif +} + +static void transmit(void) +{ + timeMs_t timeSinceMsg = millis() - t_lastMessageSent; + uint8_t triggers = SIM_TX_FLAG; + uint32_t accSq = sq(imuMeasuredAccelBF.x) + sq(imuMeasuredAccelBF.y) + sq(imuMeasuredAccelBF.z); + + if (telemetryConfig()->accEventThresholdHigh > 0 && accSq > sq(telemetryConfig()->accEventThresholdHigh)) { + triggers |= SIM_TX_FLAG_ACC; + accEvent = ACC_EVENT_HIGH; + } else if (accSq < sq(telemetryConfig()->accEventThresholdLow)) { + triggers |= SIM_TX_FLAG_ACC; + accEvent = ACC_EVENT_LOW; + } else if (telemetryConfig()->accEventThresholdNegX > 0 && imuMeasuredAccelBF.x < -telemetryConfig()->accEventThresholdNegX) { + triggers |= SIM_TX_FLAG_ACC; + accEvent = ACC_EVENT_NEG_X; + } + + if ((lastMessageTriggeredBy & SIM_TX_FLAG_ACC) && timeSinceMsg < 2000) + accEvent = ACC_EVENT_NONE; + + if (FLIGHT_MODE(FAILSAFE_MODE)) + triggers |= SIM_TX_FLAG_FAILSAFE; + if (!navigationPositionEstimateIsHealthy()) + triggers |= SIM_TX_FLAG_GPS; + if (gpsSol.fixType != GPS_NO_FIX && FLIGHT_MODE(SIM_LOW_ALT_WARNING_MODES) && getAltitudeMeters() < telemetryConfig()->simLowAltitude) + triggers |= SIM_TX_FLAG_LOW_ALT; + + triggers &= transmitFlags; + + if (!triggers) + return; + if (!ARMING_FLAG(WAS_EVER_ARMED)) + return; + + if ((triggers & ~lastMessageTriggeredBy) // if new trigger activated after last msg, don't wait + || timeSinceMsg > 1000 * MAX(SIM_MIN_TRANSMIT_INTERVAL, telemetryConfig()->simTransmitInterval)) { + requestSendSMS(triggers); + } +} + +static void sendATCommand(const char* command) +{ + atCommandStatus = SIM_AT_WAITING_FOR_RESPONSE; + uint8_t len = MIN((uint8_t)strlen(command), SIM_AT_COMMAND_MAX_SIZE); + serialWriteBuf(simPort, (const uint8_t*) command, len); +} + +static void sendSMS(void) +{ + char pluscode_url[20]; + int16_t groundSpeed = 0; + uint16_t vbat = getBatteryVoltage(); + int16_t amps = isAmperageConfigured() ? getAmperage() / 10 : 0; // 1 = 100 milliamps + uint16_t avgSpeed = lrintf(10 * calculateAverageSpeed()); + uint32_t now = millis(); + + memset(pluscode_url, 0, sizeof(pluscode_url)); + + if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { + groundSpeed = gpsSol.groundSpeed / 100; + + char buf[20]; + olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, 11, buf, sizeof(buf)); + + // URLencode plus code (replace plus sign with %2B) + for (char *in = buf, *out = pluscode_url; *in; ) { + if (*in == '+') { + in++; + *out++ = '%'; + *out++ = '2'; + *out++ = 'B'; + } + else { + *out++ = *in++; + } + } + } + + // \x1a sends msg, \x1b cancels + uint8_t len = tfp_sprintf((char*)atCommand, "%s%d.%02dV %d.%dA ALT:%d SPD:%d/%d.%d DIS:%lu/%lu HDG:%d SAT:%d%c SIG:%d %s https://maps.google.com/?q=%s\x1a", + accEventDescriptions[accEvent], + vbat / 100, vbat % 100, + amps / 10, amps % 10, + getAltitudeMeters(), + groundSpeed, avgSpeed / 10, avgSpeed % 10, + GPS_distanceToHome, getTotalTravelDistance() / 100, + attitude.values.yaw, + gpsSol.numSat, gpsFixIndicators[gpsSol.fixType], + simRssi, + getStateOfForcedRTH() == RTH_IDLE ? modeDescriptions[getFlightModeForTelemetry()] : "RTH", + pluscode_url); + + serialWriteBuf(simPort, atCommand, len); + t_lastMessageSent = now; + accEvent = ACC_EVENT_NONE; + atCommandStatus = SIM_AT_WAITING_FOR_RESPONSE; +} + +void handleSimTelemetry() +{ + static uint16_t simResponseIndex = 0; + uint32_t now = millis(); + + if (!simEnabled) + return; + if (!simPort) + return; + + while (serialRxBytesWaiting(simPort) > 0) { + uint8_t c = serialRead(simPort); + if (c == '\n' || simResponseIndex == SIM_RESPONSE_BUFFER_SIZE) { + simResponse[simResponseIndex] = '\0'; + if (simResponseIndex > 0) simResponseIndex--; + if (simResponse[simResponseIndex] == '\r') simResponse[simResponseIndex] = '\0'; + simResponseIndex = 0; //data ok + readSimResponse(); + break; + } else { + simResponse[simResponseIndex] = c; + simResponseIndex++; + } + } + + transmit(); + + if (now < sim_t_stateChange) + return; + + sim_t_stateChange = now + SIM_AT_COMMAND_DELAY_MS; // by default, if OK or ERROR not received, wait this long + simWaitAfterResponse = false; // by default, if OK or ERROR received, go to next state immediately. + switch (simTelemetryState) { + case SIM_STATE_INIT: + sendATCommand("AT\r"); + simTelemetryState = SIM_STATE_INIT2; + break; + case SIM_STATE_INIT2: + sendATCommand("ATE0\r"); + simTelemetryState = SIM_STATE_INIT_ENTER_PIN; + break; + case SIM_STATE_INIT_ENTER_PIN: + sendATCommand("AT+CPIN=" SIM_PIN "\r"); + simTelemetryState = SIM_STATE_SET_MODES; + break; + case SIM_STATE_SET_MODES: + sendATCommand("AT+CMGF=1;+CNMI=3,2;+CLIP=1;+CSQ\r"); + simTelemetryState = SIM_STATE_INIT; + sim_t_stateChange = now + SIM_CYCLE_MS; + simWaitAfterResponse = true; + break; + case SIM_STATE_SEND_SMS: + sendATCommand("AT+CMGS=\""); + sendATCommand((char*)telemetryConfig()->simGroundStationNumber); + sendATCommand("\"\r"); + simTelemetryState = SIM_STATE_SEND_SMS_ENTER_MESSAGE; + sim_t_stateChange = now + 100; + break; + case SIM_STATE_SEND_SMS_ENTER_MESSAGE: + sendSMS(); + simTelemetryState = SIM_STATE_INIT; + sim_t_stateChange = now + SIM_CYCLE_MS; + simWaitAfterResponse = true; + break; + } +} + +// XXX UNUSED +#if 0 +static void freeSimTelemetryPort(void) +{ + closeSerialPort(simPort); + simPort = NULL; + simEnabled = false; +} +#endif + +void initSimTelemetry(void) +{ + portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SIM); +} + +static void configureSimTelemetryPort(void) +{ + if (!portConfig) { + return; + } + baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex; + simPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SIM, NULL, NULL, + baudRates[baudRateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); + + if (!simPort) { + return; + } + + sim_t_stateChange = millis() + SIM_STARTUP_DELAY_MS; + simTelemetryState = SIM_STATE_INIT; + readState = SIM_READSTATE_RESPONSE; + readTransmitFlags(telemetryConfig()->simTransmitFlags); + simEnabled = true; +} + +void checkSimTelemetryState(void) +{ + if (simEnabled) { + return; + } + configureSimTelemetryPort(); +} + +#endif diff --git a/src/main/telemetry/sim.h b/src/main/telemetry/sim.h new file mode 100644 index 00000000000..2df17f1d3df --- /dev/null +++ b/src/main/telemetry/sim.h @@ -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 . + */ + +#pragma once + +#define SIM_MIN_TRANSMIT_INTERVAL 10 +#define SIM_DEFAULT_TRANSMIT_INTERVAL 60 +#define SIM_N_TX_FLAGS 5 +#define SIM_DEFAULT_TX_FLAGS "f" + + +void handleSimTelemetry(void); +void initSimTelemetry(void); +void checkSimTelemetryState(void); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 5e3ac73ad11..408cd494269 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -304,7 +304,7 @@ static void freeSmartPortTelemetryPort(void) static void configureSmartPortTelemetryPort(void) { if (portConfig) { - portOptions_t portOptions = (telemetryConfig()->smartportUartUnidirectional ? SERIAL_UNIDIR : SERIAL_BIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED); + portOptions_t portOptions = (telemetryConfig()->uartUnidirectional ? SERIAL_UNIDIR : SERIAL_BIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED); smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions); } diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index cc5b20b8575..7fe3a85d54c 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -48,8 +48,10 @@ #include "telemetry/jetiexbus.h" #include "telemetry/ibus.h" #include "telemetry/crsf.h" +#include "telemetry/sim.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 1); + +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 3); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .gpsNoFixLatitude = 0, @@ -62,10 +64,24 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .frsky_pitch_roll = 0, .report_cell_voltage = 0, .hottAlarmSoundInterval = 5, - .smartportUartUnidirectional = 0, + .uartUnidirectional = 0, .smartportFuelUnit = SMARTPORT_FUEL_UNIT_MAH, .ibusTelemetryType = 0, .ltmUpdateRate = LTM_RATE_NORMAL, + .simTransmitInterval = SIM_DEFAULT_TRANSMIT_INTERVAL, + .simTransmitFlags = SIM_DEFAULT_TX_FLAGS, + .simLowAltitude = INT16_MIN, + .accEventThresholdHigh = 0, + .accEventThresholdLow = 0, + .accEventThresholdNegX = 0, + + .mavlink = { + .extended_status_rate = 2, + .rc_channels_rate = 5, + .position_rate = 2, + .extra1_rate = 10, + .extra2_rate = 2 + } ); void telemetryInit(void) @@ -98,6 +114,10 @@ void telemetryInit(void) initIbusTelemetry(); #endif +#if defined(USE_TELEMETRY_SIM) + initSimTelemetry(); +#endif + #if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF) initCrsfTelemetry(); #endif @@ -156,6 +176,10 @@ void telemetryCheckState(void) checkIbusTelemetryState(); #endif +#if defined(USE_TELEMETRY_SIM) + checkSimTelemetryState(); +#endif + #if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF) checkCrsfTelemetryState(); #endif @@ -193,6 +217,10 @@ void telemetryProcess(timeUs_t currentTimeUs) handleIbusTelemetry(); #endif +#if defined(USE_TELEMETRY_SIM) + handleSimTelemetry(); +#endif + #if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF) handleCrsfTelemetry(currentTimeUs); #endif diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 9c03c8711d2..e2928da15aa 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -64,10 +64,24 @@ typedef struct telemetryConfig_s { uint8_t frsky_pitch_roll; uint8_t report_cell_voltage; uint8_t hottAlarmSoundInterval; - uint8_t smartportUartUnidirectional; + uint8_t uartUnidirectional; smartportFuelUnit_e smartportFuelUnit; uint8_t ibusTelemetryType; uint8_t ltmUpdateRate; + uint16_t simTransmitInterval; + uint8_t simTransmitFlags[4]; + uint16_t accEventThresholdHigh; + uint16_t accEventThresholdLow; + uint16_t accEventThresholdNegX; + int16_t simLowAltitude; + uint8_t simGroundStationNumber[16]; + struct { + uint8_t extended_status_rate; + uint8_t rc_channels_rate; + uint8_t position_rate; + uint8_t extra1_rate; + uint8_t extra2_rate; + } mavlink; } telemetryConfig_t; PG_DECLARE(telemetryConfig_t, telemetryConfig); diff --git a/src/test/Makefile b/src/test/Makefile index 6ce38c4ae61..444c5ba0657 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -696,7 +696,6 @@ $(OBJECT_DIR)/rcdevice_unittest : \ $(OBJECT_DIR)/io/rcdevice.o \ $(OBJECT_DIR)/io/rcdevice_cam.o \ $(OBJECT_DIR)/fc/rc_modes.o \ - $(OBJECT_DIR)/rx/rx.o \ $(OBJECT_DIR)/common/maths.o \ $(OBJECT_DIR)/gtest_main.a diff --git a/src/test/unit/flight_failsafe_unittest.cc.txt b/src/test/unit/flight_failsafe_unittest.cc.txt index 21a97172611..f72b61a7bb7 100644 --- a/src/test/unit/flight_failsafe_unittest.cc.txt +++ b/src/test/unit/flight_failsafe_unittest.cc.txt @@ -458,9 +458,4 @@ uint16_t getCurrentMinthrottle(void) return testMinThrottle; } -bool isUsingSticksForArming(void) -{ - return isUsingSticksToArm; -} - } diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index ff174a1159c..a900257c422 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -78,3 +78,7 @@ extern SysTick_Type *SysTick; #define WS2811_DMA_HANDLER_IDENTIFER 0 #include "target.h" + +#define FAST_CODE +#define NOINLINE +#define EXTENDED_FASTRAM \ No newline at end of file diff --git a/src/test/unit/rcdevice_unittest.cc.txt b/src/test/unit/rcdevice_unittest.cc similarity index 67% rename from src/test/unit/rcdevice_unittest.cc.txt rename to src/test/unit/rcdevice_unittest.cc index e63bd82a0a1..fcaf5c94436 100644 --- a/src/test/unit/rcdevice_unittest.cc.txt +++ b/src/test/unit/rcdevice_unittest.cc @@ -29,44 +29,65 @@ extern "C" { #include "common/maths.h" #include "common/utils.h" #include "common/streambuf.h" - #include "common/time.h" - #include "config/parameter_group.h" - #include "config/parameter_group_ids.h" + #include "drivers/serial.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" + #include "io/beeper.h" #include "io/serial.h" #include "scheduler/scheduler.h" - #include "drivers/serial.h" - #include "drivers/vcd.h" #include "io/rcdevice_cam.h" #include "io/osd.h" #include "io/rcdevice.h" + #include "config/parameter_group_ids.h" + // #include "pg/pg_ids.h" + // #include "pg/vcd.h" + // #include "pg/rx.h" + // #include "pg/rcdevice.h" + #include "rx/rx.h" int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] + int16_t rcCommand[4]; + uint32_t stateFlags; + rcControlsConfig_t rcControlsConfig_System; extern rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; extern runcamDevice_t *camDevice; - extern bool needRelease; + extern bool isButtonPressed; + extern bool rcdeviceInMenu; + extern rcdeviceWaitingResponseQueue watingResponseQueue; bool unitTestIsSwitchActivited(boxId_e boxId) { uint8_t adjustBoxID = boxId - BOXCAMERA1; rcdeviceSwitchState_s switchState = switchStates[adjustBoxID]; return switchState.isActivated; } - timeUs_t micros(void) { return 0; } - void tfp_sprintf(char *, char*, ...) { } - bool rtcGetDateTime(dateTime_t *dt) { UNUSED(dt); return false; } + int16_t rxGetChannelValue(unsigned ch) + { + return rcData[ch]; + } + + uint32_t millis(void); + int minTimeout = 180; + + void rcdeviceSend5KeyOSDCableSimualtionEvent(rcdeviceCamSimulationKeyEvent_e key); + rcdeviceResponseParsingContext_t* rcdeviceRespCtxQueueShift(rcdeviceWaitingResponseQueue *queue); + + const uint32_t baudRates[] = { 0, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 115200, 230400, 250000, + 460800, 921600, 1000000, 1500000, 2000000, 2470000 }; // see baudRate_e } #define MAX_RESPONSES_COUNT 10 +#define FIVE_KEY_JOYSTICK_MIN FIVE_KEY_CABLE_JOYSTICK_MIN - 1 +#define FIVE_KEY_JOYSTICK_MID FIVE_KEY_CABLE_JOYSTICK_MID_START + 1 +#define FIVE_KEY_JOYSTICK_MAX FIVE_KEY_CABLE_JOYSTICK_MAX + 1 typedef struct testData_s { bool isRunCamSplitPortConfigurated; @@ -82,6 +103,7 @@ typedef struct testData_s { } testData_t; static testData_t testData; +extern rcdeviceWaitingResponseQueue watingResponseQueue; static void clearResponseBuff() { @@ -89,17 +111,22 @@ static void clearResponseBuff() testData.responseBufCount = 0; memset(testData.responseBufsLen, 0, MAX_RESPONSES_COUNT); memset(testData.responesBufs, 0, MAX_RESPONSES_COUNT * 60); -} -static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlushSerial) -{ - if (withDataForFlushSerial) { - memcpy(testData.responesBufs[testData.responseBufCount], "0", 1); - testData.responseBufsLen[testData.responseBufCount] = 1; - testData.responseBufCount++; + while (rcdeviceRespCtxQueueShift(&watingResponseQueue)) { + } +} +static void resetRCDeviceStatus() +{ + isButtonPressed = false; + rcdeviceInMenu = false; + clearResponseBuff(); +} +static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlushSerial) +{ + UNUSED(withDataForFlushSerial); memcpy(testData.responesBufs[testData.responseBufCount], data, dataLen); testData.responseBufsLen[testData.responseBufCount] = dataLen; testData.responseBufCount++; @@ -109,28 +136,43 @@ TEST(RCDeviceTest, TestRCSplitInitWithoutPortConfigurated) { runcamDevice_t device; + resetRCDeviceStatus(); + + watingResponseQueue.headPos = 0; + watingResponseQueue.tailPos = 0; + watingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); - bool result = runcamDeviceInit(&device); - EXPECT_EQ(false, result); + runcamDeviceInit(&device); + EXPECT_EQ(false, device.isReady); } TEST(RCDeviceTest, TestRCSplitInitWithoutOpenPortConfigurated) { runcamDevice_t device; + resetRCDeviceStatus(); + + watingResponseQueue.headPos = 0; + watingResponseQueue.tailPos = 0; + watingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = false; testData.isRunCamSplitPortConfigurated = true; - bool result = runcamDeviceInit(&device); - EXPECT_EQ(false, result); + runcamDeviceInit(&device); + EXPECT_EQ(false, device.isReady); } TEST(RCDeviceTest, TestInitDevice) { runcamDevice_t device; + resetRCDeviceStatus(); + // test correct response + watingResponseQueue.headPos = 0; + watingResponseQueue.tailPos = 0; + watingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; @@ -138,15 +180,26 @@ TEST(RCDeviceTest, TestInitDevice) uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; addResponseData(responseData, sizeof(responseData), true); - bool result = runcamDeviceInit(&device); - EXPECT_EQ(result, true); + runcamDeviceInit(&device); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(device.isReady, true); } TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse) { runcamDevice_t device; + resetRCDeviceStatus(); + // test correct response data with incorrect len + watingResponseQueue.headPos = 0; + watingResponseQueue.tailPos = 0; + watingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; @@ -154,46 +207,82 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse) uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD, 0x33 }; addResponseData(responseData, sizeof(responseData), true); - bool result = runcamDeviceInit(&device); - EXPECT_EQ(result, true); + runcamDeviceInit(&device); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + EXPECT_EQ(device.isReady, true); clearResponseBuff(); + testData.millis += minTimeout; // invalid crc uint8_t responseDataWithInvalidCRC[] = { 0xCC, 0x01, 0x37, 0x00, 0xBE }; addResponseData(responseDataWithInvalidCRC, sizeof(responseDataWithInvalidCRC), true); - result = runcamDeviceInit(&device); - EXPECT_EQ(result, false); + runcamDeviceInit(&device); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + EXPECT_EQ(device.isReady, false); clearResponseBuff(); + testData.millis += minTimeout; // incomplete response data uint8_t incompleteResponseData[] = { 0xCC, 0x01, 0x37 }; addResponseData(incompleteResponseData, sizeof(incompleteResponseData), true); - result = runcamDeviceInit(&device); - EXPECT_EQ(result, false); + runcamDeviceInit(&device); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(device.isReady, false); clearResponseBuff(); + testData.millis += minTimeout; // test timeout memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; testData.isAllowBufferReadWrite = true; - result = runcamDeviceInit(&device); - EXPECT_EQ(result, false); + runcamDeviceInit(&device); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + EXPECT_EQ(device.isReady, false); clearResponseBuff(); + testData.millis += minTimeout; } TEST(RCDeviceTest, TestWifiModeChangeWithDeviceUnready) { + resetRCDeviceStatus(); + // test correct response + watingResponseQueue.headPos = 0; + watingResponseQueue.tailPos = 0; + watingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; testData.isAllowBufferReadWrite = true; testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBC }; + uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBC }; // wrong response addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, false); + rcdeviceInit(); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(camDevice->isReady, false); // bind aux1, aux2, aux3 channel to wifi button, power button and change mode for (uint8_t i = 0; i <= (BOXCAMERA3 - BOXCAMERA1); i++) { @@ -218,6 +307,8 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceUnready) modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1300); modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(1600); + updateUsedModeActivationConditionFlags(); + // make the binded mode inactive rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1800; rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 900; @@ -235,6 +326,8 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceUnready) TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady) { + resetRCDeviceStatus(); + // test correct response memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; @@ -243,14 +336,23 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady) testData.maxTimesOfRespDataAvailable = 0; uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, true); + camDevice->info.features = 15; + rcdeviceInit(); + testData.millis += 3001; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(camDevice->isReady, true); // bind aux1, aux2, aux3 channel to wifi button, power button and change mode for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) { memset(modeActivationConditionsMutable(i), 0, sizeof(modeActivationCondition_t)); } + // bind aux1 to wifi button with range [900,1600] modeActivationConditionsMutable(0)->auxChannelIndex = 0; modeActivationConditionsMutable(0)->modeId = BOXCAMERA1; @@ -273,7 +375,6 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady) rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2000; rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700; - updateUsedModeActivationConditionFlags(); updateActivatedModes(); // runn process loop @@ -288,6 +389,8 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady) TEST(RCDeviceTest, TestWifiModeChangeCombine) { + resetRCDeviceStatus(); + memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; @@ -295,8 +398,14 @@ TEST(RCDeviceTest, TestWifiModeChangeCombine) testData.maxTimesOfRespDataAvailable = 0; uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(true, result); + rcdeviceInit(); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, camDevice->isReady); // bind aux1, aux2, aux3 channel to wifi button, power button and change mode for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) { @@ -364,6 +473,8 @@ TEST(RCDeviceTest, TestWifiModeChangeCombine) TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol) { + resetRCDeviceStatus(); + memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; @@ -371,123 +482,203 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol) testData.maxTimesOfRespDataAvailable = 0; uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(true, result); + rcdeviceInit(); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, camDevice->isReady); + clearResponseBuff(); // test timeout of open connection - result = runcamDeviceOpen5KeyOSDCableConnection(camDevice); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); + rcdeviceReceive(millis() * 1000); + testData.millis += 3000; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, rcdeviceInMenu); clearResponseBuff(); // open connection with correct response uint8_t responseDataOfOpenConnection[] = { 0xCC, 0x11, 0xe7 }; addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true); - result = runcamDeviceOpen5KeyOSDCableConnection(camDevice); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); clearResponseBuff(); // open connection with correct response but wrong data length uint8_t incorrectResponseDataOfOpenConnection1[] = { 0xCC, 0x11, 0xe7, 0x55 }; addResponseData(incorrectResponseDataOfOpenConnection1, sizeof(incorrectResponseDataOfOpenConnection1), true); - result = runcamDeviceOpen5KeyOSDCableConnection(camDevice); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); clearResponseBuff(); // open connection with invalid crc uint8_t incorrectResponseDataOfOpenConnection2[] = { 0xCC, 0x10, 0x42 }; addResponseData(incorrectResponseDataOfOpenConnection2, sizeof(incorrectResponseDataOfOpenConnection2), true); - result = runcamDeviceOpen5KeyOSDCableConnection(camDevice); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); // when crc wrong won't change the menu state clearResponseBuff(); // test timeout of close connection - runcamDeviceClose5KeyOSDCableConnection(camDevice); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE); + rcdeviceReceive(millis() * 1000); + testData.millis += 3000; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); // close menu timeout won't change the menu state clearResponseBuff(); // close connection with correct response uint8_t responseDataOfCloseConnection[] = { 0xCC, 0x21, 0x11 }; addResponseData(responseDataOfCloseConnection, sizeof(responseDataOfCloseConnection), true); - result = runcamDeviceClose5KeyOSDCableConnection(camDevice); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, rcdeviceInMenu); clearResponseBuff(); // close connection with correct response but wrong data length + addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); // open menu again + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); + clearResponseBuff(); + uint8_t responseDataOfCloseConnection1[] = { 0xCC, 0x21, 0x11, 0xC1 }; addResponseData(responseDataOfCloseConnection1, sizeof(responseDataOfCloseConnection1), true); - result = runcamDeviceClose5KeyOSDCableConnection(camDevice); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, rcdeviceInMenu); clearResponseBuff(); // close connection with response that invalid crc + addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); // open menu again + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); + clearResponseBuff(); + uint8_t responseDataOfCloseConnection2[] = { 0xCC, 0x21, 0xA1 }; addResponseData(responseDataOfCloseConnection2, sizeof(responseDataOfCloseConnection2), true); - result = runcamDeviceClose5KeyOSDCableConnection(camDevice); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, rcdeviceInMenu); + clearResponseBuff(); + + // release button first + uint8_t responseDataOfSimulation4[] = { 0xCC, 0xA5 }; + addResponseData(responseDataOfSimulation4, sizeof(responseDataOfSimulation4), true); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, isButtonPressed); clearResponseBuff(); // simulate press button with no response - result = runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER); + testData.millis += 2000; + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, isButtonPressed); clearResponseBuff(); // simulate press button with correct response uint8_t responseDataOfSimulation1[] = { 0xCC, 0xA5 }; addResponseData(responseDataOfSimulation1, sizeof(responseDataOfSimulation1), true); - result = runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, isButtonPressed); clearResponseBuff(); // simulate press button with correct response but wrong data length + addResponseData(responseDataOfSimulation4, sizeof(responseDataOfSimulation4), true); // release first + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, isButtonPressed); + clearResponseBuff(); + uint8_t responseDataOfSimulation2[] = { 0xCC, 0xA5, 0x22 }; addResponseData(responseDataOfSimulation2, sizeof(responseDataOfSimulation2), true); - result = runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, isButtonPressed); clearResponseBuff(); // simulate press button event with incorrect response uint8_t responseDataOfSimulation3[] = { 0xCC, 0xB5, 0x22 }; addResponseData(responseDataOfSimulation3, sizeof(responseDataOfSimulation3), true); - result = runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET); - EXPECT_EQ(false, result); - clearResponseBuff(); - - // simulate release button event - result = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, isButtonPressed); clearResponseBuff(); // simulate release button with correct response - uint8_t responseDataOfSimulation4[] = { 0xCC, 0xA5 }; addResponseData(responseDataOfSimulation4, sizeof(responseDataOfSimulation4), true); - result = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, isButtonPressed); clearResponseBuff(); // simulate release button with correct response but wrong data length + addResponseData(responseDataOfSimulation1, sizeof(responseDataOfSimulation1), true); // press first + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(true, isButtonPressed); + clearResponseBuff(); + uint8_t responseDataOfSimulation5[] = { 0xCC, 0xA5, 0xFF }; addResponseData(responseDataOfSimulation5, sizeof(responseDataOfSimulation5), true); - result = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice); - EXPECT_EQ(true, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, isButtonPressed); clearResponseBuff(); // simulate release button with incorrect response uint8_t responseDataOfSimulation6[] = { 0xCC, 0x31, 0xFF }; addResponseData(responseDataOfSimulation6, sizeof(responseDataOfSimulation6), true); - result = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice); - EXPECT_EQ(false, result); + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + rcdeviceReceive(millis() * 1000); + testData.millis += minTimeout; + EXPECT_EQ(false, isButtonPressed); clearResponseBuff(); } TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport) { + resetRCDeviceStatus(); + // test simulation without device init - rcData[THROTTLE] = 1500; // THROTTLE Mid - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1900; // Yaw High - rcdeviceUpdate(0); + rcData[THROTTLE] = FIVE_KEY_JOYSTICK_MID; // THROTTLE Mid + rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid + rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid + rcData[YAW] = FIVE_KEY_JOYSTICK_MAX; // Yaw High + rcdeviceUpdate(millis() * 1000); EXPECT_EQ(false, rcdeviceInMenu); + // remove all request from queue + for (int i = 0; i < 10; i++) { + testData.millis += 500000; + rcdeviceReceive(millis()); + } // init device that have not 5 key OSD cable simulation feature memset(&testData, 0, sizeof(testData)); @@ -496,225 +687,31 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport) testData.isAllowBufferReadWrite = true; testData.maxTimesOfRespDataAvailable = 0; uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; + rcdeviceInit(); + testData.millis += 3001; + rcdeviceReceive(millis() * 1000); + testData.millis += 200; + testData.responseDataReadPos = 0; + testData.indexOfCurrentRespBuf = 0; addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, true); + rcdeviceReceive(millis() * 1000); + testData.millis += 200; + EXPECT_EQ(camDevice->isReady, true); clearResponseBuff(); // open connection, rcdeviceInMenu will be false if the codes is right uint8_t responseDataOfOpenConnection[] = { 0xCC, 0x11, 0xe7 }; addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), false); - rcdeviceUpdate(0); + rcdeviceUpdate(millis() * 1000); EXPECT_EQ(false, rcdeviceInMenu); clearResponseBuff(); } -TEST(RCDeviceTest, Test5KeyOSDCableSimulationWith5KeyFeatureSupport) -{ - // test simulation without device init - rcData[THROTTLE] = 1500; // THROTTLE Mid - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1900; // Yaw High - rcdeviceUpdate(0); - EXPECT_EQ(false, rcdeviceInMenu); - - // init device that have not 5 key OSD cable simulation feature - memset(&testData, 0, sizeof(testData)); - testData.isRunCamSplitOpenPortSupported = true; - testData.isRunCamSplitPortConfigurated = true; - testData.isAllowBufferReadWrite = true; - testData.maxTimesOfRespDataAvailable = 0; - uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 }; - addResponseData(responseData, sizeof(responseData), true); - bool result = rcdeviceInit(); - EXPECT_EQ(result, true); - clearResponseBuff(); - - // open connection - uint8_t responseDataOfOpenConnection[] = { 0xCC, 0x11, 0xe7 }; - addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, rcdeviceInMenu); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // relase button - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1500; // Yaw High - uint8_t responseDataOfReleaseButton[] = { 0xCC, 0xA5 }; - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - clearResponseBuff(); - - // close connection - rcData[THROTTLE] = 1500; // THROTTLE Mid - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 900; // Yaw High - uint8_t responseDataOfCloseConnection[] = { 0xCC, 0x21, 0x11 }; - addResponseData(responseDataOfCloseConnection, sizeof(responseDataOfCloseConnection), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, rcdeviceInMenu); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // relase button - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1500; // Yaw High - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - clearResponseBuff(); - - // open osd menu again - rcData[THROTTLE] = 1500; // THROTTLE Mid - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1900; // Yaw High - addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, rcdeviceInMenu); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // relase button - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1500; // Yaw High - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - clearResponseBuff(); - - // send down button event - rcData[PITCH] = 900; - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // relase button - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1500; // Yaw High - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - rcData[PITCH] = 1500; // rest down button - clearResponseBuff(); - - // simulate right button long press - rcData[ROLL] = 1900; - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // send relase button event - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1500; // Yaw High - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - rcData[ROLL] = 1500; // reset right button - clearResponseBuff(); - - // simulate right button and get failed response, then FC should release the controller of joysticks - rcData[ROLL] = 1900; - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // send relase button with empty response, so the release will failed - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1500; // Yaw High - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - EXPECT_EQ(false, rcdeviceInMenu); // if failed on release button event, then FC side need release the controller of joysticks - rcData[ROLL] = 1500; // rest right button - // send again release button with correct response - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1500; // Yaw High - clearResponseBuff(); - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - EXPECT_EQ(false, rcdeviceInMenu); - clearResponseBuff(); - - // open OSD menu again - rcData[THROTTLE] = 1500; // THROTTLE Mid - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1900; // Yaw High - clearResponseBuff(); - addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, rcdeviceInMenu); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - - // relase button - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1500; // Yaw High - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - clearResponseBuff(); - - // send left event - rcData[ROLL] = 900; - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // send relase button - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1500; // Yaw High - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - EXPECT_EQ(true, rcdeviceInMenu); - clearResponseBuff(); - rcData[ROLL] = 1500; // rest right button - - // close connection - rcData[THROTTLE] = 1500; // THROTTLE Mid - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 900; // Yaw High - addResponseData(responseDataOfCloseConnection, sizeof(responseDataOfCloseConnection), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, rcdeviceInMenu); - EXPECT_EQ(true, needRelease); - clearResponseBuff(); - // relase button - rcData[ROLL] = 1500; // ROLL Mid - rcData[PITCH] = 1500; // PITCH Mid - rcData[YAW] = 1500; // Yaw High - addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true); - rcdeviceUpdate(0); - EXPECT_EQ(false, needRelease); - clearResponseBuff(); -} - extern "C" { - serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e function, - serialReceiveCallbackPtr rxCallback, void *rxCallbackData, - uint32_t baudRate, portMode_t mode, portOptions_t options) + serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, void *callbackData, uint32_t baudRate, portMode_t mode, portOptions_t options) { UNUSED(identifier); - UNUSED(function); - UNUSED(rxCallback); - UNUSED(rxCallbackData); + UNUSED(functionMask); UNUSED(baudRate); UNUSED(mode); UNUSED(options); @@ -732,7 +729,8 @@ extern "C" { s.txBuffer = s.txBuffer; // callback works for IRQ-based RX ONLY - s.rxCallback = NULL; + s.rxCallback = callback; + s.rxCallbackData = callbackData; s.baudRate = 0; return (serialPort_t *)&s; @@ -751,6 +749,7 @@ extern "C" { portConfig.msp_baudrateIndex = BAUD_115200; portConfig.gps_baudrateIndex = BAUD_57600; portConfig.telemetry_baudrateIndex = BAUD_AUTO; + portConfig.peripheral_baudrateIndex = BAUD_115200; portConfig.functionMask = FUNCTION_MSP; return &portConfig; @@ -765,10 +764,10 @@ extern "C" { uint8_t bufIndex = testData.indexOfCurrentRespBuf; uint8_t leftDataLen = 0; - if (testData.responseDataReadPos >= testData.responseBufsLen[bufIndex]) { + if (testData.responseDataReadPos + 1 > testData.responseBufsLen[bufIndex]) { return 0; } else { - leftDataLen = testData.responseBufsLen[bufIndex] - testData.responseDataReadPos; + leftDataLen = testData.responseBufsLen[bufIndex] - (testData.responseDataReadPos); } if (leftDataLen) { @@ -801,16 +800,17 @@ extern "C" { void sbufWriteString(sbuf_t *dst, const char *string) { - UNUSED(dst); UNUSED(string); + UNUSED(dst); + UNUSED(string); if (testData.isAllowBufferReadWrite) { sbufWriteData(dst, string, strlen(string)); } } - void sbufWriteU8(sbuf_t *dst, uint8_t val) { - UNUSED(dst); UNUSED(val); + UNUSED(dst); + UNUSED(val); if (testData.isAllowBufferReadWrite) { *dst->ptr++ = val; @@ -819,7 +819,9 @@ extern "C" { void sbufWriteData(sbuf_t *dst, const void *data, int len) { - UNUSED(dst); UNUSED(data); UNUSED(len); + UNUSED(dst); + UNUSED(data); + UNUSED(len); if (testData.isAllowBufferReadWrite) { memcpy(dst->ptr, data, len); @@ -830,7 +832,8 @@ extern "C" { // modifies streambuf so that written data are prepared for reading void sbufSwitchToReader(sbuf_t *buf, uint8_t *base) { - UNUSED(buf); UNUSED(base); + UNUSED(buf); + UNUSED(base); if (testData.isAllowBufferReadWrite) { buf->end = buf->ptr; @@ -867,10 +870,11 @@ extern "C" { return buf->ptr; } - void sbufReadData(const sbuf_t *src, void *data, int len) + void sbufReadData(const sbuf_t *dst, void *data, int len) { + UNUSED(dst); if (testData.isAllowBufferReadWrite) { - memcpy(data, src->ptr, len); + memcpy(data, data, len); } } @@ -898,11 +902,16 @@ extern "C" { void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) { - UNUSED(instance); UNUSED(data); UNUSED(count); + UNUSED(instance); + UNUSED(data); + UNUSED(count); - // // reset the input buffer + // reset the input buffer testData.responseDataReadPos = 0; testData.indexOfCurrentRespBuf++; + if (testData.indexOfCurrentRespBuf >= testData.responseBufCount) { + testData.indexOfCurrentRespBuf = 0; + } // testData.maxTimesOfRespDataAvailable = testData.responseDataLen + 1; } @@ -934,14 +943,10 @@ extern "C" { } uint32_t millis(void) { return testData.millis++; } - + uint32_t micros(void) { return millis() * 1000; } void beeper(beeperMode_e mode) { UNUSED(mode); } uint8_t armingFlags = 0; bool cmsInMenu; uint32_t resumeRefreshAt = 0; - - void failsafeOnRxSuspend(uint32_t ) {} - void failsafeOnRxResume(void) {} - void failsafeOnValidDataReceived(void) {} - void failsafeOnValidDataFailed(void) {} + int getArmingDisableFlags(void) {return 0;} } diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index 52168e14cd3..a17312e4dc4 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -140,8 +140,6 @@ timeMs_t millis(void) {return milliTime++;} uint32_t micros(void) {return 0;} void beeper(beeperMode_e) {} uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE }; -void addBootlogEvent6(bootLogEventCode_e eventCode, uint16_t eventFlags, uint16_t param1, uint16_t param2, uint16_t param3, uint16_t param4) - {UNUSED(eventCode);UNUSED(eventFlags);UNUSED(param1);UNUSED(param2);UNUSED(param3);UNUSED(param4);} timeDelta_t getLooptime(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(cfTaskId_e) {} diff --git a/src/test/unit/serial_msp_unittest.cc.txt b/src/test/unit/serial_msp_unittest.cc.txt index 14d75c9c179..2e125eb4d10 100644 --- a/src/test/unit/serial_msp_unittest.cc.txt +++ b/src/test/unit/serial_msp_unittest.cc.txt @@ -545,7 +545,7 @@ uint16_t cycleTime = 0; // this is the number in micro second to achieve // from navigation.c int32_t GPS_home[2]; int32_t GPS_hold[2]; -uint16_t GPS_distanceToHome; // distance to home point in meters +uint32_t GPS_distanceToHome; // distance to home point in meters int16_t GPS_directionToHome; // direction to home or hol point in degrees navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode void GPS_set_next_wp(int32_t *lat, int32_t *lon) {UNUSED(lat);UNUSED(lon);} diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index 3e643b5f6fb..88b64d9681d 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -174,7 +174,7 @@ gpsSolutionData_t gpsSol; uint8_t GPS_numSat; int32_t GPS_coord[2]; uint16_t GPS_speed; // speed in 0.1m/s -uint16_t GPS_distanceToHome; // distance to home point in meters +uint32_t GPS_distanceToHome; // distance to home point in meters uint16_t GPS_altitude; // altitude in 0.1m uint16_t vbat; int16_t GPS_directionToHome; // direction to home or hol point in degrees diff --git a/src/utils/compiler.rb b/src/utils/compiler.rb index 54979c193a7..3a8f4d99191 100644 --- a/src/utils/compiler.rb +++ b/src/utils/compiler.rb @@ -61,6 +61,10 @@ def default_args if flag == "" || flag == "-MMD" || flag == "-MP" || flag.start_with?("-save-temps") next end + # -Wstrict-prototypes is not valid for C++ + if flag == "-Wstrict-prototypes" + next + end if flag.start_with? "-std=" flag = "-std=c++11" end diff --git a/src/utils/declination.py b/src/utils/declination.py new file mode 100644 index 00000000000..8608ffda95f --- /dev/null +++ b/src/utils/declination.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python + +# Ported from https://github.com/ArduPilot/ardupilot/tree/master/libraries/AP_Declination/generate +# Run this script with python3! +# To install the igrf module, use python3 -m pip install --user igrf12 +# Note that it requires a fortran compiler + +''' +generate field tables from IGRF12 +''' + +import collections +import datetime +import pathlib +import sys + +import igrf12 +import numpy as np + +SAMPLING_RES = 10 +SAMPLING_MIN_LAT = -90 +SAMPLING_MAX_LAT = 90 +SAMPLING_MIN_LON = -180 +SAMPLING_MAX_LON = 180 + +# This is used for flash constrained environments. We limit +# the latitude range to [-60, 60], so the values fit in an int8_t +SAMPLING_COMPACT_MIN_LAT = -60 +SAMPLING_COMPACT_MAX_LAT = 60 + +PREPROCESSOR_SYMBOL = 'NAV_AUTO_MAG_DECLINATION_PRECISE' + +Query = collections.namedtuple('Query', ['date', 'res', 'min_lat', 'max_lat', 'min_lon', 'max_lon']) +Result = collections.namedtuple('Result', ['query', 'lats', 'lons', 'declination', 'inclination', 'intensity']) + +def write_table(f, name, table, compact): + '''write one table''' + + if compact: + format_entry = lambda x: '%d' % round(x) + table_type = 'int8_t' + else: + table_type = 'float' + format_entry = lambda x: '%.5ff' % x + + num_lat = len(table) + num_lon = len(table[0]) + + f.write("static const %s %s[%u][%u] = {\n" % + (table_type, name, num_lat, num_lon)) + for i in range(num_lat): + f.write(" {") + for j in range(num_lon): + f.write(format_entry(table[i][j])) + if j != num_lon - 1: + f.write(",") + f.write("}") + if i != num_lat - 1: + f.write(",") + f.write("\n") + f.write("};\n\n") + +def declination_tables(query): + lats = np.arange(query.min_lat, query.max_lat + query.res, query.res) + lons = np.arange(query.min_lon, query.max_lon + query.res, query.res) + + num_lat = lats.size + num_lon = lons.size + + intensity = np.empty((num_lat, num_lon)) + inclination = np.empty((num_lat, num_lon)) + declination = np.empty((num_lat, num_lon)) + + for i, lat in enumerate(lats): + for j, lon in enumerate(lons): + mag = igrf12.igrf(date, glat=lat, glon=lon, alt_km=0., isv=0, itype=1) + intensity[i][j] = mag.total / 1e5 + inclination[i][j] = mag.incl + declination[i][j] = mag.decl + + return Result(query=query, lats=lats, lons=lons, + declination=declination, inclination=inclination, intensity=intensity) + +def generate_constants(f, query): + f.write('#define SAMPLING_RES\t\t%.5ff\n' % query.res) + f.write('#define SAMPLING_MIN_LON\t%.5ff\n' % query.min_lon) + f.write('#define SAMPLING_MAX_LON\t%.5ff\n' % query.max_lon) + f.write('#define SAMPLING_MIN_LAT\t%.5ff\n' % query.min_lat) + f.write('#define SAMPLING_MAX_LAT\t%.5ff\n' % query.max_lat) + f.write('\n') + +def generate_tables(f, query, compact): + result = declination_tables(query) + write_table(f, 'declination_table', result.declination, compact) + + # We're not using these tables for now + #if not compact: + # write_table(f, 'inclination_table', result.inclination, False) + # write_table(f, 'intensity_table', result.intensity, False) + +def generate_code(f, date): + + compact_query = Query(date=date, res=SAMPLING_RES, + min_lat=SAMPLING_COMPACT_MIN_LAT, max_lat=SAMPLING_COMPACT_MAX_LAT, + min_lon=SAMPLING_MIN_LON, max_lon=SAMPLING_MAX_LON) + + precise_query = Query(date=date, res=SAMPLING_RES, + min_lat=SAMPLING_MIN_LAT, max_lat=SAMPLING_MAX_LAT, + min_lon=SAMPLING_MIN_LON, max_lon=SAMPLING_MAX_LON) + + f.write('/* this file is automatically generated by src/utils/declination.py - DO NOT EDIT! */\n\n\n') + f.write('/* Updated on %s */\n\n\n' % date) + f.write('#include \n\n') + + + f.write('\n\n#if defined(%s)\n' % PREPROCESSOR_SYMBOL) + generate_constants(f, precise_query) + generate_tables(f, precise_query, False) + # We're not using these tables for now + # write_table(f, 'inclination_table', inclination_table) + # write_table(f, 'intensity_table', intensity_table) + f.write('#else /* !%s */\n' % PREPROCESSOR_SYMBOL) + generate_constants(f, compact_query) + generate_tables(f, compact_query, True) + f.write('#endif\n') + +if __name__ == '__main__': + + output = pathlib.PurePath(__file__).parent / '..' / 'main' / 'navigation' / 'navigation_declination_gen.c' + date = datetime.datetime.now() + + with open(output, 'w') as f: + generate_code(f, date)