diff --git a/Makefile b/Makefile index 6ccd9e5bca..ab74b50e35 100644 --- a/Makefile +++ b/Makefile @@ -131,6 +131,7 @@ FC_VER_MINOR := $(shell grep " FC_VERSION_MINOR" src/main/build/version.h | awk FC_VER_PATCH := $(shell grep " FC_VERSION_PATCH" src/main/build/version.h | awk '{print $$3}' ) FC_VER := $(FC_VER_MAJOR).$(FC_VER_MINOR).$(FC_VER_PATCH) +FC_VER_SUFFIX ?= BUILD_DATE = $(shell date +%Y%m%d) @@ -266,13 +267,16 @@ CPPCHECK = cppcheck $(CSOURCES) --enable=all --platform=unix64 \ # # Things we will build # -ifeq ($(BUILD_SUFFIX),) -TARGET_BIN = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET).bin -TARGET_HEX = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET).hex -else -TARGET_BIN = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET)_$(BUILD_SUFFIX).bin -TARGET_HEX = $(BIN_DIR)/$(FORKNAME)_$(FC_VER)_$(TARGET)_$(BUILD_SUFFIX).hex +TARGET_BIN := $(BIN_DIR)/$(FORKNAME)_$(FC_VER) +ifneq ($(FC_VER_SUFFIX),) + TARGET_BIN := $(TARGET_BIN)-$(FC_VER_SUFFIX) endif +TARGET_BIN := $(TARGET_BIN)_$(TARGET) +ifneq ($(BUILD_SUFFIX),) + TARGET_BIN := $(TARGET_BIN)_$(BUILD_SUFFIX) +endif +TARGET_BIN := $(TARGET_BIN).bin +TARGET_HEX = $(TARGET_BIN:.bin=.hex) TARGET_OBJ_DIR = $(OBJECT_DIR)/$(TARGET) TARGET_ELF = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).elf @@ -386,7 +390,7 @@ clean: ## clean_test : clean up all temporary / machine-generated files (tests) clean_test: - $(V0) cd src/test && $(MAKE) clean || true + $(V0) cd src/test && $(MAKE) clean ## clean_ : clean up one specific target $(CLEAN_TARGETS) : @@ -461,7 +465,7 @@ targets: ## test : run the cleanflight test suite test: - $(V0) cd src/test && $(MAKE) test || true + $(V0) cd src/test && $(MAKE) test # rebuild everything when makefile changes # Make the generated files and the build stamp order only prerequisites, diff --git a/docs/Battery.md b/docs/Battery.md index 2ca9679184..e4c53ceafa 100644 --- a/docs/Battery.md +++ b/docs/Battery.md @@ -28,23 +28,42 @@ On the first battery connection is always advisable to use a current limiter dev See the [Sparky board chapter](Board - Sparky.md). -## Configuration +## Voltage measurement -Enable the `VBAT` feature. +Enable the `VBAT` feature to enable the measurement of the battery voltage and the use of the voltage based OSD battery gauge, voltage based and energy based battery alarms. -Configure min/max cell voltages using the following CLI setting: +### Calibration -`vbat_scale` - Adjust this to match actual measured battery voltage to reported value. +`vbat_scale` - Adjust this setting to match actual measured battery voltage to reported value. Increasing this value increases the measured voltage. -`vbat_max_cell_voltage` - Maximum voltage per cell, used for auto-detecting battery voltage in 0.01V units, i.e. 430 = 4.30V +### Voltage measurement source -`set vbat_warning_cell_voltage` - Warning voltage per cell; this triggers battery-out alarms, in 0.01V units, i.e. 340 = 3.40V +Two voltage sources are available: raw voltage and sag compensated voltage. The raw voltage is the voltage directly measured at the battery while the sag compensated voltage is calculated by an algorithm aiming to provide a stable voltage source for gauges, telemetry and alarms. When the current drawn from a battery varies the provided voltage also varies due to the internal resistance of the battery, it is called sag. The sag can often trigger the battery alarms before the battery is empty and if you are relying on the battery voltage to know the charge state of your battery you have to land or cut the throttle to know the real, without load, battery voltage. The sag compensation algorithm simulates a battery with zero internal resistance and provides a stable reading independent from the drawn current. -`vbat_min_cell_voltage` - Minimum voltage per cell; this triggers battery-out alarms, in 0.01V units, i.e. 330 = 3.30V +You can select the voltage source used for battery alarms and telemetry with the `bat_voltage_source` setting. It can be set to either `RAW` for using raw battery voltage or `SAG_COMP` for using the calculated sag compensated voltage. + +You can see an illustration of the sag compensation algorithm in action in the following graph: + +![Voltage graph](assets/images/sag_compensated_battery_voltage_plot.png) + +### Voltage based OSD gauge and alarms + +Up to 3 battery profiles are supported. You can select the battery profile from the GUI, OSD menu, [stick commands](Controls.md) and CLI command `battery_profile n`. Each profile stores the following voltage settings: + +`bat_cells` - Specify the number of cells of your battery. Allows the automatic selection of the battery profile when set to a value greater than 0. Set to 0 (default) for auto-detecting the number of cells (see next setting) + +`vbat_cell_detect_voltage` - Maximum voltage per cell, used for auto-detecting the number of cells of the battery. Should be higher than maximum cell voltage to take into account possible drift in measured voltage and keep cell count detection accurate (0.01V unit, i.e. 430 = 4.30V) + +`vbat_max_cell_voltage` - Maximum voltage per cell when the battery is fully charged. Used for the OSD voltage based battery gauge (0.01V unit, i.e. 420 = 4.20V) + +`vbat_warning_cell_voltage` - Cell warning voltage. A cell voltage bellow this value triggers the first (short beeps) voltage based battery alarm if used and also the blinking of the OSD voltage indicator if the battery capacity is not used instead (see bellow) (0.01V unit, i.e. 370 = 3.70V) + +`vbat_min_cell_voltage` - Cell minimum voltage. A cell voltage bellow this value triggers the second (long beeps) voltage based battery alarm if used and the OSD gauge will display 0% if the battery capacity is not used instead (see bellow) (0.01V unit, i.e. 350 = 3.50V) e.g. ``` +battery_profile 1 set vbat_scale = 1100 set vbat_max_cell_voltage = 430 set vbat_warning_cell_voltage = 340 @@ -172,3 +191,124 @@ set battery_capacity_critical = 440 // the battery critical alarm will sound ``` Note that in this example even though your warning capacity (`battery_capacity_warning`) is set to 30% (660mAh), since 440mAh (`battery_capacity_critical`) is considered empty (0% left), the OSD capacity related items will only start to blink when the remaining battery percentage shown on the OSD is below 12%: (`battery_capacity_warning`-`battery_capacity_critical`)*100/(`battery_capacity`-`battery_capacity_critical`)=(660-440)*100/(2200-440)=12.5 + + +## Battery profiles + +Up to 3 battery profiles are supported. You can select the battery profile from the GUI, OSD menu, [stick commands](Controls.md) and CLI command `battery_profile n`. Battery profiles store the following settings (see above for an explanation of each setting): `bat_cells`, `vbat_cell_detect_voltage`, `vbat_max_cell_voltage`, `vbat_warning_cell_voltage`, `vbat_min_cell_voltage`, `battery_capacity_unit`, `battery_capacity`, `battery_capacity_warning`, `battery_capacity_critical` + +To enable the automatic battery profile switching based on battery voltage enable the `BAT_PROF_AUTOSWITCH` feature. For a profile to be automatically selected the number of cells of the battery needs to be specified (>0). + +### Battery profiles configuration examples + +#### Simple example + +In this example we want to use two different type of batteries for the same aircraft and switch manually between them. The first battery is a Li-Po (4.20V/cell) and the second battery is a Li-Ion (4.10V/cell). + +``` +battery_profile 1 + +set bat_cells = 0 +set vbat_max_cell_voltage = 420 +set vbat_warning_cell_voltage = 370 +set vbat_min_cell_voltage = 340 + + +battery_profile 2 + +set bat_cells = 0 +set vbat_max_cell_voltage = 410 +set vbat_warning_cell_voltage = 280 +set vbat_min_cell_voltage = 250 +``` + +#### Simple example with automatic profile switching + +In this example we want to use two different batteries for the same aircraft and automatically switch between them when the battery is plugged in. The first battery is a Li-Po 2200mAh 3S and the second battery is a LiPo 1500mAh 4S. Since the iNav defaults for the cell detection voltage and max voltage are adequate for standard LiPo batteries they will not be modified. The warning and minimum voltage are not modified either in this example but you can set them to the value you like. Since we are using battery capacities only the warning voltage (kept at default in this example) will be used and only for triggering the battery voltage indicator blinking in the OSD. + +``` +feature BAT_PROF_AUTOSWITCH + + +battery_profile 1 + +set bat_cells = 3 +set battery_capacity_unit = MAH +set battery_capacity = 2200 +set battery_capacity_warning = 440 +set battery_capacity_critical = 220 + + +battery_profile 2 + +set bat_cells = 4 +set battery_capacity_unit = MAH +set battery_capacity = 1500 +set battery_capacity_warning = 300 +set battery_capacity_critical = 150 +``` + +#### Advanced automatic switching example + +Profile 1 is for a 3S 2200mAh Li-Po pack (max 4.20V/cell), profile 2 for a 3S 4000mAh Li-Ion pack (max 4.10V/cell) and profile 3 for a 4S 1500mAh Li-Po pack (max 4.20V/cell). +With this configuration if the battery plugged in is less than 12.36V (3 x 4.12) the profile 2 will be automatically selected else if the battery voltage is less than 12.66V (3 x 4.22) the profile 1 will be automatically selected else if the battery voltage is less 17.20V (4 x 4.3) the profile 3 will be automatically selected. If a matching profile can't be found the last selected profile is used. + +``` +feature BAT_PROF_AUTOSWITCH + + +battery_profile 1 + +set bat_cells = 3 +set vbat_cell_detect_voltage = 422 +set vbat_max_cell_voltage = 420 +set vbat_warning_cell_voltage = 350 +set vbat_min_cell_voltage = 330 +set battery_capacity = 2200 +set battery_capacity_warning = 440 +set battery_capacity_critical = 220 + + +battery_profile 2 + +set bat_cells = 3 +set vbat_cell_detect_voltage = 412 +set vbat_max_cell_voltage = 410 +set vbat_warning_cell_voltage = 300 +set vbat_min_cell_voltage = 280 +set battery_capacity = 4000 +set battery_capacity_warning = 800 +set battery_capacity_critical = 400 + + +battery_profile 3 + +set bat_cells = 4 +set vbat_cell_detect_voltage = 430 +set vbat_max_cell_voltage = 420 +set vbat_warning_cell_voltage = 350 +set vbat_min_cell_voltage = 330 +set battery_capacity = 1500 +set battery_capacity_warning = 300 +set battery_capacity_critical = 150 +``` + +## Remaining flight time and flight distance estimation + +The estimated remaining flight time and flight distance estimations can be displayed on the OSD (for fixed wing only for the moment). They are calculated from the GPS distance from home, remaining battery capacity and average power draw. They are taking into account the requested altitude change and heading to home change after altitude change following the switch to RTH. They are also taking into account the estimated wind if `osd_use_wind_compensation` is set to `ON`. When the timer and distance indicator reach 0 they will blink and you need to go home in a straight line manually or by engaging RTH. You should be left with at least `rth_energy_margin`% of battery left when arriving home if the cruise speed and power are set correctly (see bellow). + +To use this feature the following conditions need to be met: +- The `VBAT`, `CURRENT_METER` and `GPS` features need to be enabled +- The battery capacity needs to be specified in mWh (`battery_capacity` setting > 0 and `battery_capacity_unit` set to `MWH`) +- The average ground speed of the aircraft without wind at cruise throttle needs to be set (`nav_fw_cruise_speed` setting in cm/s) +- The average power draw at zero throttle needs to be specified (`idle_power` setting in 0.01W unit) +- The average power draw at cruise throttle needs to be specified (`cruise_power` setting in 0.01W unit) +- The battery needs to be full when plugged in (voltage >= (`vbat_max_cell_voltage` - 100mV) * cells) + +It is advised to set `nav_fw_cruise_speed` a bit lower than the real speed and `cruise_power` 10% higher than the power at cruise throttle to ensure variations in throttle during cruise won't cause the aircraft to draw more energy than estimated. + +If `---` is displayed during flight instead of the remaining flight time/distance it means at least one of the above conditions aren't met. If the OSD element is blinking and the digits are replaced by the horizontal wind symbol it means that the estimated horizontal wind is too strong to be able to return home at `nav_fw_cruise_speed`. + +## Automatic throttle compensation based on battery voltage + +Automatic throttle compensation based on battery voltage can be used by enabling the `THR_VBAT_COMP` feature. It is working like this: used_throttle = requested_throttle * battery_max_voltage / sag_compensated_voltage. diff --git a/docs/Board - Omnibus F4.md b/docs/Board - Omnibus F4.md index da5c2d7414..6871efadab 100644 --- a/docs/Board - Omnibus F4.md +++ b/docs/Board - Omnibus F4.md @@ -38,7 +38,7 @@ * PPM and UART6 can be used together when S.BUS jumper is removed (close to PPM/SBUS connector) * Uses target **OMNIBUSF4V3** -### Omnibus F4 v4 +### Omnibus F4 v4/v5 * Switching voltage regulator - solves problem of overheating BEC * SD Card slot instead of flash memory @@ -143,10 +143,14 @@ Please note that this is *not* the motor PWM5/PWM6 pins, but small surface mount | CH5 | RX | | CH6 | TX | -## SoftwareSerial - ![Omnibus F4 Pro SmartPort using SoftwareSerial](assets/images/omnibusf4pro_ss.png) +### Omnibus F4 v3/v4/v5 SoftwareSerial Connections + +The SOFTSERIAL1 is an uni-directional port mapped to UART6-TX pin. +When enabled, the UART6 is still available as hardware port but it's then RX-only port (good for e.g. receiving S.BUS input). TX instead is controlled in software (can be used for e.g. transmitting one-way telemetry). + + # Wiring diagrams for Omnibus F4 Pro Following diagrams applies to _Pro_ version with integrated current meter and JST connectors only diff --git a/docs/Cli.md b/docs/Cli.md index 4726c3c8d3..0ffd3425ec 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -101,7 +101,6 @@ Re-apply any new defaults as desired. | acc_task_frequency | 500 | Determines accelerometer task frequency in async_mode = ALL. Depending on UAV type this frequency can be lowered to conserve CPU resources as long as vibrations are not a problem. | | attitude_task_frequency | 250 | Determines attitude task frequency when async_mode = ALL | | async_mode | NONE | Enables asynchronous mode processing for gyro/accelerometer and attitude computations. Allowed modes: NONE -> default behavior, all calculations are executed in main PID loop. GYRO -> gyro sampling and filtering is detached from main PID loop. PID loop runs based on looptime while gyro sampling uses gyro_sync_denom and gyro_lpf combination to determine its frequency. ALL -> in this mode, gyro, accelerometer and attitude are running as separate tasks. Accelerometer task frequency is determined by acc_task_frequency, attitude task frequency by attitude_task_frequency. In this mode ANGLE and HORIZON, as well GPS assisted flight modes (including PosHold) performance might be lowered. | -| mid_rc | 1500 | This is an important number to set in order to avoid trimming receiver/transmitter. Most standard receivers will have this at 1500, however Futaba transmitters will need this set to 1520. A way to find out if this needs to be changed, is to clear all trim/subtrim on transmitter, and connect to GUI. Note the value most channels idle at - this should be the number to choose. Once midrc is set, use subtrim on transmitter to make sure all channels (except throttle of course) are centered at midrc value. | | min_check | 1100 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | | max_check | 1900 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | | rssi_channel | 0 | RX channel containing the RSSI signal | @@ -121,6 +120,7 @@ Re-apply any new defaults as desired. | 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 | | gps_provider | UBLOX | Which GPS protocol to be used | @@ -135,6 +135,7 @@ Re-apply any new defaults as desired. | 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] | | inav_w_z_baro_p | 0.350 | Weight of barometer measurements in estimated altitude and climb rate | | inav_w_z_gps_p | 0.200 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes | @@ -146,6 +147,7 @@ Re-apply any new defaults as desired. | inav_w_acc_bias | 0.010 | Weight for accelerometer drift estimation | | inav_max_eph_epv | 1000.000 | Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] | | inav_baro_epv | 100.000 | Uncertainty value for barometric sensor [cm] | +| 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 | @@ -168,17 +170,20 @@ Re-apply any new defaults as desired. | nav_rth_climb_ignore_emerg | OFF | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | | nav_rth_alt_mode | AT_LEAST | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details | | nav_rth_altitude | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | +| nav_rth_home_altitude | 0 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] | | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] | | nav_mc_bank_angle | 30 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude | | nav_mc_hover_thr | 1500 | Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering. | | nav_mc_auto_disarm_delay | 2000 | | | nav_fw_cruise_thr | 1400 | Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded ) | +| nav_fw_cruise_speed | 0 | Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s | +| nav_fw_allow_manual_thr_increase | OFF | Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_bank_angle | 20 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll | | nav_fw_climb_angle | 20 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit | | nav_fw_dive_angle | 15 | Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit | -| nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes | +| nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | | nav_fw_loiter_radius | 5000 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] | | nav_fw_launch_velocity | 300 | Forward velocity threshold for swing-launch detection [cm/s] | | nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | @@ -194,6 +199,7 @@ Re-apply any new defaults as desired. | nav_fw_launch_min_time | 0 | Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]. | | nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. | | nav_fw_land_dive_angle | 2 | Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees | +| nav_fw_cruise_yaw_rate | 20 | Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]| | serialrx_provider | SPEK1024 | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. | | serialrx_halfduplex | OFF | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire | | serialrx_inverted | OFF | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). | @@ -213,7 +219,10 @@ Re-apply any new defaults as desired. | ltm_update_rate | NORMAL | Defines the LTM update rate (use of bandwidth [NORMAL/MEDIUM/SLOW]). See Telemetry.md, LTM section for details. | | battery_capacity | 0 | Battery capacity in mAH. This value is used in conjunction with the current meter to determine remaining battery capacity. | | vbat_scale | 1100 | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | -| vbat_max_cell_voltage | 424 | Maximum voltage per cell, used for auto-detecting battery voltage in 0.01V units, default is 424 (4.24V) | +| bat_voltage_src | RAW | Chose between raw and sag compensated battery voltage to use for battery alarms and telemetry. Possible values are `RAW` and `SAG_COMP` | +| bat_cells | 0 | Number of cells of the battery (0 = autodetect), see battery documentation | +| vbat_cell_detect_voltage | 430 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V | +| vbat_max_cell_voltage | 420 | Maximum voltage per cell in 0.01V units, default is 4.20V | | vbat_min_cell_voltage | 330 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | | vbat_warning_cell_voltage | 350 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | | current_meter_scale | 400 | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt. | @@ -222,6 +231,9 @@ Re-apply any new defaults as desired. | battery_capacity_warning | 0 | If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink. | | battery_capacity_critical | 0 | If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps. | | battery_capacity_unit | MAH | Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour). | +| cruise_power | 0 | Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit | +| idle_power | 0 | Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit | +| rth_energy_margin | 5 | Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation | | multiwii_current_meter_output | OFF | Default current output via MSP is in 0.01A steps. Setting this to 1 causes output in default multiwii scaling (1mA steps) | | current_meter_type | ADC | ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. | | align_gyro | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | @@ -233,6 +245,7 @@ Re-apply any new defaults as desired. | align_mag_roll | 0 | Set the external mag alignment on the roll axis (in 0.1 degree steps). If this value is non-zero, the compass is assumed to be externally mounted and both the board and on-board compass alignent (align_mag) are ignored. See also align_mag_pitch and align_mag_yaw. | | align_mag_pitch | 0 | Same as align_mag_roll, but for the pitch axis. | | align_mag_yaw | 0 | Same as align_mag_roll, but for the yaw axis. | +| align_opflow | 5 | Optical flow module alignment (default CW0_DEG_FLIP) | | gyro_hardware_lpf | 42HZ | Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 10HZ,20HZ,42HZ,98HZ,188HZ,256Hz (8khz mode). If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first. | | moron_threshold | 32 | When powering up, gyro bias is calculated. If the model is shaking/moving during this initial calibration, offsets are calculated incorrectly, and could lead to poor flying performance. This threshold means how much average gyro reading could differ before re-calibration is triggered. | | imu_dcm_kp | 2500 | Inertial Measurement Unit KP Gain for accelerometer measurements | @@ -276,38 +289,18 @@ Re-apply any new defaults as desired. | blackbox_device | SPIFLASH | Selection of where to write blackbox data | | sdcard_detect_inverted | `TARGET dependent` | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. | | ledstrip_visual_beeper | OFF | | -| osd_video_system | 0 | | -| osd_row_shiftdown | 0 | | +| osd_video_system | AUTO | Video system used. Possible values are `AUTO`, `PAL` and `NTSC` | +| osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) | | osd_units | METRIC| IMPERIAL, METRIC, UK | | osd_stats_energy_unit | MAH | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | | osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | -| osd_wh_drawn_pos | | | -| osd_bat_remaining_capacity_pos | | | -| osd_bat_remaining_percent_pos | | | -| osd_efficiency_mah_pos | | | -| osd_efficiency_wh_pos | | | -| osd_rssi_alarm | 20 | | -| osd_time_alarm | 10 | | -| osd_alt_alarm | 100 | | -| osd_main_voltage_pos | 0 | | -| osd_rssi_pos | 0 | | -| osd_flytimer_pos | 0 | | -| osd_ontime_pos | 0 | | -| osd_flymode_pos | 0 | | -| osd_throttle_pos | 0 | | -| osd_vtx_channel_pos | 0 | | -| osd_crosshairs | 0 | | -| osd_artificial_horizon | 0 | | -| osd_current_draw_pos | 0 | | -| osd_mah_drawn_pos | 0 | | -| osd_craft_name_pos | 0 | | -| osd_gps_speed_pos | 0 | | -| osd_gps_sats_pos | 0 | | -| osd_altitude_pos | 0 | | -| osd_pid_roll_pos | 0 | | -| osd_pid_pitch_pos | 0 | | -| osd_pid_yaw_pos | 0 | | -| osd_power_pos | 0 | | +| osd_rssi_alarm | 20 | Value bellow which to make the OSD RSSI indicator blink | +| osd_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) | +| 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_estimations_wind_compensation | ON | Use wind estimation for remaining flight time/distance estimation | +| osd_attitude_angle_decimals | 0 | Chose the numbers of decimals displayed by OSD attitude angle elements [0-1] | | magzero_x | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | | magzero_y | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | | magzero_z | 0 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. | @@ -375,6 +368,7 @@ Re-apply any new defaults as desired. | fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point | | max_angle_inclination_rll | 300 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° | | max_angle_inclination_pit | 300 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° | +| 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 | | 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. | | 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 | | @@ -417,6 +411,15 @@ Re-apply any new defaults as desired. | model_preview_type | -1 | ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons. | | tz_offset | 0 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs | | tz_automatic_dst | OFF | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. | +| vtx_band | 4 | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | +| vtx_channel | 1 | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | +| vtx_freq | 5740 | Set the VTX frequency using raw MHz. This parameter is ignored unless `vtx_band` is 0. | +| vtx_halfduplex | ON | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | +| vtx_low_power_disarm | OFF | When the craft is disarmed, set the VTX to its lowest power. `ON` will set the power to its minimum value on startup, increase it to `vtx_power` when arming and change it back to its lowest setting after disarming. `UNTIL_FIRST_ARM` will start with minimum power, but once the craft is armed it will increase to `vtx_power` and it will never decrease until the craft is power cycled. | +| vtx_pit_mode_freq | Frequency to use (in MHz) when the VTX is in pit mode. | +| vtx_power | 1 | VTX RF power level to use. The exact number of mw depends on the VTX hardware. | +| motor_accel_time | 0 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] | +| motor_decel_time | 0 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] | This Markdown table is made by MarkdwonTableMaker addon for google spreadsheet. Original Spreadsheet used to make this table can be found here https://docs.google.com/spreadsheets/d/1ubjYdMGmZ2aAMUNYkdfe3hhIF7wRfIjcuPOi_ysmp00/edit?usp=sharing diff --git a/docs/Controls.md b/docs/Controls.md index 8581a17a5b..a1a1a8b3cc 100644 --- a/docs/Controls.md +++ b/docs/Controls.md @@ -28,6 +28,9 @@ The stick positions are combined to activate different functions: | Profile 1 | LOW | LOW | CENTER | LOW | | Profile 2 | LOW | LOW | HIGH | CENTER | | Profile 3 | LOW | LOW | CENTER | HIGH | +| Battery profile 1 | HIGH | LOW | CENTER | LOW | +| Battery profile 2 | HIGH | LOW | HIGH | CENTER | +| Battery profile 3 | HIGH | LOW | CENTER | HIGH | | Calibrate Gyro | LOW | LOW | LOW | CENTER | | Calibrate Acc | HIGH | LOW | LOW | CENTER | | Calibrate Mag/Compass | HIGH | HIGH | LOW | CENTER | diff --git a/docs/assets/images/StickPositions.png b/docs/assets/images/StickPositions.png index a3feac95ed..658423f595 100644 Binary files a/docs/assets/images/StickPositions.png and b/docs/assets/images/StickPositions.png differ diff --git a/docs/assets/images/StickPositions.svg b/docs/assets/images/StickPositions.svg index 724282d151..d6a98e7af3 100644 --- a/docs/assets/images/StickPositions.svg +++ b/docs/assets/images/StickPositions.svg @@ -10,11 +10,11 @@ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd" xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape" width="2478.5964mm" - height="1086.9835mm" - viewBox="0 0 8782.4284 3851.5164" + height="1400.9835mm" + viewBox="0 0 8782.4284 4964.1146" id="svg2" version="1.1" - inkscape:version="0.91 r13725" + inkscape:version="0.92.2 (5c3e80d, 2017-08-06)" sodipodi:docname="StickPositions.svg" inkscape:export-filename="C:\Users\stuphi\Dropbox\projects\quad\StickPositions.png" inkscape:export-xdpi="74.996788" @@ -29,22 +29,27 @@ inkscape:pageopacity="0.0" inkscape:pageshadow="2" inkscape:zoom="0.08" - inkscape:cx="2990.2403" - inkscape:cy="3041.654" + inkscape:cx="3126.7378" + inkscape:cy="2663.9858" inkscape:document-units="px" inkscape:current-layer="g4157" showgrid="false" inkscape:window-width="1920" - inkscape:window-height="1137" - inkscape:window-x="-8" - inkscape:window-y="-8" + inkscape:window-height="1103" + inkscape:window-x="1680" + inkscape:window-y="27" inkscape:window-maximized="1" fit-margin-top="1" fit-margin-left="1" fit-margin-right="1" fit-margin-bottom="1" units="mm" - showborder="true" /> + showborder="true" + showguides="true" + inkscape:guide-bbox="true" + inkscape:snap-object-midpoints="true" + inkscape:snap-others="false" + inkscape:object-nodes="true" /> @@ -61,81 +66,81 @@ inkscape:label="Frame" inkscape:groupmode="layer" id="layer1" - transform="translate(2203.3186,1653.7717)" + transform="translate(2203.3186,2766.3699)" style="display:inline" /> + transform="translate(2203.3186,3074.6376)" /> + transform="translate(2203.3186,3074.6376)" /> + transform="translate(2203.3186,3074.6376)" /> + transform="translate(2203.3186,3074.6376)" /> + transform="translate(2203.3186,3074.6376)" /> + transform="translate(2203.3186,3074.6376)" /> + transform="translate(2203.3186,3074.6376)" /> + transform="translate(2203.3186,3074.6376)" /> + transform="translate(2203.3186,3074.6376)"> Arm + style="font-size:286.5987854px;line-height:1.25;text-align:end;text-anchor:end">Arm @@ -181,29 +185,28 @@ width="288.94339" height="288.94339" x="1114.0997" - y="-969.96918" + y="-1942.6254" ry="0" /> Disarm @@ -223,27 +226,26 @@ width="288.94339" height="288.94339" x="1114.0997" - y="-549.56104" + y="-1522.2173" ry="0" /> Profile 1 Profile 2 + style="font-size:286.5987854px;line-height:1.25;text-align:end;text-anchor:end">Profile 2 @@ -309,25 +310,24 @@ width="288.94339" height="288.94339" x="1114.0997" - y="291.25531" + y="-681.40088" ry="0" /> Profile 3 Calibrate Gyro + style="font-size:286.5987854px;line-height:1.25;text-align:end;text-anchor:end">Calibrate Gyro Calibrate Acc + style="font-size:286.5987854px;line-height:1.25;text-align:end;text-anchor:end">Calibrate Acc @@ -434,30 +432,29 @@ id="rect4949" width="288.94339" height="288.94339" - x="1114.0997" - y="1552.4799" + x="6241.2427" + y="-2372.1223" ry="0" /> Calibrate Compass @@ -477,26 +474,25 @@ width="288.94339" height="288.94339" x="6241.2427" - y="-1390.3773" + y="-1964.5959" ry="0" /> In-flight Calibration Controls Trim Acc Left + style="font-size:286.5987854px;line-height:1.25;text-align:end;text-anchor:end">Trim Acc Left Trim Acc Right + style="font-size:286.5987854px;line-height:1.25;text-align:end;text-anchor:end">Trim Acc Right @@ -601,24 +595,23 @@ width="288.94339" height="288.94339" x="6241.2427" - y="-129.15286" + y="-703.37158" ry="0" /> Trim Acc Forwards Trim Acc Backwards + style="font-size:286.5987854px;line-height:1.25;text-align:end;text-anchor:end">Trim Acc Backwards Save waypoint mission Load waypoint mission Save Setting + style="font-size:286.5987854px;line-height:1.25;text-align:end;text-anchor:end">Save Setting Mode 2 Stick Functions + style="font-size:352.85842896px;line-height:1.25;text-align:center;text-anchor:middle">Mode 2 Stick Functions + + + + Battery profile 1 + + + Battery profile 2 + + + Battery profile 3 + + + + + diff --git a/docs/assets/images/sag_compensated_battery_voltage_plot.png b/docs/assets/images/sag_compensated_battery_voltage_plot.png new file mode 100644 index 0000000000..cc72e00a76 Binary files /dev/null and b/docs/assets/images/sag_compensated_battery_voltage_plot.png differ diff --git a/make/source.mk b/make/source.mk index e1a76eea0f..8fccace041 100644 --- a/make/source.mk +++ b/make/source.mk @@ -74,6 +74,7 @@ COMMON_SRC = \ flight/mixer.c \ flight/pid.c \ flight/pid_autotune.c \ + flight/rth_estimator.c \ flight/servos.c \ flight/wind_estimator.c \ io/beeper.c \ @@ -123,6 +124,7 @@ COMMON_SRC = \ blackbox/blackbox_encoding.c \ blackbox/blackbox_io.c \ cms/cms.c \ + cms/cms_menu_battery.c \ cms/cms_menu_blackbox.c \ cms/cms_menu_builtin.c \ cms/cms_menu_imu.c \ @@ -163,12 +165,15 @@ COMMON_SRC = \ navigation/navigation_geo.c \ navigation/navigation_multicopter.c \ navigation/navigation_pos_estimator.c \ + navigation/navigation_pos_estimator_agl.c \ + navigation/navigation_pos_estimator_flow.c \ sensors/barometer.c \ sensors/pitotmeter.c \ sensors/rangefinder.c \ sensors/opflow.c \ telemetry/crsf.c \ telemetry/frsky.c \ + telemetry/frsky_d.c \ telemetry/hott.c \ telemetry/ibus_shared.c \ telemetry/ibus.c \ @@ -177,6 +182,7 @@ COMMON_SRC = \ telemetry/msp_shared.c \ telemetry/smartport.c \ telemetry/telemetry.c \ + io/vtx.c \ io/vtx_string.c \ io/vtx_smartaudio.c \ io/vtx_tramp.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 5f9823e0f9..5bb6d18fac 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -193,6 +193,34 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"axisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)}, {"axisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)}, {"axisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)}, + {"fwAltP", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, + {"fwAltI", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, + {"fwAltD", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, + {"fwAltOut", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, + {"fwPosP", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, + {"fwPosI", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, + {"fwPosD", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, + {"fwPosOut", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, + {"mcPosAxisP", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcPosAxisP", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcPosAxisP", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcVelAxisP", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcVelAxisP", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcVelAxisP", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcVelAxisI", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcVelAxisI", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcVelAxisI", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"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)}, + {"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)}, + {"mcSurfaceP", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcSurfaceI", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcSurfaceD", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + {"mcSurfaceOut",-1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)}, + /* rcData are encoded together as a group: */ {"rcData", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, {"rcData", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, @@ -309,6 +337,8 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { {"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)}, {"hwHealthStatus", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"powerSupplyImpedance", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"sagCompensatedVBat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, }; typedef enum BlackboxState { @@ -337,6 +367,18 @@ typedef struct blackboxMainState_s { int32_t axisPID_D[XYZ_AXIS_COUNT]; int32_t axisPID_Setpoint[XYZ_AXIS_COUNT]; + int32_t mcPosAxisP[XYZ_AXIS_COUNT]; + int32_t mcVelAxisPID[3][XYZ_AXIS_COUNT]; + int32_t mcVelAxisOutput[XYZ_AXIS_COUNT]; + + int32_t mcSurfacePID[3]; + int32_t mcSurfacePIDOutput; + + int32_t fwAltPID[3]; + int32_t fwAltPIDOutput; + int32_t fwPosPID[3]; + int32_t fwPosPIDOutput; + int16_t rcData[4]; int16_t rcCommand[4]; int16_t gyroADC[XYZ_AXIS_COUNT]; @@ -392,6 +434,8 @@ typedef struct blackboxSlowState_s { bool rxSignalReceived; bool rxFlightChannelsValid; int32_t hwHealthStatus; + uint16_t powerSupplyImpedance; + uint16_t sagCompensatedVBat; } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() //From rc_controls.c @@ -506,7 +550,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) return feature(FEATURE_VBAT); case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC: - return feature(FEATURE_CURRENT_METER) && batteryConfig()->current.type == CURRENT_SENSOR_ADC; + return feature(FEATURE_CURRENT_METER) && batteryMetersConfig()->current.type == CURRENT_SENSOR_ADC; case FLIGHT_LOG_FIELD_CONDITION_SURFACE: #ifdef USE_RANGEFINDER @@ -515,6 +559,20 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) return false; #endif + case FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV: +#ifdef USE_NAV + return STATE(FIXED_WING); +#else + return false; +#endif + + case FLIGHT_LOG_FIELD_CONDITION_MC_NAV: +#ifdef USE_NAV + return !STATE(FIXED_WING); +#else + return false; +#endif + case FLIGHT_LOG_FIELD_CONDITION_RSSI: return rxConfig()->rssi_channel > 0 || feature(FEATURE_RSSI_ADC); @@ -601,6 +659,27 @@ static void writeIntraframe(void) } } + if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) { + blackboxWriteSignedVBArray(blackboxCurrent->fwAltPID, 3); + blackboxWriteSignedVB(blackboxCurrent->fwAltPIDOutput); + blackboxWriteSignedVBArray(blackboxCurrent->fwPosPID, 3); + blackboxWriteSignedVB(blackboxCurrent->fwPosPIDOutput); + } + + if (testBlackboxCondition(CONDITION(MC_NAV))) { + + blackboxWriteSignedVBArray(blackboxCurrent->mcPosAxisP, XYZ_AXIS_COUNT); + + for (int i = 0; i < 3; i++) { + blackboxWriteSignedVBArray(blackboxCurrent->mcVelAxisPID[i], XYZ_AXIS_COUNT); + } + + blackboxWriteSignedVBArray(blackboxCurrent->mcVelAxisOutput, XYZ_AXIS_COUNT); + + blackboxWriteSignedVBArray(blackboxCurrent->mcSurfacePID, 3); + blackboxWriteSignedVB(blackboxCurrent->mcSurfacePIDOutput); + } + // Write raw stick positions blackboxWriteSigned16VBArray(blackboxCurrent->rcData, 4); @@ -773,6 +852,38 @@ static void writeInterframe(void) } } + if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) { + + arraySubInt32(deltas, blackboxCurrent->fwAltPID, blackboxLast->fwAltPID, 3); + blackboxWriteSignedVBArray(deltas, 3); + + blackboxWriteSignedVB(blackboxCurrent->fwAltPIDOutput); + + arraySubInt32(deltas, blackboxCurrent->fwPosPID, blackboxLast->fwPosPID, 3); + blackboxWriteSignedVBArray(deltas, 3); + + blackboxWriteSignedVB(blackboxCurrent->fwPosPIDOutput); + + } + + if (testBlackboxCondition(CONDITION(MC_NAV))) { + arraySubInt32(deltas, blackboxCurrent->mcPosAxisP, blackboxLast->mcPosAxisP, XYZ_AXIS_COUNT); + blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); + + for (int i = 0; i < XYZ_AXIS_COUNT; i++) { + arraySubInt32(deltas, blackboxCurrent->mcVelAxisPID[i], blackboxLast->mcVelAxisPID[i], XYZ_AXIS_COUNT); + blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); + } + + arraySubInt32(deltas, blackboxCurrent->mcVelAxisOutput, blackboxLast->mcVelAxisOutput, XYZ_AXIS_COUNT); + blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); + + arraySubInt32(deltas, blackboxCurrent->mcSurfacePID, blackboxLast->mcSurfacePID, 3); + blackboxWriteSignedVBArray(deltas, 3); + + blackboxWriteSignedVB(blackboxCurrent->mcSurfacePIDOutput); + } + /* * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that * can pack multiple values per byte: @@ -907,6 +1018,9 @@ static void writeSlowFrame(void) blackboxWriteUnsignedVB(slowHistory.hwHealthStatus); + blackboxWriteUnsignedVB(slowHistory.powerSupplyImpedance); + blackboxWriteUnsignedVB(slowHistory.sagCompensatedVBat); + blackboxSlowFrameIterationTimer = 0; } @@ -927,6 +1041,8 @@ static void loadSlowState(blackboxSlowState_t *slow) (getHwGPSStatus() << 2 * 4) | (getHwRangefinderStatus() << 2 * 5) | (getHwPitotmeterStatus() << 2 * 6); + slow->powerSupplyImpedance = getPowerSupplyImpedance(); + slow->sagCompensatedVBat = getBatterySagCompensatedVoltage(); } /** @@ -1102,7 +1218,7 @@ static void writeGPSFrame(timeUs_t currentTimeUs) blackboxWriteUnsignedVB(gpsSol.numSat); blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[0]); blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[1]); - blackboxWriteUnsignedVB(gpsSol.llh.alt / 100); // meters + blackboxWriteSignedVB(gpsSol.llh.alt / 100); // meters blackboxWriteUnsignedVB(gpsSol.groundSpeed); blackboxWriteUnsignedVB(gpsSol.groundCourse); blackboxWriteUnsignedVB(gpsSol.hdop); @@ -1124,6 +1240,10 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->time = currentTimeUs; +#ifdef USE_NAV + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); +#endif + for (int i = 0; i < XYZ_AXIS_COUNT; i++) { blackboxCurrent->axisPID_Setpoint[i] = axisPID_Setpoint[i]; blackboxCurrent->axisPID_P[i] = axisPID_P[i]; @@ -1133,9 +1253,36 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->accADC[i] = lrintf(acc.accADCf[i] * acc.dev.acc_1G); #ifdef USE_MAG blackboxCurrent->magADC[i] = mag.magADC[i]; +#endif +#ifdef USE_NAV + if (!STATE(FIXED_WING)) { + blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained / 10); + blackboxCurrent->mcVelAxisPID[0][i] = lrintf(nav_pids->vel[i].proportional / 10); + blackboxCurrent->mcVelAxisPID[1][i] = lrintf(nav_pids->vel[i].integral / 10); + blackboxCurrent->mcVelAxisPID[2][i] = lrintf(nav_pids->vel[i].derivative / 10); + } #endif } +#ifdef USE_NAV + if (STATE(FIXED_WING)) { + blackboxCurrent->fwAltPID[0] = lrintf(nav_pids->fw_alt.proportional / 10); + blackboxCurrent->fwAltPID[1] = lrintf(nav_pids->fw_alt.integral / 10); + blackboxCurrent->fwAltPID[2] = lrintf(nav_pids->fw_alt.derivative / 10); + blackboxCurrent->fwAltPIDOutput = lrintf(nav_pids->fw_alt.output_constrained / 10); + + blackboxCurrent->fwPosPID[0] = lrintf(nav_pids->fw_nav.proportional / 10); + blackboxCurrent->fwPosPID[1] = lrintf(nav_pids->fw_nav.integral / 10); + blackboxCurrent->fwPosPID[2] = lrintf(nav_pids->fw_nav.derivative / 10); + blackboxCurrent->fwPosPIDOutput = lrintf(nav_pids->fw_nav.output_constrained / 10); + } else { + blackboxCurrent->mcSurfacePID[0] = lrintf(nav_pids->surface.proportional / 10); + blackboxCurrent->mcSurfacePID[1] = lrintf(nav_pids->surface.integral / 10); + blackboxCurrent->mcSurfacePID[2] = lrintf(nav_pids->surface.derivative / 10); + blackboxCurrent->mcSurfacePIDOutput = lrintf(nav_pids->surface.output_constrained / 10); + } +#endif + for (int i = 0; i < 4; i++) { blackboxCurrent->rcData[i] = rcData[i]; blackboxCurrent->rcCommand[i] = rcCommand[i]; @@ -1355,21 +1502,21 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE_CUSTOM( if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { - blackboxPrintfHeaderLine("vbat_scale", "%u", batteryConfig()->voltage.scale / 10); + blackboxPrintfHeaderLine("vbat_scale", "%u", batteryMetersConfig()->voltage_scale / 10); } else { xmitState.headerIndex += 2; // Skip the next two vbat fields too } ); - BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u", batteryConfig()->voltage.cellMin / 10, - batteryConfig()->voltage.cellWarning / 10, - batteryConfig()->voltage.cellMax / 10); + BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u", currentBatteryProfile->voltage.cellMin / 10, + currentBatteryProfile->voltage.cellWarning / 10, + currentBatteryProfile->voltage.cellMax / 10); BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference); BLACKBOX_PRINT_HEADER_LINE_CUSTOM( //Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too: if (feature(FEATURE_CURRENT_METER)) { - blackboxPrintfHeaderLine("currentMeter", "%d,%d", batteryConfig()->current.offset, - batteryConfig()->current.scale); + blackboxPrintfHeaderLine("currentMeter", "%d,%d", batteryMetersConfig()->current.offset, + batteryMetersConfig()->current.scale); } ); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 6e20a46b5b..189e5828e1 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -40,6 +40,8 @@ typedef enum FlightLogFieldCondition { FLIGHT_LOG_FIELD_CONDITION_VBAT, FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC, FLIGHT_LOG_FIELD_CONDITION_SURFACE, + FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV, + FLIGHT_LOG_FIELD_CONDITION_MC_NAV, FLIGHT_LOG_FIELD_CONDITION_RSSI, FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0, diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 6843ffd2ef..47cdd5557d 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -16,6 +16,8 @@ */ #include +#include +#include #define DEBUG16_VALUE_COUNT 4 extern int16_t debug[DEBUG16_VALUE_COUNT]; @@ -54,12 +56,17 @@ typedef enum { DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, DEBUG_AGL, DEBUG_FLOW_RAW, + DEBUG_FLOW, DEBUG_SBUS, DEBUG_FPORT, DEBUG_ALWAYS, DEBUG_STAGE2, DEBUG_WIND_ESTIMATOR, DEBUG_SAG_COMP_VOLTAGE, + DEBUG_VIBE, + DEBUG_CRUISE, + DEBUG_REM_FLIGHT_TIME, + DEBUG_SMARTAUDIO, DEBUG_COUNT } debugType_e; diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c index bc00a960ec..e85b37e745 100644 --- a/src/main/cms/cms.c +++ b/src/main/cms/cms.c @@ -430,7 +430,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t const OSD_SETTING_t *ptr = p->data; const setting_t *var = &settingsTable[ptr->val]; int32_t value; - const void *valuePointer = setting_get_value_pointer(var); + const void *valuePointer = settingGetValuePointer(var); switch (SETTING_TYPE(var)) { case VAR_UINT8: value = *(uint8_t *)valuePointer; @@ -453,6 +453,9 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, const OSD_Entry *p, uint8_t // a data type yet. ftoa(*(float *)valuePointer, buff); break; + case VAR_STRING: + strncpy(buff, valuePointer, sizeof(buff)); + break; } if (buff[0] == '\0') { const char *suffix = NULL; @@ -1006,13 +1009,13 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) if (p->data) { const OSD_SETTING_t *ptr = p->data; const setting_t *var = &settingsTable[ptr->val]; - setting_min_t min = setting_get_min(var); - setting_max_t max = setting_get_max(var); + setting_min_t min = settingGetMin(var); + setting_max_t max = settingGetMax(var); float step = ptr->step ?: 1; if (key != KEY_RIGHT) { step = -step; } - const void *valuePointer = setting_get_value_pointer(var); + const void *valuePointer = settingGetValuePointer(var); switch (SETTING_TYPE(var)) { case VAR_UINT8: { @@ -1057,6 +1060,8 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key) break; } break; + case VAR_STRING: + break; } SET_PRINTVALUE(p, currentCtx.cursorRow); if (p->func) { diff --git a/src/main/cms/cms_menu_battery.c b/src/main/cms/cms_menu_battery.c new file mode 100644 index 0000000000..fd6efaa304 --- /dev/null +++ b/src/main/cms/cms_menu_battery.c @@ -0,0 +1,152 @@ +/* + * 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" + +#ifdef USE_CMS + +#include "common/utils.h" + +#include "config/feature.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" +#include "cms/cms_menu_misc.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/settings.h" + +#include "sensors/battery.h" + +// Battery menu + +static uint8_t battDispProfileIndex; +static uint8_t battProfileIndex; +static bool featureProfAutoswitchEnabled; +static char battProfileIndexString[] = " p"; + + +static long cmsx_menuBattery_onEnter(const OSD_Entry *from) +{ + UNUSED(from); + + battProfileIndex = getConfigBatteryProfile(); + battDispProfileIndex = battProfileIndex + 1; + battProfileIndexString[1] = '0' + battDispProfileIndex; + featureProfAutoswitchEnabled = feature(FEATURE_BAT_PROFILE_AUTOSWITCH); + + return 0; +} + +static long cmsx_menuBattery_onExit(const OSD_Entry *self) +{ + UNUSED(self); + + setConfigBatteryProfile(battProfileIndex); + activateBatteryProfile(); + + if (featureProfAutoswitchEnabled) { + featureSet(FEATURE_BAT_PROFILE_AUTOSWITCH); + } else { + featureClear(FEATURE_BAT_PROFILE_AUTOSWITCH); + } + + return 0; +} + +static long cmsx_onBatteryProfileIndexChange(displayPort_t *displayPort, const void *ptr) +{ + UNUSED(displayPort); + UNUSED(ptr); + + battProfileIndex = battDispProfileIndex - 1; + battProfileIndexString[1] = '0' + battDispProfileIndex; + batteryDisableProfileAutoswitch(); + + return 0; +} + +static long cmsx_menuBattSettings_onEnter(const OSD_Entry *from) +{ + UNUSED(from); + + setConfigBatteryProfile(battProfileIndex); + + return 0; +} + +static const OSD_Entry menuBattSettingsEntries[]= +{ + OSD_LABEL_DATA_ENTRY("-- BATT SETTINGS --", battProfileIndexString), + +#ifdef USE_ADC + OSD_SETTING_ENTRY("CELLS", SETTING_BAT_CELLS), + OSD_SETTING_ENTRY("CELL DET.", SETTING_VBAT_CELL_DETECT_VOLTAGE), + OSD_SETTING_ENTRY("CELL MAX", SETTING_VBAT_MAX_CELL_VOLTAGE), + OSD_SETTING_ENTRY("CELL WARN", SETTING_VBAT_WARNING_CELL_VOLTAGE), + OSD_SETTING_ENTRY("CELL MIN", SETTING_VBAT_MIN_CELL_VOLTAGE), +#endif /* USE_ADC */ + OSD_SETTING_ENTRY("CAP UNIT", SETTING_BATTERY_CAPACITY_UNIT), + OSD_SETTING_ENTRY("CAPACITY", SETTING_BATTERY_CAPACITY), + OSD_SETTING_ENTRY("CAP WARN", SETTING_BATTERY_CAPACITY_WARNING), + OSD_SETTING_ENTRY("CAP CRIT", SETTING_BATTERY_CAPACITY_CRITICAL), + + OSD_BACK_ENTRY, + OSD_END_ENTRY +}; + +static CMS_Menu cmsx_menuBattSettings = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XBATT", + .GUARD_type = OME_MENU, +#endif + .onEnter = cmsx_menuBattSettings_onEnter, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = menuBattSettingsEntries +}; + +static OSD_Entry menuBatteryEntries[]= +{ + OSD_LABEL_ENTRY("-- BATTERY --"), + +#ifdef USE_ADC + OSD_BOOL_ENTRY("PROF AUTOSWITCH", &featureProfAutoswitchEnabled), +#endif + 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 +}; + +CMS_Menu cmsx_menuBattery = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XBATT", + .GUARD_type = OME_MENU, +#endif + .onEnter = cmsx_menuBattery_onEnter, + .onExit = cmsx_menuBattery_onExit, + .onGlobalExit = NULL, + .entries = menuBatteryEntries +}; + +#endif // CMS diff --git a/src/main/cms/cms_menu_battery.h b/src/main/cms/cms_menu_battery.h new file mode 100644 index 0000000000..ca4e10643b --- /dev/null +++ b/src/main/cms/cms_menu_battery.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 + +extern CMS_Menu cmsx_menuBattery; diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c index 5d759d8546..cab1fe8737 100644 --- a/src/main/cms/cms_menu_blackbox.c +++ b/src/main/cms/cms_menu_blackbox.c @@ -39,6 +39,7 @@ #include "drivers/time.h" #include "fc/config.h" +#include "fc/settings.h" #include "io/flashfs.h" diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c index 08f9c403a7..32a9dd68ab 100644 --- a/src/main/cms/cms_menu_builtin.c +++ b/src/main/cms/cms_menu_builtin.c @@ -46,6 +46,7 @@ #include "cms/cms_menu_vtx.h" #include "cms/cms_menu_osd.h" #include "cms/cms_menu_ledstrip.h" +#include "cms/cms_menu_battery.h" #include "cms/cms_menu_misc.h" // VTX supplied menus @@ -111,11 +112,11 @@ static const OSD_Entry menuFeaturesEntries[] = #if defined(VTX) || defined(USE_RTC6705) OSD_SUBMENU_ENTRY("VTX", &cmsx_menuVtx), #endif // VTX || USE_RTC6705 -#if defined(VTX_CONTROL) -#if defined(VTX_SMARTAUDIO) +#if defined(USE_VTX_CONTROL) +#if defined(USE_VTX_SMARTAUDIO) OSD_SUBMENU_ENTRY("VTX SA", &cmsx_menuVtxSmartAudio), #endif -#if defined(VTX_TRAMP) +#if defined(USE_VTX_TRAMP) OSD_SUBMENU_ENTRY("VTX TR", &cmsx_menuVtxTramp), #endif #endif // VTX_CONTROL @@ -150,6 +151,7 @@ static const OSD_Entry menuMainEntries[] = OSD_SUBMENU_ENTRY("OSD LAYOUTS", &cmsx_menuOsdLayout), OSD_SUBMENU_ENTRY("ALARMS", &cmsx_menuAlarms), #endif + OSD_SUBMENU_ENTRY("BATTERY", &cmsx_menuBattery), OSD_SUBMENU_ENTRY("FC&FW INFO", &menuInfo), OSD_SUBMENU_ENTRY("MISC", &cmsx_menuMisc), diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index a2d1bbff00..25accaa35c 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -39,6 +39,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/rc_controls.h" +#include "fc/settings.h" #include "navigation/navigation.h" @@ -77,15 +78,6 @@ static long cmsx_menuImu_onEnter(const OSD_Entry *from) return 0; } -static long cmsx_menuImu_onExit(const OSD_Entry *self) -{ - UNUSED(self); - - setConfigProfile(profileIndex); - - return 0; -} - static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *ptr) { UNUSED(displayPort); @@ -93,6 +85,7 @@ static long cmsx_profileIndexOnChange(displayPort_t *displayPort, const void *pt profileIndex = tmpProfileIndex - 1; profileIndexString[1] = '0' + tmpProfileIndex; + setConfigProfile(profileIndex); return 0; } @@ -483,7 +476,7 @@ const CMS_Menu cmsx_menuImu = { .GUARD_type = OME_MENU, #endif .onEnter = cmsx_menuImu_onEnter, - .onExit = cmsx_menuImu_onExit, + .onExit = NULL, .onGlobalExit = NULL, .entries = cmsx_menuImuEntries, }; diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index e597f88c09..7a8b5af430 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -32,6 +32,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/settings.h" #include "rx/rx.h" @@ -86,19 +87,10 @@ static const OSD_Entry menuMiscEntries[]= OSD_LABEL_ENTRY("-- MISC --"), OSD_SETTING_ENTRY("MIN THR", SETTING_MIN_THROTTLE), -#ifdef USE_ADC - OSD_SETTING_ENTRY("VBATCELL MAX", SETTING_VBAT_MAX_CELL_VOLTAGE), - OSD_SETTING_ENTRY("VBATCELL WARN", SETTING_VBAT_WARNING_CELL_VOLTAGE), - OSD_SETTING_ENTRY("VBATCELL MIN", SETTING_VBAT_MIN_CELL_VOLTAGE), - OSD_SETTING_ENTRY("BAT CAP UNIT", SETTING_BATTERY_CAPACITY_UNIT), - OSD_SETTING_ENTRY("BAT CAPACITY", SETTING_BATTERY_CAPACITY), - OSD_SETTING_ENTRY("BAT CAP WARN", SETTING_BATTERY_CAPACITY_WARNING), - OSD_SETTING_ENTRY("BAT CAP CRIT", SETTING_BATTERY_CAPACITY_CRITICAL), -#ifdef USE_OSD +#if defined(USE_OSD) && defined(USE_ADC) OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS), OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT), -#endif /* USE_OSD */ -#endif /* USE_ADC */ +#endif OSD_SUBMENU_ENTRY("RC PREV", &cmsx_menuRcPreview), diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 152255d0a7..e7d6f7eda9 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -35,6 +35,7 @@ #include "cms/cms_types.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "navigation/navigation.h" diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 6a4af060ac..b0058d04da 100755 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -150,19 +150,23 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("MAIN BATTERY", OSD_MAIN_BATT_VOLTAGE), OSD_ELEMENT_ENTRY("MAIN BATT SC", OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE), OSD_ELEMENT_ENTRY("CELL VOLTAGE", OSD_MAIN_BATT_CELL_VOLTAGE), + OSD_ELEMENT_ENTRY("CELL VOLT. SC", OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE), + OSD_ELEMENT_ENTRY("BAT IMPEDANCE", OSD_POWER_SUPPLY_IMPEDANCE), OSD_ELEMENT_ENTRY("CROSSHAIRS", OSD_CROSSHAIRS), OSD_ELEMENT_ENTRY("HORIZON", OSD_ARTIFICIAL_HORIZON), OSD_ELEMENT_ENTRY("HORIZON SIDEBARS", OSD_HORIZON_SIDEBARS), OSD_ELEMENT_ENTRY("ON TIME", OSD_ONTIME), OSD_ELEMENT_ENTRY("FLY TIME", OSD_FLYTIME), OSD_ELEMENT_ENTRY("ON/FLY TIME", OSD_ONTIME_FLYTIME), + OSD_ELEMENT_ENTRY("REM. TIME", OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH), + OSD_ELEMENT_ENTRY("REM. DIST", OSD_REMAINING_DISTANCE_BEFORE_RTH), OSD_ELEMENT_ENTRY("TIME (HOUR)", OSD_RTC_TIME), OSD_ELEMENT_ENTRY("FLY MODE", OSD_FLYMODE), OSD_ELEMENT_ENTRY("NAME", OSD_CRAFT_NAME), OSD_ELEMENT_ENTRY("THR. (MANU)", OSD_THROTTLE_POS), OSD_ELEMENT_ENTRY("THR. (MANU/AUTO)", OSD_THROTTLE_POS_AUTO_THR), OSD_ELEMENT_ENTRY("SYS MESSAGES", OSD_MESSAGES), -#ifdef VTX_COMMON +#ifdef USE_VTX_COMMON OSD_ELEMENT_ENTRY("VTX CHAN", OSD_VTX_CHANNEL), #endif // VTX OSD_ELEMENT_ENTRY("CURRENT (A)", OSD_CURRENT_DRAW), @@ -175,6 +179,7 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("BATT % REM", OSD_BATTERY_REMAINING_PERCENT), #ifdef USE_GPS OSD_ELEMENT_ENTRY("HOME DIR", OSD_HOME_DIR), + OSD_ELEMENT_ENTRY("HOME HEAD. ERR", OSD_HOME_HEADING_ERROR), OSD_ELEMENT_ENTRY("HOME DIST", OSD_HOME_DIST), OSD_ELEMENT_ENTRY("TRIP DIST", OSD_TRIP_DIST), OSD_ELEMENT_ENTRY("GPS SPEED", OSD_GPS_SPEED), @@ -185,6 +190,8 @@ static const OSD_Entry menuOsdElemsEntries[] = #endif // GPS OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING), OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH), + OSD_ELEMENT_ENTRY("CRS HEAD. ERR", OSD_CRUISE_HEADING_ERROR), + OSD_ELEMENT_ENTRY("CRS HEAD. ADJ", OSD_CRUISE_HEADING_ADJUSTMENT), #if defined(USE_BARO) || defined(USE_GPS) OSD_ELEMENT_ENTRY("VARIO", OSD_VARIO), OSD_ELEMENT_ENTRY("VARIO NUM", OSD_VARIO_NUM), @@ -198,9 +205,38 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("MAP TAKE OFF", OSD_MAP_TAKEOFF), OSD_ELEMENT_ENTRY("RADAR", OSD_RADAR), #endif + OSD_ELEMENT_ENTRY("EXPO", OSD_RC_EXPO), + OSD_ELEMENT_ENTRY("YAW EXPO", OSD_RC_YAW_EXPO), + OSD_ELEMENT_ENTRY("THR EXPO", OSD_THROTTLE_EXPO), + OSD_ELEMENT_ENTRY("ROLL RATE", OSD_ROLL_RATE), + OSD_ELEMENT_ENTRY("PITCH RATE", OSD_PITCH_RATE), + OSD_ELEMENT_ENTRY("YAW RATE", OSD_YAW_RATE), + OSD_ELEMENT_ENTRY("M EXPO", OSD_MANUAL_RC_EXPO), + OSD_ELEMENT_ENTRY("M YAW EXPO", OSD_MANUAL_RC_YAW_EXPO), + OSD_ELEMENT_ENTRY("M ROLL RATE", OSD_MANUAL_ROLL_RATE), + OSD_ELEMENT_ENTRY("M PITCH RATE", OSD_MANUAL_PITCH_RATE), + OSD_ELEMENT_ENTRY("M YAW RATE", OSD_MANUAL_YAW_RATE), + OSD_ELEMENT_ENTRY("CRUISE THR", OSD_NAV_FW_CRUISE_THR), + OSD_ELEMENT_ENTRY("PITCH TO THR", OSD_NAV_FW_PITCH2THR), OSD_ELEMENT_ENTRY("ROLL PIDS", OSD_ROLL_PIDS), OSD_ELEMENT_ENTRY("PITCH PIDS", OSD_PITCH_PIDS), OSD_ELEMENT_ENTRY("YAW PIDS", OSD_YAW_PIDS), + OSD_ELEMENT_ENTRY("LEVEL PIDS", OSD_LEVEL_PIDS), + OSD_ELEMENT_ENTRY("POSXY PIDS", OSD_POS_XY_PIDS), + OSD_ELEMENT_ENTRY("POSZ PIDS", OSD_POS_Z_PIDS), + OSD_ELEMENT_ENTRY("VELXY PIDS", OSD_VEL_XY_PIDS), + OSD_ELEMENT_ENTRY("VELZ PIDS", OSD_VEL_Z_PIDS), + OSD_ELEMENT_ENTRY("HEAD P", OSD_HEADING_P), + OSD_ELEMENT_ENTRY("ALIGN ROLL", OSD_BOARD_ALIGN_ROLL), + OSD_ELEMENT_ENTRY("ALIGN PITCH", OSD_BOARD_ALIGN_PITCH), + OSD_ELEMENT_ENTRY("0THR PITCH", OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE), + + OSD_ELEMENT_ENTRY("FW ALT PID OUT", OSD_FW_ALT_PID_OUTPUTS), + OSD_ELEMENT_ENTRY("FW POS PID OUT", OSD_FW_POS_PID_OUTPUTS), + OSD_ELEMENT_ENTRY("MC VELX PID OUT", OSD_MC_VEL_X_PID_OUTPUTS), + OSD_ELEMENT_ENTRY("MC VELY PID OUT", OSD_MC_VEL_Y_PID_OUTPUTS), + OSD_ELEMENT_ENTRY("MC VELZ PID OUT", OSD_MC_VEL_Z_PID_OUTPUTS), + OSD_ELEMENT_ENTRY("MC POS PID OUT", OSD_MC_POS_XYZ_P_OUTPUTS), OSD_ELEMENT_ENTRY("ATTI PITCH", OSD_ATTITUDE_PITCH), OSD_ELEMENT_ENTRY("ATTI ROLL", OSD_ATTITUDE_ROLL), @@ -212,7 +248,7 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_END_ENTRY, }; -#if defined(VTX_COMMON) && defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) +#if defined(USE_VTX_COMMON) && defined(USE_GPS) && defined(USE_BARO) && defined(USE_PITOT) // All CMS OSD elements should be enabled in this case. The menu has 3 extra // elements (label, back and end), but there's an OSD element that we intentionally // don't show here (OSD_DEBUG). diff --git a/src/main/cms/cms_menu_vtx_smartaudio.c b/src/main/cms/cms_menu_vtx_smartaudio.c index 7e28dd3ec2..a7e81bd989 100644 --- a/src/main/cms/cms_menu_vtx_smartaudio.c +++ b/src/main/cms/cms_menu_vtx_smartaudio.c @@ -1,27 +1,31 @@ /* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * 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 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 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. + * 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 Cleanflight. If not, see . + * along with this software. + * + * If not, see . */ #include #include +#include #include #include "platform.h" -#if defined(USE_CMS) && defined(VTX_SMARTAUDIO) +#if defined(USE_CMS) && defined(USE_VTX_SMARTAUDIO) #include "common/printf.h" #include "common/utils.h" @@ -36,6 +40,7 @@ #include "io/vtx_string.h" #include "io/vtx_smartaudio.h" +#include "io/vtx.h" // Interface to CMS @@ -65,8 +70,10 @@ uint16_t saCmsDeviceFreq = 0; uint8_t saCmsDeviceStatus = 0; uint8_t saCmsPower; -uint8_t saCmsPitFMode; // In-Range or Out-Range +uint8_t saCmsPitFMode; // Undef(0), In-Range(1) or Out-Range(2) + uint8_t saCmsFselMode; // Channel(0) or User defined(1) +uint8_t saCmsFselModeNew; // Channel(0) or User defined(1) uint16_t saCmsORFreq = 0; // POR frequency uint16_t saCmsORFreqNew; // POR frequency @@ -74,44 +81,33 @@ uint16_t saCmsORFreqNew; // POR frequency uint16_t saCmsUserFreq = 0; // User defined frequency uint16_t saCmsUserFreqNew; // User defined frequency -void saCmsUpdate(void) -{ -// XXX Take care of pit mode update somewhere??? - - if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) { - // This is a first valid response to GET_SETTINGS. - saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE; - - saCmsFselMode = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 1 : 0; - - saCmsBand = (saDevice.channel / 8) + 1; - saCmsChan = (saDevice.channel % 8) + 1; - saCmsFreqRef = vtx58frequencyTable[saDevice.channel / 8][saDevice.channel % 8]; - - saCmsDeviceFreq = vtx58frequencyTable[saDevice.channel / 8][saDevice.channel % 8]; - - if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) { - saCmsRFState = SACMS_TXMODE_ACTIVE; - } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { - saCmsRFState = SACMS_TXMODE_PIT_INRANGE; - } else { - saCmsRFState = SACMS_TXMODE_PIT_OUTRANGE; - } - - if (saDevice.version == 2) { - saCmsPower = saDevice.power + 1; // XXX Take care V1 - } else { - saCmsPower = saDacToPowerIndex(saDevice.power) + 1; - } - } -} - static long saCmsConfigOpmodelByGvar(displayPort_t *, const void *self); static long saCmsConfigPitFModeByGvar(displayPort_t *, const void *self); static long saCmsConfigBandByGvar(displayPort_t *, const void *self); static long saCmsConfigChanByGvar(displayPort_t *, const void *self); static long saCmsConfigPowerByGvar(displayPort_t *, const void *self); +static bool saCmsUpdateCopiedState(void) +{ + if (saDevice.version == 0) + return false; + + if (saCmsDeviceStatus == 0 && saDevice.version != 0) + saCmsDeviceStatus = saDevice.version; + if (saCmsORFreq == 0 && saDevice.orfreq != 0) + saCmsORFreq = saDevice.orfreq; + if (saCmsUserFreq == 0 && saDevice.freq != 0) + saCmsUserFreq = saDevice.freq; + + if (saDevice.version == 2) { + if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) + saCmsPitFMode = 1; + else + saCmsPitFMode = 0; + } + return true; +} + static bool saCmsDrawStatusString(char *buf, unsigned bufsize) { const char *defaultString = "- -- ---- ---"; @@ -124,21 +120,10 @@ static bool saCmsDrawStatusString(char *buf, unsigned bufsize) strcpy(buf, defaultString); - if (saDevice.version == 0) + if (!saCmsUpdateCopiedState()) { + // VTX is not ready return true; - - // XXX These should be done somewhere else - if (saCmsDeviceStatus == 0 && saDevice.version != 0) - saCmsDeviceStatus = saDevice.version; - if (saCmsORFreq == 0 && saDevice.orfreq != 0) - saCmsORFreq = saDevice.orfreq; - if (saCmsUserFreq == 0 && saDevice.freq != 0) - saCmsUserFreq = saDevice.freq; - - if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) - saCmsPitFMode = 1; - else - saCmsPitFMode = 0; + } buf[0] = "-FR"[saCmsOpmodel]; @@ -176,6 +161,47 @@ static bool saCmsDrawStatusString(char *buf, unsigned bufsize) return true; } +void saCmsUpdate(void) +{ +// XXX Take care of pit mode update somewhere??? + if (saCmsOpmodel == SACMS_OPMODEL_UNDEF) { + // This is a first valid response to GET_SETTINGS. + saCmsOpmodel = (saDevice.mode & SA_MODE_GET_PITMODE) ? SACMS_OPMODEL_RACE : SACMS_OPMODEL_FREE; + + saCmsFselMode = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 1 : 0; + + saCmsBand = vtxSettingsConfig()->band; + saCmsChan = vtxSettingsConfig()->channel; + saCmsFreqRef = vtxSettingsConfig()->freq; + saCmsDeviceFreq = saCmsFreqRef; + + if ((saDevice.mode & SA_MODE_GET_PITMODE) == 0) { + saCmsRFState = SACMS_TXMODE_ACTIVE; + } else if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { + saCmsRFState = SACMS_TXMODE_PIT_INRANGE; + } else { + saCmsRFState = SACMS_TXMODE_PIT_OUTRANGE; + } + + saCmsPower = vtxSettingsConfig()->power; + + // if user-freq mode then track possible change + if (saCmsFselMode && vtxSettingsConfig()->freq) { + saCmsUserFreq = vtxSettingsConfig()->freq; + } + + saCmsFselModeNew = saCmsFselMode; //init mode for menu + } + + saCmsUpdateCopiedState(); +} + +void saCmsResetOpmodel() +{ + // trigger data refresh in 'saCmsUpdate()' + saCmsOpmodel = SACMS_OPMODEL_UNDEF; +} + static long saCmsConfigBandByGvar(displayPort_t *pDisp, const void *self) { UNUSED(pDisp); @@ -243,8 +269,9 @@ static long saCmsConfigPowerByGvar(displayPort_t *pDisp, const void *self) return 0; } - if (saCmsOpmodel == SACMS_OPMODEL_FREE) - saSetPowerByIndex(saCmsPower - 1); + if (saCmsOpmodel == SACMS_OPMODEL_FREE && !saDeferred) { + vtxSettingsConfigMutable()->power = saCmsPower; + } return 0; } @@ -254,9 +281,21 @@ static long saCmsConfigPitFModeByGvar(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); + if (saDevice.version == 1) { + // V1 device doesn't support PIT mode; bounce back. + saCmsPitFMode = 0; + return 0; + } + dprintf(("saCmsConfigPitFmodeByGbar: saCmsPitFMode %d\r\n", saCmsPitFMode)); if (saCmsPitFMode == 0) { + // Bounce back + saCmsPitFMode = 1; + return 0; + } + + if (saCmsPitFMode == 1) { saSetMode(SA_MODE_SET_IN_RANGE_PITMODE); } else { saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE); @@ -272,6 +311,12 @@ static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); + if (saDevice.version == 1) { + if (saCmsOpmodel != SACMS_OPMODEL_FREE) + saCmsOpmodel = SACMS_OPMODEL_FREE; + return 0; + } + uint8_t opmodel = saCmsOpmodel; dprintf(("saCmsConfigOpmodelByGvar: opmodel %d\r\n", opmodel)); @@ -288,7 +333,7 @@ static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self) saCmsConfigPitFModeByGvar(pDisp, self); // Direct frequency mode is not available in RACE opmodel - saCmsFselMode = 0; + saCmsFselModeNew = 0; saCmsConfigFreqModeByGvar(pDisp, self); } else { // Trying to go back to unknown state; bounce back @@ -298,6 +343,7 @@ static long saCmsConfigOpmodelByGvar(displayPort_t *pDisp, const void *self) return 0; } +#ifdef USE_EXTENDED_CMS_MENUS static const char * const saCmsDeviceStatusNames[] = { "OFFL", "ONL V1", @@ -332,20 +378,13 @@ static const CMS_Menu saCmsMenuStats = { .onGlobalExit = NULL, .entries = saCmsMenuStatsEntries }; +#endif /* USE_EXTENDED_CMS_MENUS */ -static const OSD_TAB_t saCmsEntBand = { &saCmsBand, 5, vtx58BandNames }; +static const OSD_TAB_t saCmsEntBand = { &saCmsBand, VTX_SMARTAUDIO_BAND_COUNT, vtx58BandNames }; -static const OSD_TAB_t saCmsEntChan = { &saCmsChan, 8, vtx58ChannelNames }; +static const OSD_TAB_t saCmsEntChan = { &saCmsChan, VTX_SMARTAUDIO_CHANNEL_COUNT, vtx58ChannelNames }; -static const char * const saCmsPowerNames[] = { - "---", - "25 ", - "200", - "500", - "800", -}; - -static const OSD_TAB_t saCmsEntPower = { &saCmsPower, 4, saCmsPowerNames}; +static const OSD_TAB_t saCmsEntPower = { &saCmsPower, VTX_SMARTAUDIO_POWER_COUNT, saPowerNames}; static const char * const saCmsOpmodelNames[] = { "----", @@ -359,6 +398,7 @@ static const char * const saCmsFselModeNames[] = { }; static const char * const saCmsPitFModeNames[] = { + "---", "PIR", "POR" }; @@ -372,19 +412,15 @@ static long saCmsConfigFreqModeByGvar(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); - if (saCmsFselMode == 0) { - // CHAN - saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1); - } else { - // USER: User frequency mode is only available in FREE opmodel. - if (saCmsOpmodel == SACMS_OPMODEL_FREE) { - saSetFreq(saCmsUserFreq); - } else { - // Bounce back - saCmsFselMode = 0; - } + // if trying to do user frequency mode in RACE opmodel then + // revert because user-freq only available in FREE opmodel + if (saCmsFselModeNew != 0 && saCmsOpmodel != SACMS_OPMODEL_FREE) { + saCmsFselModeNew = 0; } + // don't call 'saSetBandAndChannel()' / 'saSetFreq()' here, + // wait until SET / 'saCmsCommence()' is activated + sacms_SetupTopMenu(NULL); return 0; @@ -395,12 +431,22 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); + const vtxSettingsConfig_t prevSettings = { + .band = vtxSettingsConfig()->band, + .channel = vtxSettingsConfig()->channel, + .freq = vtxSettingsConfig()->freq, + .power = vtxSettingsConfig()->power, + .lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm, + }; + vtxSettingsConfig_t newSettings = prevSettings; + if (saCmsOpmodel == SACMS_OPMODEL_RACE) { // Race model // Setup band, freq and power. - saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1); - + newSettings.band = saCmsBand; + newSettings.channel = saCmsChan; + newSettings.freq = vtx58_Bandchan2Freq(saCmsBand, saCmsChan); // If in pit mode, cancel it. if (saCmsPitFMode == 0) @@ -410,13 +456,26 @@ static long saCmsCommence(displayPort_t *pDisp, const void *self) } else { // Freestyle model // Setup band and freq / user freq - if (saCmsFselMode == 0) - saSetBandAndChannel(saCmsBand - 1, saCmsChan - 1); - else - saSetFreq(saCmsUserFreq); + if (saCmsFselModeNew == 0) { + newSettings.band = saCmsBand; + newSettings.channel = saCmsChan; + newSettings.freq = vtx58_Bandchan2Freq(saCmsBand, saCmsChan); + } else { + saSetMode(0); //make sure FREE mode is setup + newSettings.band = 0; + newSettings.freq = saCmsUserFreq; + } } - saSetPowerByIndex(saCmsPower - 1); + newSettings.power = saCmsPower; + + if (memcmp(&prevSettings, &newSettings, sizeof(vtxSettingsConfig_t))) { + vtxSettingsConfigMutable()->band = newSettings.band; + vtxSettingsConfigMutable()->channel = newSettings.channel; + vtxSettingsConfigMutable()->power = newSettings.power; + vtxSettingsConfigMutable()->freq = newSettings.freq; + saveConfigAndNotify(); + } return MENU_CHAIN_BACK; } @@ -425,6 +484,9 @@ static long saCmsSetPORFreqOnEnter(const OSD_Entry *from) { UNUSED(from); + if (saDevice.version == 1) + return MENU_CHAIN_BACK; + saCmsORFreqNew = saCmsORFreq; return 0; @@ -435,7 +497,7 @@ static long saCmsSetPORFreq(displayPort_t *pDisp, const void *self) UNUSED(pDisp); UNUSED(self); - saSetFreq(saCmsORFreqNew|SA_FREQ_SETPIT); + saSetPitFreq(saCmsORFreqNew); return 0; } @@ -473,7 +535,6 @@ static long saCmsConfigUserFreq(displayPort_t *pDisp, const void *self) UNUSED(self); saCmsUserFreq = saCmsUserFreqNew; - //saSetFreq(saCmsUserFreq); return MENU_CHAIN_BACK; } @@ -526,7 +587,7 @@ static const CMS_Menu saCmsMenuUserFreq = .entries = saCmsMenuUserFreqEntries, }; -static const OSD_TAB_t saCmsEntFselMode = { &saCmsFselMode, 1, saCmsFselModeNames }; +static const OSD_TAB_t saCmsEntFselMode = { &saCmsFselModeNew, 1, saCmsFselModeNames }; static const OSD_Entry saCmsMenuConfigEntries[] = { @@ -536,7 +597,9 @@ static const OSD_Entry saCmsMenuConfigEntries[] = { "FSEL MODE", OME_TAB, {.func = saCmsConfigFreqModeByGvar}, &saCmsEntFselMode, DYNAMIC }, OSD_TAB_CALLBACK_ENTRY("PIT FMODE", saCmsConfigPitFModeByGvar, &saCmsEntPitFMode), { "POR FREQ", OME_Submenu, {.menufunc = saCmsORFreqGetString}, (void *)&saCmsMenuPORFreq, OPTSTRING }, +#ifdef USE_EXTENDED_CMS_MENUS OSD_SUBMENU_ENTRY("STATX", &saCmsMenuStats), +#endif /* USE_EXTENDED_CMS_MENUS */ OSD_BACK_ENTRY, OSD_END_ENTRY, @@ -602,7 +665,7 @@ static const OSD_Entry saCmsMenuChanModeEntries[] = OSD_TAB_CALLBACK_ENTRY("CHAN", saCmsConfigChanByGvar, &saCmsEntChan), OSD_UINT16_RO_ENTRY("(FREQ)", &saCmsFreqRef), OSD_TAB_CALLBACK_ENTRY("POWER", saCmsConfigPowerByGvar, &saCmsEntPower), - OSD_SUBMENU_ENTRY("SET", &saCmsMenuCommence), + OSD_SUBMENU_ENTRY("SET", &saCmsMenuCommence), OSD_SUBMENU_ENTRY("CONFIG", &saCmsMenuConfig), OSD_BACK_ENTRY, @@ -614,7 +677,9 @@ static const OSD_Entry saCmsMenuOfflineEntries[] = OSD_LABEL_ENTRY("- VTX SMARTAUDIO -"), OSD_LABEL_FUNC_DYN_ENTRY("", saCmsDrawStatusString), +#ifdef USE_EXTENDED_CMS_MENUS OSD_SUBMENU_ENTRY("STATX", &saCmsMenuStats), +#endif /* USE_EXTENDED_CMS_MENUS */ OSD_BACK_ENTRY, OSD_END_ENTRY, @@ -627,7 +692,7 @@ static long sacms_SetupTopMenu(const OSD_Entry *from) UNUSED(from); if (saCmsDeviceStatus) { - if (saCmsFselMode == 0) + if (saCmsFselModeNew == 0) cmsx_menuVtxSmartAudio.entries = saCmsMenuChanModeEntries; else cmsx_menuVtxSmartAudio.entries = saCmsMenuFreqModeEntries; diff --git a/src/main/cms/cms_menu_vtx_smartaudio.h b/src/main/cms/cms_menu_vtx_smartaudio.h index b660f863f5..d8aeea3ab5 100644 --- a/src/main/cms/cms_menu_vtx_smartaudio.h +++ b/src/main/cms/cms_menu_vtx_smartaudio.h @@ -1,18 +1,21 @@ /* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * 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 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 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. + * 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 Cleanflight. If not, see . + * along with this software. + * + * If not, see . */ #pragma once diff --git a/src/main/cms/cms_menu_vtx_tramp.c b/src/main/cms/cms_menu_vtx_tramp.c index c67b1d01f4..735d480339 100644 --- a/src/main/cms/cms_menu_vtx_tramp.c +++ b/src/main/cms/cms_menu_vtx_tramp.c @@ -1,27 +1,31 @@ /* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * 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 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 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. + * 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 Cleanflight. If not, see . + * along with this software. + * + * If not, see . */ +#include #include #include #include #include "platform.h" -#if defined(USE_CMS) && defined(VTX_TRAMP) +#if defined(USE_CMS) && defined(USE_VTX_TRAMP) #include "common/printf.h" #include "common/utils.h" @@ -35,6 +39,7 @@ #include "io/vtx_string.h" #include "io/vtx_tramp.h" +#include "io/vtx.h" static bool trampCmsDrawStatusString(char *buf, unsigned bufsize) { @@ -48,7 +53,8 @@ static bool trampCmsDrawStatusString(char *buf, unsigned bufsize) strcpy(buf, defaultString); - if (!trampIsAvailable()) { + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_TRAMP || !vtxCommonDeviceIsReady(vtxDevice)) { return true; } @@ -65,8 +71,7 @@ static bool trampCmsDrawStatusString(char *buf, unsigned bufsize) if (trampData.power) { tfp_sprintf(&buf[9], " %c%3d", (trampData.power == trampData.configuredPower) ? ' ' : '*', trampData.power); - } - else { + } else { tfp_sprintf(&buf[9], " ----"); } @@ -78,13 +83,13 @@ uint8_t trampCmsBand = 1; uint8_t trampCmsChan = 1; uint16_t trampCmsFreqRef; -static const OSD_TAB_t trampCmsEntBand = { &trampCmsBand, 5, vtx58BandNames }; +static const OSD_TAB_t trampCmsEntBand = { &trampCmsBand, VTX_TRAMP_BAND_COUNT, vtx58BandNames }; -static const OSD_TAB_t trampCmsEntChan = { &trampCmsChan, 8, vtx58ChannelNames }; +static const OSD_TAB_t trampCmsEntChan = { &trampCmsChan, VTX_TRAMP_CHANNEL_COUNT, vtx58ChannelNames }; static uint8_t trampCmsPower = 1; -static const OSD_TAB_t trampCmsEntPower = { &trampCmsPower, 5, trampPowerNames }; +static const OSD_TAB_t trampCmsEntPower = { &trampCmsPower, sizeof(trampPowerTable), trampPowerNames }; static void trampCmsUpdateFreqRef(void) { @@ -164,20 +169,27 @@ static long trampCmsCommence(displayPort_t *pDisp, const void *self) // If it fails, the user should retry later trampCommitChanges(); + // update'vtx_' settings + vtxSettingsConfigMutable()->band = trampCmsBand; + vtxSettingsConfigMutable()->channel = trampCmsChan; + vtxSettingsConfigMutable()->power = trampCmsPower; + vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(trampCmsBand, trampCmsChan); + + saveConfigAndNotify(); return MENU_CHAIN_BACK; } static void trampCmsInitSettings(void) { - if(trampData.band > 0) trampCmsBand = trampData.band; - if(trampData.channel > 0) trampCmsChan = trampData.channel; + if (trampData.band > 0) trampCmsBand = trampData.band; + if (trampData.channel > 0) trampCmsChan = trampData.channel; trampCmsUpdateFreqRef(); trampCmsPitMode = trampData.pitMode + 1; if (trampData.configuredPower > 0) { - for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) { + for (uint8_t i = 0; i < VTX_TRAMP_POWER_COUNT; i++) { if (trampData.configuredPower <= trampPowerTable[i]) { trampCmsPower = i + 1; break; @@ -194,7 +206,8 @@ static long trampCmsOnEnter(const OSD_Entry *from) return 0; } -static const OSD_Entry trampCmsMenuCommenceEntries[] = { +static const OSD_Entry trampCmsMenuCommenceEntries[] = +{ OSD_LABEL_ENTRY("CONFIRM"), OSD_FUNC_CALL_ENTRY("YES", trampCmsCommence), diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h index ce869cc05f..5a639db503 100644 --- a/src/main/cms/cms_types.h +++ b/src/main/cms/cms_types.h @@ -24,8 +24,6 @@ #include -#include "fc/settings.h" - //type of elements typedef enum diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 2e285f36e4..37f76fce6b 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -49,6 +49,14 @@ void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT) pt1FilterInitRC(filter, 1.0f / (2.0f * M_PIf * f_cut), dT); } +void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) { + filter->RC = tau; +} + +float pt1FilterGetLastOutput(pt1Filter_t *filter) { + return filter->state; +} + float pt1FilterApply(pt1Filter_t *filter, float input) { filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state); diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 3f314d921a..88f8cbd0aa 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -57,6 +57,8 @@ float nullFilterApply(void *filter, float input); void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT); void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT); +void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau); +float pt1FilterGetLastOutput(pt1Filter_t *filter); float pt1FilterApply(pt1Filter_t *filter, float input); float pt1FilterApply3(pt1Filter_t *filter, float input, float dT); float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dt); diff --git a/src/main/common/maths.h b/src/main/common/maths.h index d59c9f180d..270d02b449 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -153,7 +153,9 @@ float cos_approx(float x); float atan2_approx(float y, float x); float acos_approx(float x); #define tan_approx(x) (sin_approx(x) / cos_approx(x)) +#define asin_approx(x) (M_PIf / 2 - acos_approx(x)) #else +#define asin_approx(x) asinf(x) #define sin_approx(x) sinf(x) #define cos_approx(x) cosf(x) #define atan2_approx(y,x) atan2f(y,x) diff --git a/src/main/common/time.h b/src/main/common/time.h index 70e64a1045..87365bec90 100644 --- a/src/main/common/time.h +++ b/src/main/common/time.h @@ -41,6 +41,13 @@ typedef uint32_t timeUs_t; #define MILLISECS_PER_SEC 1000 #define USECS_PER_SEC (1000 * 1000) +#define HZ2US(hz) (1000000 / (hz)) +#define US2S(us) ((us) * 1e-6f) +#define US2MS(us) ((us) * 1e-3f) +#define MS2US(ms) ((ms) * 1000) +#define MS2S(ms) ((ms) * 1e-3f) +#define HZ2S(hz) US2S(HZ2US(hz)) + static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); } typedef enum { diff --git a/src/main/common/vector.h b/src/main/common/vector.h index fdb2ce29d4..391fa04265 100644 --- a/src/main/common/vector.h +++ b/src/main/common/vector.h @@ -41,6 +41,13 @@ typedef struct { void rotationMatrixFromAngles(fpMat3_t * rmat, const fp_angles_t * angles); void rotationMatrixFromAxisAngle(fpMat3_t * rmat, const fpAxisAngle_t * a); +static inline void vectorZero(fpVector3_t * v) +{ + v->x = 0.0f; + v->y = 0.0f; + v->z = 0.0f; +} + static inline fpVector3_t * rotationMatrixRotateVector(fpVector3_t * result, const fpVector3_t * a, const fpMat3_t * rmat) { fpVector3_t r; diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 2b4dfe47ba..a3e23b392b 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -27,7 +27,7 @@ //#define PG_SENSOR_ALIGNMENT_CONFIG 8 -- NOT USED in iNav //#define PG_SENSOR_TRIMS 9 -- NOT USED in iNav #define PG_GYRO_CONFIG 10 -#define PG_BATTERY_CONFIG 11 +#define PG_BATTERY_PROFILES 11 #define PG_CONTROL_RATE_PROFILES 12 #define PG_SERIAL_CONFIG 13 #define PG_PID_PROFILE 14 @@ -63,6 +63,7 @@ #define PG_SERVO_PARAMS 42 //#define PG_RX_FAILSAFE_CHANNEL_CONFIG 43 #define PG_RX_CHANNEL_RANGE_CONFIG 44 +#define PG_BATTERY_METERS_CONFIG 45 //#define PG_MODE_COLOR_CONFIG 45 //#define PG_SPECIAL_COLOR_CONFIG 46 //#define PG_PILOT_CONFIG 47 @@ -80,6 +81,9 @@ //#define PG_DRIVER_PWM_RX_CONFIG 100 //#define PG_DRIVER_FLASHCHIP_CONFIG 101 +// cleanflight v2 specific parameter group ids start at 256 +#define PG_VTX_SETTINGS_CONFIG 259 + // iNav specific parameter group ids start at 1000 #define PG_INAV_START 1000 #define PG_PITOTMETER_CONFIG 1000 diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 1f9216ad14..8fcd8386db 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -262,7 +262,7 @@ void max7456ReInit(void) //here we init only CS and try to init MAX for first time -void max7456Init(const vcdProfile_t *pVcdProfile) +void max7456Init(const videoSystem_e videoSystem) { max7456dev = busDeviceInit(BUSTYPE_SPI, DEVHW_MAX7456, 0, OWNER_OSD); @@ -275,7 +275,7 @@ void max7456Init(const vcdProfile_t *pVcdProfile) // force soft reset on Max7456 busWrite(max7456dev, MAX7456ADD_VM0, MAX7456_RESET); - videoSignalCfg = pVcdProfile->video_system; + videoSignalCfg = videoSystem; // Set screenbuffer to all blanks for (uint_fast16_t ii = 0; ii < ARRAYLEN(screenBuffer); ii++) { diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h index bd941f72f8..2227ec7483 100644 --- a/src/main/drivers/max7456.h +++ b/src/main/drivers/max7456.h @@ -18,6 +18,7 @@ #pragma once #include +#include "drivers/vcd.h" #ifndef WHITEBRIGHTNESS #define WHITEBRIGHTNESS 0x01 @@ -43,7 +44,7 @@ enum VIDEO_TYPES { AUTO = 0, PAL, NTSC }; extern uint16_t maxScreenSize; struct vcdProfile_s; -void max7456Init(const struct vcdProfile_s *vcdProfile); +void max7456Init(const videoSystem_e videoSystem); void max7456DrawScreenPartial(void); void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data); uint8_t max7456GetRowsCount(void); diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h index 41c5598e0c..a87630d37a 100644 --- a/src/main/drivers/max7456_symbols.h +++ b/src/main/drivers/max7456_symbols.h @@ -23,7 +23,6 @@ #ifdef USE_MAX7456 -// Character Symbols #define SYM_BLANK 0x20 // Satellite Graphics @@ -156,6 +155,9 @@ #define SYM_MAIN_BATT 0x97 #define SYM_VID_BAT 0xBF +// Used for battery impedance +#define SYM_MILLIOHM 0x3F + // Unit Icon´s (Metric) #define SYM_MS 0x9F #define SYM_KMH 0xA1 diff --git a/src/main/drivers/opflow/opflow.h b/src/main/drivers/opflow/opflow.h index f78326761b..ef3033b940 100755 --- a/src/main/drivers/opflow/opflow.h +++ b/src/main/drivers/opflow/opflow.h @@ -31,7 +31,7 @@ struct opflowDev_s; typedef struct opflowData_s { timeDelta_t deltaTime; // Integration timeframe of motionX/Y - int32_t flowRateRaw[2]; // Flow rotation in raw sensor uints (per deltaTime interval) + int32_t flowRateRaw[3]; // Flow rotation in raw sensor uints (per deltaTime interval). Use dummy 3-rd axis (always zero) for compatibility with alignment functions int16_t quality; } opflowData_t; diff --git a/src/main/drivers/opflow/opflow_fake.c b/src/main/drivers/opflow/opflow_fake.c index 0dc6ae085e..517b14cfe1 100755 --- a/src/main/drivers/opflow/opflow_fake.c +++ b/src/main/drivers/opflow/opflow_fake.c @@ -43,6 +43,7 @@ void fakeOpflowSet(timeDelta_t deltaTime, int32_t flowRateX, int32_t flowRateY, fakeData.deltaTime = deltaTime; fakeData.flowRateRaw[0] = flowRateX; fakeData.flowRateRaw[1] = flowRateY; + fakeData.flowRateRaw[2] = 0; fakeData.quality = quality; } diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index abfdd9f9ea..29364228bd 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -41,7 +41,8 @@ typedef enum portOptions_t { * to actual data bytes. */ SERIAL_BIDIR_OD = 0 << 4, - SERIAL_BIDIR_PP = 1 << 4 + SERIAL_BIDIR_PP = 1 << 4, + SERIAL_BIDIR_NOPULL = 1 << 5, // disable pulls in BIDIR RX mode } portOptions_t; typedef void (*serialReceiveCallbackPtr)(uint16_t data, void *rxCallbackData); // used by serial drivers to return frames to app diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index b73d9aa8a6..e9f77cbb10 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -130,9 +130,11 @@ static void serialEnableCC(softSerial_t *softSerial) static void serialInputPortActivate(softSerial_t *softSerial) { if (softSerial->port.options & SERIAL_INVERTED) { - IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_PD, softSerial->timerHardware->alternateFunction); + const uint8_t pinConfig = (softSerial->port.options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_PP : IOCFG_AF_PP_PD; + IOConfigGPIOAF(softSerial->rxIO, pinConfig, softSerial->timerHardware->alternateFunction); } else { - IOConfigGPIOAF(softSerial->rxIO, IOCFG_AF_PP_UP, softSerial->timerHardware->alternateFunction); + const uint8_t pinConfig = (softSerial->port.options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_PP : IOCFG_AF_PP_UP; + IOConfigGPIOAF(softSerial->rxIO, pinConfig, softSerial->timerHardware->alternateFunction); } softSerial->rxActive = true; diff --git a/src/main/drivers/vcd.h b/src/main/drivers/vcd.h index 0daea327c1..085da79155 100755 --- a/src/main/drivers/vcd.h +++ b/src/main/drivers/vcd.h @@ -19,12 +19,8 @@ // Video Character Display parameters -typedef struct vcdProfile_s { - uint8_t video_system; -} vcdProfile_t; - -enum VIDEO_SYSTEMS { +typedef enum { VIDEO_SYSTEM_AUTO = 0, VIDEO_SYSTEM_PAL, VIDEO_SYSTEM_NTSC -}; +} videoSystem_e; diff --git a/src/main/drivers/vtx_common.c b/src/main/drivers/vtx_common.c index dfb45af32a..8893dfd817 100644 --- a/src/main/drivers/vtx_common.c +++ b/src/main/drivers/vtx_common.c @@ -25,42 +25,52 @@ #include "platform.h" #include "build/debug.h" -#if defined(VTX_COMMON) +#if defined(USE_VTX_COMMON) #include "vtx_common.h" -vtxDevice_t *vtxDevice = NULL; +static vtxDevice_t *commonVtxDevice = NULL; void vtxCommonInit(void) { } -// Whatever registered last will win - -void vtxCommonRegisterDevice(vtxDevice_t *pDevice) +void vtxCommonSetDevice(vtxDevice_t *vtxDevice) { - vtxDevice = pDevice; + commonVtxDevice = vtxDevice; } -void vtxCommonProcess(timeUs_t currentTimeUs) +vtxDevice_t *vtxCommonDevice(void) { - if (!vtxDevice) - return; - - if (vtxDevice->vTable->process) - vtxDevice->vTable->process(currentTimeUs); + return commonVtxDevice; } -vtxDevType_e vtxCommonGetDeviceType(void) +void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) { - if (!vtxDevice || !vtxDevice->vTable->getDeviceType) + if (vtxDevice && vtxDevice->vTable->process) { + vtxDevice->vTable->process(vtxDevice, currentTimeUs); + } +} + +vtxDevType_e vtxCommonGetDeviceType(vtxDevice_t *vtxDevice) +{ + if (!vtxDevice || !vtxDevice->vTable->getDeviceType) { return VTXDEV_UNKNOWN; + } - return vtxDevice->vTable->getDeviceType(); + return vtxDevice->vTable->getDeviceType(vtxDevice); +} + +bool vtxCommonDeviceIsReady(vtxDevice_t *vtxDevice) +{ + if (vtxDevice && vtxDevice->vTable->isReady) { + return vtxDevice->vTable->isReady(vtxDevice); + } + return false; } // band and channel are 1 origin -void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel) +void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { if (!vtxDevice) return; @@ -68,12 +78,13 @@ void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel) if ((band > vtxDevice->capability.bandCount) || (channel > vtxDevice->capability.channelCount)) return; - if (vtxDevice->vTable->setBandAndChannel) - vtxDevice->vTable->setBandAndChannel(band, channel); + if (vtxDevice->vTable->setBandAndChannel) { + vtxDevice->vTable->setBandAndChannel(vtxDevice, band, channel); + } } // index is zero origin, zero = power off completely -void vtxCommonSetPowerByIndex(uint8_t index) +void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { if (!vtxDevice) return; @@ -81,59 +92,64 @@ void vtxCommonSetPowerByIndex(uint8_t index) if (index > vtxDevice->capability.powerCount) return; - if (vtxDevice->vTable->setPowerByIndex) - vtxDevice->vTable->setPowerByIndex(index); + if (vtxDevice->vTable->setPowerByIndex) { + vtxDevice->vTable->setPowerByIndex(vtxDevice, index); + } } // on = 1, off = 0 -void vtxCommonSetPitMode(uint8_t onoff) +void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) { - if (!vtxDevice) - return; - - if (vtxDevice->vTable->setPitMode) - vtxDevice->vTable->setPitMode(onoff); + if (vtxDevice && vtxDevice->vTable->setPitMode) { + vtxDevice->vTable->setPitMode(vtxDevice, onoff); + } } -bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) +void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency) { - if (!vtxDevice) - return false; - - if (vtxDevice->vTable->getBandAndChannel) - return vtxDevice->vTable->getBandAndChannel(pBand, pChannel); - else - return false; + if (vtxDevice && vtxDevice->vTable->setFrequency) { + vtxDevice->vTable->setFrequency(vtxDevice, frequency); + } } -bool vtxCommonGetPowerIndex(uint8_t *pIndex) +bool vtxCommonGetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) { - if (!vtxDevice) - return false; - - if (vtxDevice->vTable->getPowerIndex) - return vtxDevice->vTable->getPowerIndex(pIndex); - else - return false; + if (vtxDevice && vtxDevice->vTable->getBandAndChannel) { + return vtxDevice->vTable->getBandAndChannel(vtxDevice, pBand, pChannel); + } + return false; } -bool vtxCommonGetPitMode(uint8_t *pOnOff) +bool vtxCommonGetPowerIndex(vtxDevice_t *vtxDevice, uint8_t *pIndex) { - if (!vtxDevice) - return false; - - if (vtxDevice->vTable->getPitMode) - return vtxDevice->vTable->getPitMode(pOnOff); - else - return false; + if (vtxDevice && vtxDevice->vTable->getPowerIndex) { + return vtxDevice->vTable->getPowerIndex(vtxDevice, pIndex); + } + return false; } -bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability) +bool vtxCommonGetPitMode(vtxDevice_t *vtxDevice, uint8_t *pOnOff) { - if (!vtxDevice) - return false; + if (vtxDevice && vtxDevice->vTable->getPitMode) { + return vtxDevice->vTable->getPitMode(vtxDevice, pOnOff); + } + return false; +} - memcpy(pDeviceCapability, &vtxDevice->capability, sizeof(vtxDeviceCapability_t)); - return true; +bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFrequency) +{ + if (vtxDevice && vtxDevice->vTable->getFrequency) { + return vtxDevice->vTable->getFrequency(vtxDevice, pFrequency); + } + return false; +} + +bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability) +{ + if (vtxDevice) { + memcpy(pDeviceCapability, &vtxDevice->capability, sizeof(vtxDeviceCapability_t)); + return true; + } + return false; } #endif diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 8975f78c83..b8d9cec880 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -19,6 +19,53 @@ #include "common/time.h" +#define VTX_SETTINGS_NO_BAND 0 // used for custom frequency selection mode +#define VTX_SETTINGS_MIN_BAND 1 +#define VTX_SETTINGS_MAX_BAND 5 +#define VTX_SETTINGS_MIN_CHANNEL 1 +#define VTX_SETTINGS_MAX_CHANNEL 8 + +#define VTX_SETTINGS_BAND_COUNT (VTX_SETTINGS_MAX_BAND - VTX_SETTINGS_MIN_BAND + 1) +#define VTX_SETTINGS_CHANNEL_COUNT (VTX_SETTINGS_MAX_CHANNEL - VTX_SETTINGS_MIN_CHANNEL + 1) + +#define VTX_SETTINGS_DEFAULT_BAND 4 +#define VTX_SETTINGS_DEFAULT_CHANNEL 1 +#define VTX_SETTINGS_DEFAULT_FREQ 5740 +#define VTX_SETTINGS_DEFAULT_PITMODE_FREQ 0 +#define VTX_SETTINGS_DEFAULT_LOW_POWER_DISARM 0 + +#define VTX_SETTINGS_MIN_FREQUENCY_MHZ 0 //min freq (in MHz) for 'vtx_freq' setting +#define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting + +#if defined(USE_VTX_RTC6705) + +#include "drivers/vtx_rtc6705.h" + +#endif + +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) + +#define VTX_SETTINGS_POWER_COUNT 5 +#define VTX_SETTINGS_DEFAULT_POWER 1 +#define VTX_SETTINGS_MIN_POWER 0 +#define VTX_SETTINGS_MIN_USER_FREQ 5000 +#define VTX_SETTINGS_MAX_USER_FREQ 5999 +#define VTX_SETTINGS_FREQCMD + +#elif defined(USE_VTX_RTC6705) + +#define VTX_SETTINGS_POWER_COUNT VTX_RTC6705_POWER_COUNT +#define VTX_SETTINGS_DEFAULT_POWER VTX_RTC6705_DEFAULT_POWER +#define VTX_SETTINGS_MIN_POWER VTX_RTC6705_MIN_POWER + +#endif + +#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - 1) + +// check value for MSP_SET_VTX_CONFIG to determine if value is encoded +// band/channel or frequency in MHz (3 bits for band and 3 bits for channel) +#define VTXCOMMON_MSP_BANDCHAN_CHKVAL ((uint16_t)((7 << 3) + 7)) + typedef enum { VTXDEV_UNSUPPORTED = 0, // reserved for MSP VTXDEV_RTC6705 = 1, @@ -58,17 +105,19 @@ typedef struct vtxDevice_s { // {set,get}PitMode: 0 = OFF, 1 = ON typedef struct vtxVTable_s { - void (*process)(uint32_t currentTimeUs); - vtxDevType_e (*getDeviceType)(void); - bool (*isReady)(void); + void (*process)(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs); + vtxDevType_e (*getDeviceType)(const vtxDevice_t *vtxDevice); + bool (*isReady)(const vtxDevice_t *vtxDevice); - void (*setBandAndChannel)(uint8_t band, uint8_t channel); - void (*setPowerByIndex)(uint8_t level); - void (*setPitMode)(uint8_t onoff); + void (*setBandAndChannel)(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel); + void (*setPowerByIndex)(vtxDevice_t *vtxDevice, uint8_t level); + void (*setPitMode)(vtxDevice_t *vtxDevice, uint8_t onoff); + void (*setFrequency)(vtxDevice_t *vtxDevice, uint16_t freq); - bool (*getBandAndChannel)(uint8_t *pBand, uint8_t *pChannel); - bool (*getPowerIndex)(uint8_t *pIndex); - bool (*getPitMode)(uint8_t *pOnOff); + bool (*getBandAndChannel)(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel); + bool (*getPowerIndex)(const vtxDevice_t *vtxDevice, uint8_t *pIndex); + bool (*getPitMode)(const vtxDevice_t *vtxDevice, uint8_t *pOnOff); + bool (*getFrequency)(const vtxDevice_t *vtxDevice, uint16_t *pFreq); } vtxVTable_t; // 3.1.0 @@ -77,15 +126,19 @@ typedef struct vtxVTable_s { // - It is *NOT* RF on/off control ? void vtxCommonInit(void); -void vtxCommonRegisterDevice(vtxDevice_t *pDevice); +void vtxCommonSetDevice(vtxDevice_t *vtxDevice); +vtxDevice_t *vtxCommonDevice(void); // VTable functions -void vtxCommonProcess(timeUs_t currentTimeUs); -uint8_t vtxCommonGetDeviceType(void); -void vtxCommonSetBandAndChannel(uint8_t band, uint8_t channel); -void vtxCommonSetPowerByIndex(uint8_t level); -void vtxCommonSetPitMode(uint8_t onoff); -bool vtxCommonGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel); -bool vtxCommonGetPowerIndex(uint8_t *pIndex); -bool vtxCommonGetPitMode(uint8_t *pOnOff); -bool vtxCommonGetDeviceCapability(vtxDeviceCapability_t *pDeviceCapability); +void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs); +vtxDevType_e vtxCommonGetDeviceType(vtxDevice_t *vtxDevice); +bool vtxCommonDeviceIsReady(vtxDevice_t *vtxDevice); +void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel); +void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index); +void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff); +void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t frequency); +bool vtxCommonGetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel); +bool vtxCommonGetPowerIndex(vtxDevice_t *vtxDevice, uint8_t *pIndex); +bool vtxCommonGetPitMode(vtxDevice_t *vtxDevice, uint8_t *pOnOff); +bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFreq); +bool vtxCommonGetDeviceCapability(vtxDevice_t *vtxDevice, vtxDeviceCapability_t *pDeviceCapability); diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c index 3471d928dd..591f22efc3 100644 --- a/src/main/drivers/vtx_rtc6705.c +++ b/src/main/drivers/vtx_rtc6705.c @@ -27,7 +27,7 @@ #include "platform.h" -#if defined(VTX_RTC6705) && !defined(VTX_RTC6705SOFTSPI) +#if defined(USE_VTX_RTC6705) && !defined(VTX_RTC6705SOFTSPI) #include "common/maths.h" @@ -118,7 +118,7 @@ static IO_t vtxCLKPin = IO_NONE; // Define variables -static const uint32_t channelArray[RTC6705_BAND_COUNT][RTC6705_CHANNEL_COUNT] = { +static const uint32_t channelArray[VTX_RTC6705_BAND_COUNT][VTX_RTC6705_CHANNEL_COUNT] = { { RTC6705_SET_A1, RTC6705_SET_A2, RTC6705_SET_A3, RTC6705_SET_A4, RTC6705_SET_A5, RTC6705_SET_A6, RTC6705_SET_A7, RTC6705_SET_A8 }, { RTC6705_SET_B1, RTC6705_SET_B2, RTC6705_SET_B3, RTC6705_SET_B4, RTC6705_SET_B5, RTC6705_SET_B6, RTC6705_SET_B7, RTC6705_SET_B8 }, { RTC6705_SET_E1, RTC6705_SET_E2, RTC6705_SET_E3, RTC6705_SET_E4, RTC6705_SET_E5, RTC6705_SET_E6, RTC6705_SET_E7, RTC6705_SET_E8 }, @@ -200,8 +200,8 @@ static void rtc6705Transfer(uint32_t command) */ void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel) { - band = constrain(band, 0, RTC6705_BAND_COUNT - 1); - channel = constrain(channel, 0, RTC6705_CHANNEL_COUNT - 1); + band = constrain(band, 0, VTX_RTC6705_BAND_COUNT - 1); + channel = constrain(channel, 0, VTX_RTC6705_CHANNEL_COUNT - 1); spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW); @@ -215,7 +215,7 @@ void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel) */ void rtc6705SetFreq(uint16_t frequency) { - frequency = constrain(frequency, RTC6705_FREQ_MIN, RTC6705_FREQ_MAX); + frequency = constrain(frequency, VTX_RTC6705_FREQ_MIN, VTX_RTC6705_FREQ_MAX); uint32_t val_hex = 0; @@ -235,7 +235,7 @@ void rtc6705SetFreq(uint16_t frequency) void rtc6705SetRFPower(uint8_t rf_power) { - rf_power = constrain(rf_power, 0, RTC6705_RF_POWER_COUNT - 1); + rf_power = constrain(rf_power, 0, VTX_RTC6705_POWER_COUNT - 1); spiSetSpeed(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW); diff --git a/src/main/drivers/vtx_rtc6705.h b/src/main/drivers/vtx_rtc6705.h index 3baa594573..7a41938607 100644 --- a/src/main/drivers/vtx_rtc6705.h +++ b/src/main/drivers/vtx_rtc6705.h @@ -26,14 +26,21 @@ #include -#define RTC6705_BAND_COUNT 5 -#define RTC6705_CHANNEL_COUNT 8 -#define RTC6705_RF_POWER_COUNT 2 +#define VTX_RTC6705_BAND_COUNT 5 +#define VTX_RTC6705_CHANNEL_COUNT 8 +#define VTX_RTC6705_POWER_COUNT 3 +#define VTX_RTC6705_DEFAULT_POWER 1 -#define RTC6705_FREQ_MIN 5600 -#define RTC6705_FREQ_MAX 5950 +#if defined(RTC6705_POWER_PIN) +#define VTX_RTC6705_MIN_POWER 0 +#else +#define VTX_RTC6705_MIN_POWER 1 +#endif -#define RTC6705_BOOT_DELAY 350 // milliseconds +#define VTX_RTC6705_FREQ_MIN 5600 +#define VTX_RTC6705_FREQ_MAX 5950 + +#define VTX_RTC6705_BOOT_DELAY 350 // milliseconds void rtc6705IOInit(void); void rtc6705SetBandAndChannel(const uint8_t band, const uint8_t channel); diff --git a/src/main/drivers/vtx_rtc6705_soft_spi.c b/src/main/drivers/vtx_rtc6705_soft_spi.c index 0acd4e683e..d7795e6c47 100644 --- a/src/main/drivers/vtx_rtc6705_soft_spi.c +++ b/src/main/drivers/vtx_rtc6705_soft_spi.c @@ -20,7 +20,7 @@ #include "platform.h" -#if defined(VTX_RTC6705) && defined(VTX_RTC6705SOFTSPI) +#if defined(USE_VTX_RTC6705) && defined(VTX_RTC6705SOFTSPI) #include "drivers/bus_spi.h" #include "drivers/io.h" diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index f6f079272c..91aaf2e38b 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -108,7 +108,7 @@ extern uint8_t __config_end; #include "sensors/opflow.h" #include "sensors/sensors.h" -#include "telemetry/frsky.h" +#include "telemetry/frsky_d.h" #include "telemetry/telemetry.h" #include "build/debug.h" @@ -135,11 +135,9 @@ static void cliAssert(char *cmdline); static void cliBootlog(char *cmdline); #endif -static const char* const emptyName = "-"; - // sync this with features_e static const char * const featureNames[] = { - "RX_PPM", "VBAT", "TX_PROF_SEL", "", "MOTOR_STOP", + "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP", "", "SOFTSERIAL", "GPS", "", "", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", @@ -229,11 +227,12 @@ static void cliPutp(void *p, char ch) typedef enum { DUMP_MASTER = (1 << 0), DUMP_PROFILE = (1 << 1), - DUMP_RATES = (1 << 2), - DUMP_ALL = (1 << 3), - DO_DIFF = (1 << 4), - SHOW_DEFAULTS = (1 << 5), - HIDE_UNUSED = (1 << 6) + DUMP_BATTERY_PROFILE = (1 << 2), + DUMP_RATES = (1 << 3), + DUMP_ALL = (1 << 4), + DO_DIFF = (1 << 5), + SHOW_DEFAULTS = (1 << 6), + HIDE_UNUSED = (1 << 7) } dumpFlags_e; static void cliPrintfva(const char *format, va_list va) @@ -329,11 +328,15 @@ static void printValuePointer(const setting_t *var, const void *valuePointer, ui cliPrintf("%s", ftoa(*(float *)valuePointer, buf)); if (full) { if (SETTING_MODE(var) == MODE_DIRECT) { - cliPrintf(" %s", ftoa((float)setting_get_min(var), buf)); - cliPrintf(" %s", ftoa((float)setting_get_max(var), buf)); + cliPrintf(" %s", ftoa((float)settingGetMin(var), buf)); + cliPrintf(" %s", ftoa((float)settingGetMax(var), buf)); } } return; // return from case for float only + + case VAR_STRING: + cliPrintf("%s", (const char *)valuePointer); + return; } switch (SETTING_MODE(var)) { @@ -344,7 +347,7 @@ static void printValuePointer(const setting_t *var, const void *valuePointer, ui cliPrintf("%d", value); if (full) { if (SETTING_MODE(var) == MODE_DIRECT) { - cliPrintf(" %d %u", setting_get_min(var), setting_get_max(var)); + cliPrintf(" %d %u", settingGetMin(var), settingGetMax(var)); } } break; @@ -352,17 +355,17 @@ static void printValuePointer(const setting_t *var, const void *valuePointer, ui if (var->config.lookup.tableIndex < LOOKUP_TABLE_COUNT) { cliPrintf(settingLookupTables[var->config.lookup.tableIndex].values[value]); } else { - setting_get_name(var, buf); + settingGetName(var, buf); cliPrintLinef("VALUE %s OUT OF RANGE", buf); } break; } } -static bool valuePtrEqualsDefault(uint8_t type, const void *ptr, const void *ptrDefault) +static bool valuePtrEqualsDefault(const setting_t *value, const void *ptr, const void *ptrDefault) { bool result = false; - switch (type & SETTING_TYPE_MASK) { + switch (SETTING_TYPE(value)) { case VAR_UINT8: result = *(uint8_t *)ptr == *(uint8_t *)ptrDefault; break; @@ -386,6 +389,10 @@ static bool valuePtrEqualsDefault(uint8_t type, const void *ptr, const void *ptr case VAR_FLOAT: result = *(float *)ptr == *(float *)ptrDefault; break; + + case VAR_STRING: + result = strncmp(ptr, ptrDefault, settingGetStringMaxLength(value) + 1) == 0; + break; } return result; } @@ -397,14 +404,14 @@ static void dumpPgValue(const setting_t *value, uint8_t dumpMask) const char *defaultFormat = "#set %s = "; // During a dump, the PGs have been backed up to their "copy" // regions and the actual values have been reset to its - // defaults. This means that setting_get_value_pointer() will - // return the default value while setting_get_copy_value_pointer() + // defaults. This means that settingGetValuePointer() will + // return the default value while settingGetCopyValuePointer() // will return the actual value. - const void *valuePointer = setting_get_copy_value_pointer(value); - const void *defaultValuePointer = setting_get_value_pointer(value); - const bool equalsDefault = valuePtrEqualsDefault(value->type, valuePointer, defaultValuePointer); + const void *valuePointer = settingGetCopyValuePointer(value); + const void *defaultValuePointer = settingGetValuePointer(value); + const bool equalsDefault = valuePtrEqualsDefault(value, valuePointer, defaultValuePointer); if (((dumpMask & DO_DIFF) == 0) || !equalsDefault) { - setting_get_name(value, name); + settingGetName(value, name); if (dumpMask & SHOW_DEFAULTS && !equalsDefault) { cliPrintf(defaultFormat, name); printValuePointer(value, defaultValuePointer, 0); @@ -429,7 +436,7 @@ static void dumpAllValues(uint16_t valueSection, uint8_t dumpMask) static void cliPrintVar(const setting_t *var, uint32_t full) { - const void *ptr = setting_get_value_pointer(var); + const void *ptr = settingGetValuePointer(var); printValuePointer(var, ptr, full); } @@ -437,10 +444,15 @@ static void cliPrintVar(const setting_t *var, uint32_t full) static void cliPrintVarRange(const setting_t *var) { switch (SETTING_MODE(var)) { - case (MODE_DIRECT): - cliPrintLinef("Allowed range: %d - %u", setting_get_min(var), setting_get_max(var)); + case MODE_DIRECT: + if (SETTING_TYPE(var) == VAR_STRING) { + cliPrintLinef("Max. length: %u", settingGetStringMaxLength(var)); + break; + } + cliPrintLinef("Allowed range: %d - %u", settingGetMin(var), settingGetMax(var)); break; - case (MODE_LOOKUP): { + case MODE_LOOKUP: + { const lookupTableEntry_t *tableEntry = &settingLookupTables[var->config.lookup.tableIndex]; cliPrint("Allowed values:"); for (uint32_t i = 0; i < tableEntry->valueCount ; i++) { @@ -450,7 +462,7 @@ static void cliPrintVarRange(const setting_t *var) } cliPrintLinefeed(); } - break; + break; } } @@ -460,9 +472,9 @@ typedef union { float float_value; } int_float_value_t; -static void cliSetVar(const setting_t *var, const int_float_value_t value) +static void cliSetIntFloatVar(const setting_t *var, const int_float_value_t value) { - void *ptr = setting_get_value_pointer(var); + void *ptr = settingGetValuePointer(var); switch (SETTING_TYPE(var)) { case VAR_UINT8: @@ -482,6 +494,10 @@ static void cliSetVar(const setting_t *var, const int_float_value_t value) case VAR_FLOAT: *(float *)ptr = (float)value.float_value; break; + + case VAR_STRING: + // Handled by cliSet directly + break; } } @@ -2122,24 +2138,6 @@ static void cliMotor(char *cmdline) cliPrintLinef("motor %d: %d", motor_index, motor_disarmed[motor_index]); } -static void printName(uint8_t dumpMask, const systemConfig_t * sConfig) -{ - bool equalsDefault = strlen(sConfig->name) == 0; - cliDumpPrintLinef(dumpMask, equalsDefault, "name %s", equalsDefault ? emptyName : sConfig->name); -} - -static void cliName(char *cmdline) -{ - int32_t len = strlen(cmdline); - if (len > 0) { - memset(systemConfigMutable()->name, 0, ARRAYLEN(systemConfigMutable()->name)); - if (strncmp(cmdline, emptyName, len)) { - strncpy(systemConfigMutable()->name, cmdline, MIN(len, MAX_NAME_LENGTH)); - } - } - printName(DUMP_MASTER, systemConfig()); -} - #ifdef PLAY_SOUND static void cliPlaySound(char *cmdline) { @@ -2203,6 +2201,33 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask) dumpAllValues(CONTROL_RATE_VALUE, dumpMask); } +static void cliBatteryProfile(char *cmdline) +{ + // CLI profile index is 1-based + if (isEmpty(cmdline)) { + cliPrintLinef("battery_profile %d", getConfigBatteryProfile() + 1); + return; + } else { + const int i = fastA2I(cmdline) - 1; + if (i >= 0 && i < MAX_PROFILE_COUNT) { + setConfigBatteryProfileAndWriteEEPROM(i); + cliBatteryProfile(""); + } + } +} + +static void cliDumpBatteryProfile(uint8_t profileIndex, uint8_t dumpMask) +{ + if (profileIndex >= MAX_BATTERY_PROFILE_COUNT) { + // Faulty values + return; + } + setConfigBatteryProfile(profileIndex); + cliPrintHashLine("battery_profile"); + cliPrintLinef("battery_profile %d\r\n", getConfigBatteryProfile() + 1); + dumpAllValues(BATTERY_CONFIG_VALUE, dumpMask); +} + static void cliSave(char *cmdline) { UNUSED(cmdline); @@ -2232,7 +2257,7 @@ static void cliGet(char *cmdline) for (uint32_t i = 0; i < SETTINGS_TABLE_COUNT; i++) { val = &settingsTable[i]; - if (setting_name_contains(val, name, cmdline)) { + if (settingNameContains(val, name, cmdline)) { cliPrintf("%s = ", name); cliPrintVar(val, 0); cliPrintLinefeed(); @@ -2264,7 +2289,7 @@ static void cliSet(char *cmdline) cliPrintLine("Current settings:"); for (uint32_t i = 0; i < SETTINGS_TABLE_COUNT; i++) { val = &settingsTable[i]; - setting_get_name(val, name); + settingGetName(val, name); cliPrintf("%s = ", name); cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui cliPrintLinefeed(); @@ -2287,17 +2312,21 @@ static void cliSet(char *cmdline) for (uint32_t i = 0; i < SETTINGS_TABLE_COUNT; i++) { val = &settingsTable[i]; // ensure exact match when setting to prevent setting variables with shorter names - if (setting_name_exact_match(val, name, cmdline, variableNameLength)) { + if (settingNameIsExactMatch(val, name, cmdline, variableNameLength)) { + const setting_type_e type = SETTING_TYPE(val); + if (type == VAR_STRING) { + settingSetString(val, eqptr, strlen(eqptr)); + return; + } + const setting_mode_e mode = SETTING_MODE(val); bool changeValue = false; int_float_value_t tmp = {0}; - const int mode = SETTING_MODE(val); - const int type = SETTING_TYPE(val); switch (mode) { case MODE_DIRECT: { if (*eqptr != 0 && strspn(eqptr, "0123456789.+-") == strlen(eqptr)) { float valuef = fastA2F(eqptr); // note: compare float values - if (valuef >= (float)setting_get_min(val) && valuef <= (float)setting_get_max(val)) { + if (valuef >= (float)settingGetMin(val) && valuef <= (float)settingGetMax(val)) { if (type == VAR_FLOAT) tmp.float_value = valuef; @@ -2327,7 +2356,7 @@ static void cliSet(char *cmdline) } if (changeValue) { - cliSetVar(val, tmp); + cliSetIntFloatVar(val, tmp); cliPrintf("%s set to ", name); cliPrintVar(val, 0); @@ -2583,6 +2612,8 @@ static void printConfig(const char *cmdline, bool doDiff) dumpMask = DUMP_MASTER; // only } else if ((options = checkCommand(cmdline, "profile"))) { dumpMask = DUMP_PROFILE; // only + } else if ((options = checkCommand(cmdline, "battery_profile"))) { + dumpMask = DUMP_BATTERY_PROFILE; // only } else if ((options = checkCommand(cmdline, "all"))) { dumpMask = DUMP_ALL; // all profiles and rates } else { @@ -2594,11 +2625,13 @@ static void printConfig(const char *cmdline, bool doDiff) } const int currentProfileIndexSave = getConfigProfile(); + const int currentBatteryProfileIndexSave = getConfigBatteryProfile(); backupConfigs(); // reset all configs to defaults to do differencing resetConfigs(); // restore the profile indices, since they should not be reset for proper comparison setConfigProfile(currentProfileIndexSave); + setConfigBatteryProfile(currentBatteryProfileIndexSave); if (checkCommand(options, "showdefaults")) { dumpMask = dumpMask | SHOW_DEFAULTS; // add default values as comments for changed values @@ -2644,9 +2677,6 @@ static void printConfig(const char *cmdline, bool doDiff) cliPrintHashLine("map"); printMap(dumpMask, &rxConfig_Copy, rxConfig()); - cliPrintHashLine("name"); - printName(dumpMask, &systemConfig_Copy); - cliPrintHashLine("serial"); printSerial(dumpMask, &serialConfig_Copy, serialConfig()); @@ -2681,23 +2711,35 @@ static void printConfig(const char *cmdline, bool doDiff) if (dumpMask & DUMP_ALL) { // dump all profiles const int currentProfileIndexSave = getConfigProfile(); + const int currentBatteryProfileIndexSave = getConfigBatteryProfile(); for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) { cliDumpProfile(ii, dumpMask); } + for (int ii = 0; ii < MAX_BATTERY_PROFILE_COUNT; ++ii) { + cliDumpBatteryProfile(ii, dumpMask); + } setConfigProfile(currentProfileIndexSave); + setConfigBatteryProfile(currentBatteryProfileIndexSave); + cliPrintHashLine("restore original profile selection"); cliPrintLinef("profile %d", currentProfileIndexSave + 1); + cliPrintLinef("battery_profile %d", currentBatteryProfileIndexSave + 1); cliPrintHashLine("save configuration\r\nsave"); } else { - // dump just the current profile + // dump just the current profiles cliDumpProfile(getConfigProfile(), dumpMask); + cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask); } } if (dumpMask & DUMP_PROFILE) { cliDumpProfile(getConfigProfile(), dumpMask); } + + if (dumpMask & DUMP_BATTERY_PROFILE) { + cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask); + } // restore configs from copies restoreConfigs(); } @@ -2760,9 +2802,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), CLI_COMMAND_DEF("diff", "list configuration changes from default", - "[master|profile|rates|all] {showdefaults}", cliDiff), + "[master|battery_profile|profile|rates|all] {showdefaults}", cliDiff), CLI_COMMAND_DEF("dump", "dump configuration", - "[master|profile|rates|all] {showdefaults}", cliDump), + "[master|battery_profile|profile|rates|all] {showdefaults}", cliDump), #ifdef USE_RX_ELERES CLI_COMMAND_DEF("eleres_bind", NULL, NULL, cliEleresBind), #endif // USE_RX_ELERES @@ -2790,12 +2832,13 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("memory", "view memory usage", NULL, cliMemory), CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix), CLI_COMMAND_DEF("motor", "get/set motor", " []", cliMotor), - CLI_COMMAND_DEF("name", "name of craft", NULL, cliName), #ifdef PLAY_SOUND CLI_COMMAND_DEF("play_sound", NULL, "[]\r\n", cliPlaySound), #endif CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), + CLI_COMMAND_DEF("battery_profile", "change battery profile", + "[]", cliBatteryProfile), #if !defined(SKIP_TASK_STATISTICS) && !defined(SKIP_CLI_RESOURCES) CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), #endif @@ -2975,4 +3018,4 @@ void cliEnter(serialPort_t *serialPort) void cliInit(const serialConfig_t *serialConfig) { UNUSED(serialConfig); -} \ No newline at end of file +} diff --git a/src/main/fc/config.c b/src/main/fc/config.c index a97804728a..f67fe44a76 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -109,6 +109,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, + .current_battery_profile_index = 0, .debug_mode = DEBUG_NONE, .i2c_speed = I2C_SPEED_400KHZ, .cpuUnderclock = 0, @@ -222,7 +223,7 @@ void validateAndFixConfig(void) #endif // Disable unused features - featureClear(FEATURE_UNUSED_1 | FEATURE_UNUSED_2 | FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 ); + featureClear(FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 ); #if defined(DISABLE_RX_PWM_FEATURE) || !defined(USE_RX_PWM) if (rxConfig()->receiverType == RX_TYPE_PWM) { @@ -367,6 +368,7 @@ void resetConfigs(void) static void activateConfig(void) { activateControlRateConfig(); + activateBatteryProfile(); resetAdjustmentStates(); @@ -396,6 +398,7 @@ void readEEPROM(void) } setConfigProfile(getConfigProfile()); + setConfigBatteryProfile(getConfigBatteryProfile()); validateAndFixConfig(); activateConfig(); @@ -464,6 +467,35 @@ void setConfigProfileAndWriteEEPROM(uint8_t profileIndex) beeperConfirmationBeeps(profileIndex + 1); } +uint8_t getConfigBatteryProfile(void) +{ + return systemConfig()->current_battery_profile_index; +} + +bool setConfigBatteryProfile(uint8_t profileIndex) +{ + bool ret = true; // return true if current_battery_profile_index has changed + if (systemConfig()->current_battery_profile_index == profileIndex) { + ret = false; + } + if (profileIndex >= MAX_BATTERY_PROFILE_COUNT) {// sanity check + profileIndex = 0; + } + systemConfigMutable()->current_battery_profile_index = profileIndex; + setBatteryProfile(profileIndex); + return ret; +} + +void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex) +{ + if (setConfigBatteryProfile(profileIndex)) { + // profile has changed, so ensure current values saved before new profile is loaded + writeEEPROM(); + readEEPROM(); + } + beeperConfirmationBeeps(profileIndex + 1); +} + void beeperOffSet(uint32_t mask) { beeperConfigMutable()->beeper_off_flags |= mask; diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 0f2cfb8c59..05cb7f2a4b 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -43,10 +43,10 @@ typedef enum { } asyncMode_e; typedef enum { - FEATURE_UNUSED_1 = 1 << 0, // RX_PPM + FEATURE_THR_VBAT_COMP = 1 << 0, FEATURE_VBAT = 1 << 1, FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command - FEATURE_UNUSED_2 = 1 << 3, // RX_SERIAL + FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3, FEATURE_MOTOR_STOP = 1 << 4, NOT_USED_10 = 1 << 5, // was FEATURE_SERVO_TILT FEATURE_SOFTSERIAL = 1 << 6, @@ -81,6 +81,7 @@ typedef struct systemConfig_s { uint16_t accTaskFrequency; uint16_t attitudeTaskFrequency; uint8_t current_profile_index; + uint8_t current_battery_profile_index; uint8_t asyncMode; uint8_t debug_mode; uint8_t i2c_speed; @@ -134,6 +135,10 @@ uint8_t getConfigProfile(void); bool setConfigProfile(uint8_t profileIndex); void setConfigProfileAndWriteEEPROM(uint8_t profileIndex); +uint8_t getConfigBatteryProfile(void); +bool setConfigBatteryProfile(uint8_t profileIndex); +void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex); + bool canSoftwareSerialBeUsed(void); void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 7ab7d2ab1d..0e1759ec41 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -94,6 +94,7 @@ enum { #define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro 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; float dT; @@ -373,6 +374,8 @@ void tryArm(void) return; } + lastDisarmReason = DISARM_NONE; + ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); @@ -480,9 +483,10 @@ void processRx(timeUs_t currentTimeUs) updateActivatedModes(); - if ((!cliMode) && (!FLIGHT_MODE(FAILSAFE_MODE))) { - updateAdjustmentStates(); - processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile)); + if (!cliMode) { + bool canUseRxData = rxIsReceivingSignal() && !FLIGHT_MODE(FAILSAFE_MODE); + updateAdjustmentStates(canUseRxData); + processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData); } bool canUseHorizonMode = true; @@ -702,6 +706,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs) cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; + if (ARMING_FLAG(ARMED) && ((!STATE(FIXED_WING)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected()))) { + flightTime += cycleTime; + } + #ifdef USE_ASYNC_GYRO_PROCESSING if (getAsyncMode() == ASYNC_MODE_NONE) { taskGyro(currentTimeUs); @@ -823,3 +831,8 @@ void taskUpdateRxMain(timeUs_t currentTimeUs) processRx(currentTimeUs); isRXDataNew = true; } + +// returns seconds +float getFlightTime() { + return (float)(flightTime / 1000) / 1000; +} diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index be1d29ae49..c8de27bd01 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -17,6 +17,8 @@ #pragma once +#include "common/time.h" + typedef enum disarmReason_e { DISARM_NONE = 0, DISARM_TIMEOUT = 1, @@ -29,6 +31,7 @@ typedef enum disarmReason_e { DISARM_REASON_COUNT } disarmReason_t; + void handleInflightCalibrationStickPosition(void); void disarm(disarmReason_t disarmReason); @@ -36,3 +39,4 @@ void tryArm(void); disarmReason_t getDisarmReason(void); bool isCalibrating(void); +float getFlightTime(); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index d195bb1296..dabbc43d1e 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -103,6 +103,7 @@ #include "io/rcdevice_cam.h" #include "io/serial.h" #include "io/displayport_msp.h" +#include "io/vtx.h" #include "io/vtx_control.h" #include "io/vtx_smartaudio.h" #include "io/vtx_tramp.h" @@ -302,7 +303,7 @@ void init(void) pwm_params.useParallelPWM = (rxConfig()->receiverType == RX_TYPE_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) - && batteryConfig()->current.type == CURRENT_SENSOR_ADC; + && 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); @@ -473,7 +474,7 @@ void init(void) adc_params.adcFunctionChannel[ADC_RSSI] = adcChannelConfig()->adcFunctionChannel[ADC_RSSI]; } - if (feature(FEATURE_CURRENT_METER) && batteryConfig()->current.type == CURRENT_SENSOR_ADC) { + if (feature(FEATURE_CURRENT_METER) && batteryMetersConfig()->current.type == CURRENT_SENSOR_ADC) { adc_params.adcFunctionChannel[ADC_CURRENT] = adcChannelConfig()->adcFunctionChannel[ADC_CURRENT]; } @@ -557,9 +558,7 @@ void init(void) if (feature(FEATURE_OSD)) { #if defined(USE_MAX7456) // If there is a max7456 chip for the OSD then use it - static vcdProfile_t vcdProfile; - vcdProfile.video_system = osdConfig()->video_system; - osdDisplayPort = max7456DisplayPortInit(&vcdProfile); + osdDisplayPort = max7456DisplayPortInit(osdConfig()->video_system); #elif defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet) osdDisplayPort = displayPortMspInit(); #endif @@ -653,20 +652,23 @@ void init(void) pitotStartCalibration(); #endif -#ifdef VTX_CONTROL +#ifdef USE_VTX_CONTROL vtxControlInit(); +#if defined(USE_VTX_COMMON) vtxCommonInit(); + vtxInit(); +#endif -#ifdef VTX_SMARTAUDIO +#ifdef USE_VTX_SMARTAUDIO vtxSmartAudioInit(); #endif -#ifdef VTX_TRAMP +#ifdef USE_VTX_TRAMP vtxTrampInit(); #endif -#endif // VTX_CONTROL +#endif // USE_VTX_CONTROL // Now that everything has powered up the voltage and cell count be determined. if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 3ea1b5dd97..6455bc2ac2 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -76,6 +76,8 @@ #include "io/osd.h" #include "io/serial.h" #include "io/serial_4way.h" +#include "io/vtx.h" +#include "io/vtx_string.h" #include "msp/msp.h" #include "msp/msp_protocol.h" @@ -419,7 +421,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #endif sbufWriteU16(dst, packSensorStatus()); sbufWriteU16(dst, averageSystemLoadPercent); - sbufWriteU8(dst, getConfigProfile()); + sbufWriteU8(dst, (getConfigBatteryProfile() << 4) | getConfigProfile()); sbufWriteU32(dst, armingFlags); sbufWriteData(dst, &mspBoxModeFlags, sizeof(mspBoxModeFlags)); } @@ -539,16 +541,16 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_ANALOG: + // Bit 1: battery full, Bit 2: use capacity threshold, Bit 3-4: battery state, Bit 5-8: battery cell count + sbufWriteU8(dst, batteryWasFullWhenPluggedIn() | (batteryUsesCapacityThresholds() << 1) | (getBatteryState() << 2) | (getBatteryCellCount() << 4)); sbufWriteU16(dst, getBatteryVoltage()); - sbufWriteU8(dst, getBatteryCellCount()); - sbufWriteU8(dst, calculateBatteryPercentage()); - sbufWriteU16(dst, constrain(getPower(), 0, 0x7FFFFFFF)); // power draw - sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery - sbufWriteU16(dst, (uint16_t)constrain(getMWhDrawn(), 0, 0xFFFF)); // milliWatt hours drawn from battery - sbufWriteU16(dst, getRSSI()); - sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A - sbufWriteU8(dst, batteryWasFullWhenPluggedIn() | (batteryUsesCapacityThresholds() << 1) | (getBatteryState() << 2)); + sbufWriteU16(dst, getAmperage()); // send amperage in 0.01 A steps + sbufWriteU32(dst, getPower()); // power draw + sbufWriteU32(dst, getMAhDrawn()); // milliamp hours drawn from battery + sbufWriteU32(dst, getMWhDrawn()); // milliWatt hours drawn from battery sbufWriteU32(dst, getBatteryRemainingCapacity()); + sbufWriteU8(dst, calculateBatteryPercentage()); + sbufWriteU16(dst, getRSSI()); break; case MSP_ARMING_CONFIG: @@ -670,10 +672,10 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, compassConfig()->mag_declination / 10); - sbufWriteU8(dst, batteryConfig()->voltage.scale / 10); - sbufWriteU8(dst, batteryConfig()->voltage.cellMin / 10); - sbufWriteU8(dst, batteryConfig()->voltage.cellMax / 10); - sbufWriteU8(dst, batteryConfig()->voltage.cellWarning / 10); + sbufWriteU8(dst, batteryMetersConfig()->voltage_scale / 10); + sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10); + sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10); + sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10); break; case MSP2_INAV_MISC: @@ -698,32 +700,36 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, compassConfig()->mag_declination / 10); - sbufWriteU16(dst, batteryConfig()->voltage.scale); - sbufWriteU16(dst, batteryConfig()->voltage.cellDetect); - sbufWriteU16(dst, batteryConfig()->voltage.cellMin); - sbufWriteU16(dst, batteryConfig()->voltage.cellMax); - sbufWriteU16(dst, batteryConfig()->voltage.cellWarning); + sbufWriteU16(dst, batteryMetersConfig()->voltage_scale); + sbufWriteU8(dst, batteryMetersConfig()->voltageSource); + sbufWriteU8(dst, currentBatteryProfile->cells); + sbufWriteU16(dst, currentBatteryProfile->voltage.cellDetect); + sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin); + sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax); + sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning); - sbufWriteU32(dst, batteryConfig()->capacity.value); - sbufWriteU32(dst, batteryConfig()->capacity.warning); - sbufWriteU32(dst, batteryConfig()->capacity.critical); - sbufWriteU8(dst, batteryConfig()->capacity.unit); + sbufWriteU32(dst, currentBatteryProfile->capacity.value); + sbufWriteU32(dst, currentBatteryProfile->capacity.warning); + sbufWriteU32(dst, currentBatteryProfile->capacity.critical); + sbufWriteU8(dst, currentBatteryProfile->capacity.unit); break; case MSP2_INAV_BATTERY_CONFIG: - sbufWriteU16(dst, batteryConfig()->voltage.scale); - sbufWriteU16(dst, batteryConfig()->voltage.cellDetect); - sbufWriteU16(dst, batteryConfig()->voltage.cellMin); - sbufWriteU16(dst, batteryConfig()->voltage.cellMax); - sbufWriteU16(dst, batteryConfig()->voltage.cellWarning); + sbufWriteU16(dst, batteryMetersConfig()->voltage_scale); + sbufWriteU8(dst, batteryMetersConfig()->voltageSource); + sbufWriteU8(dst, currentBatteryProfile->cells); + sbufWriteU16(dst, currentBatteryProfile->voltage.cellDetect); + sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin); + sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax); + sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning); - sbufWriteU16(dst, batteryConfig()->current.offset); - sbufWriteU16(dst, batteryConfig()->current.scale); + sbufWriteU16(dst, batteryMetersConfig()->current.offset); + sbufWriteU16(dst, batteryMetersConfig()->current.scale); - sbufWriteU32(dst, batteryConfig()->capacity.value); - sbufWriteU32(dst, batteryConfig()->capacity.warning); - sbufWriteU32(dst, batteryConfig()->capacity.critical); - sbufWriteU8(dst, batteryConfig()->capacity.unit); + sbufWriteU32(dst, currentBatteryProfile->capacity.value); + sbufWriteU32(dst, currentBatteryProfile->capacity.warning); + sbufWriteU32(dst, currentBatteryProfile->capacity.critical); + sbufWriteU8(dst, currentBatteryProfile->capacity.unit); break; case MSP_MOTOR_PINS: @@ -809,17 +815,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP_VOLTAGE_METER_CONFIG: - sbufWriteU8(dst, batteryConfig()->voltage.scale / 10); - sbufWriteU8(dst, batteryConfig()->voltage.cellMin / 10); - sbufWriteU8(dst, batteryConfig()->voltage.cellMax / 10); - sbufWriteU8(dst, batteryConfig()->voltage.cellWarning / 10); + sbufWriteU8(dst, batteryMetersConfig()->voltage_scale / 10); + sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10); + sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10); + sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10); break; case MSP_CURRENT_METER_CONFIG: - sbufWriteU16(dst, batteryConfig()->current.scale); - sbufWriteU16(dst, batteryConfig()->current.offset); - sbufWriteU8(dst, batteryConfig()->current.type); - sbufWriteU16(dst, constrain(batteryConfig()->capacity.value, 0, 0xFFFF)); + sbufWriteU16(dst, batteryMetersConfig()->current.scale); + sbufWriteU16(dst, batteryMetersConfig()->current.offset); + sbufWriteU8(dst, batteryMetersConfig()->current.type); + sbufWriteU16(dst, constrain(currentBatteryProfile->capacity.value, 0, 0xFFFF)); break; case MSP_MIXER: @@ -879,8 +885,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, boardAlignment()->pitchDeciDegrees); sbufWriteU16(dst, boardAlignment()->yawDeciDegrees); - sbufWriteU16(dst, batteryConfig()->current.scale); - sbufWriteU16(dst, batteryConfig()->current.offset); + sbufWriteU16(dst, batteryMetersConfig()->current.scale); + sbufWriteU16(dst, batteryMetersConfig()->current.offset); break; case MSP_CF_SERIAL_CONFIG: @@ -964,7 +970,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #endif sbufWriteU8(dst, osdConfig()->units); sbufWriteU8(dst, osdConfig()->rssi_alarm); - sbufWriteU16(dst, batteryConfig()->capacity.warning); + sbufWriteU16(dst, currentBatteryProfile->capacity.warning); sbufWriteU16(dst, osdConfig()->time_alarm); sbufWriteU16(dst, osdConfig()->alt_alarm); sbufWriteU16(dst, osdConfig()->dist_alarm); @@ -1266,30 +1272,32 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; -#if defined(VTX_COMMON) +#if defined(USE_VTX_COMMON) case MSP_VTX_CONFIG: { - uint8_t deviceType = vtxCommonGetDeviceType(); - if (deviceType != VTXDEV_UNKNOWN) { + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { - uint8_t band=0, channel=0; - vtxCommonGetBandAndChannel(&band,&channel); + uint8_t deviceType = vtxCommonGetDeviceType(vtxDevice); - uint8_t powerIdx=0; // debug - vtxCommonGetPowerIndex(&powerIdx); - - uint8_t pitmode=0; - vtxCommonGetPitMode(&pitmode); + // Return band, channel and power from vtxSettingsConfig_t + // since the VTX might be configured but temporarily offline. + uint8_t pitmode = 0; + vtxCommonGetPitMode(vtxDevice, &pitmode); sbufWriteU8(dst, deviceType); - sbufWriteU8(dst, band); - sbufWriteU8(dst, channel); - sbufWriteU8(dst, powerIdx); + sbufWriteU8(dst, vtxSettingsConfig()->band); + sbufWriteU8(dst, vtxSettingsConfig()->channel); + sbufWriteU8(dst, vtxSettingsConfig()->power); sbufWriteU8(dst, pitmode); + + // Betaflight < 4 doesn't send these fields + sbufWriteU8(dst, vtxCommonDeviceIsReady(vtxDevice) ? 1 : 0); + sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm); // future extensions here... } else { - sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX detected + sbufWriteU8(dst, VTXDEV_UNKNOWN); // no VTX configured } } break; @@ -1327,16 +1335,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; #if defined(USE_OSD) - case MSP2_INAV_OSD_LAYOUTS: - sbufWriteU8(dst, OSD_LAYOUT_COUNT); - sbufWriteU8(dst, OSD_ITEM_COUNT); - for (unsigned ii = 0; ii < OSD_LAYOUT_COUNT; ii++) { - for (unsigned jj = 0; jj < OSD_ITEM_COUNT; jj++) { - sbufWriteU16(dst, osdConfig()->item_pos[ii][jj]); - } - } - break; - case MSP2_INAV_OSD_ALARMS: sbufWriteU8(dst, osdConfig()->rssi_alarm); sbufWriteU16(dst, osdConfig()->time_alarm); @@ -1426,6 +1424,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) { uint8_t tmp_u8; uint16_t tmp_u16; + batteryProfile_t *currentBatteryProfileMutable = (batteryProfile_t*)currentBatteryProfile; const unsigned int dataSize = sbufBytesRemaining(src); @@ -1636,16 +1635,16 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU16(src); #endif - batteryConfigMutable()->voltage.scale = sbufReadU8(src) * 10; - batteryConfigMutable()->voltage.cellMin = sbufReadU8(src) * 10; // vbatlevel_warn1 in MWC2.3 GUI - batteryConfigMutable()->voltage.cellMax = sbufReadU8(src) * 10; // vbatlevel_warn2 in MWC2.3 GUI - batteryConfigMutable()->voltage.cellWarning = sbufReadU8(src) * 10; // vbatlevel when buzzer starts to alert + batteryMetersConfigMutable()->voltage_scale = sbufReadU8(src) * 10; + currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10; // vbatlevel_warn1 in MWC2.3 GUI + currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10; // vbatlevel_warn2 in MWC2.3 GUI + currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10; // vbatlevel when buzzer starts to alert } else return MSP_RESULT_ERROR; break; case MSP2_INAV_SET_MISC: - if (dataSize == 39) { + if (dataSize == 41) { sbufReadU16(src); // midrc motorConfigMutable()->minthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX); @@ -1674,18 +1673,24 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU16(src); #endif - batteryConfigMutable()->voltage.scale = sbufReadU16(src); - batteryConfigMutable()->voltage.cellDetect = sbufReadU16(src); - batteryConfigMutable()->voltage.cellMin = sbufReadU16(src); - batteryConfigMutable()->voltage.cellMax = sbufReadU16(src); - batteryConfigMutable()->voltage.cellWarning = sbufReadU16(src); + batteryMetersConfigMutable()->voltage_scale = sbufReadU16(src); + batteryMetersConfigMutable()->voltageSource = sbufReadU8(src); + currentBatteryProfileMutable->cells = sbufReadU8(src); + currentBatteryProfileMutable->voltage.cellDetect = sbufReadU16(src); + currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src); + currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src); + currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src); - batteryConfigMutable()->capacity.value = sbufReadU32(src); - batteryConfigMutable()->capacity.warning = sbufReadU32(src); - batteryConfigMutable()->capacity.critical = sbufReadU32(src); - batteryConfigMutable()->capacity.unit = sbufReadU8(src); - if ((batteryConfig()->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (batteryConfig()->capacity.unit != BAT_CAPACITY_UNIT_MWH)) { - batteryConfigMutable()->capacity.unit = BAT_CAPACITY_UNIT_MAH; + currentBatteryProfileMutable->capacity.value = sbufReadU32(src); + currentBatteryProfileMutable->capacity.warning = sbufReadU32(src); + currentBatteryProfileMutable->capacity.critical = sbufReadU32(src); + currentBatteryProfileMutable->capacity.unit = sbufReadU8(src); + if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) { + batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW; + return MSP_RESULT_ERROR; + } + if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) { + currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH; return MSP_RESULT_ERROR; } } else @@ -1693,22 +1698,28 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP2_INAV_SET_BATTERY_CONFIG: - if (dataSize == 27) { - batteryConfigMutable()->voltage.scale = sbufReadU16(src); - batteryConfigMutable()->voltage.cellDetect = sbufReadU16(src); - batteryConfigMutable()->voltage.cellMin = sbufReadU16(src); - batteryConfigMutable()->voltage.cellMax = sbufReadU16(src); - batteryConfigMutable()->voltage.cellWarning = sbufReadU16(src); + if (dataSize == 29) { + batteryMetersConfigMutable()->voltage_scale = sbufReadU16(src); + batteryMetersConfigMutable()->voltageSource = sbufReadU8(src); + currentBatteryProfileMutable->cells = sbufReadU8(src); + currentBatteryProfileMutable->voltage.cellDetect = sbufReadU16(src); + currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src); + currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src); + currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src); - batteryConfigMutable()->current.offset = sbufReadU16(src); - batteryConfigMutable()->current.scale = sbufReadU16(src); + batteryMetersConfigMutable()->current.offset = sbufReadU16(src); + batteryMetersConfigMutable()->current.scale = sbufReadU16(src); - batteryConfigMutable()->capacity.value = sbufReadU32(src); - batteryConfigMutable()->capacity.warning = sbufReadU32(src); - batteryConfigMutable()->capacity.critical = sbufReadU32(src); - batteryConfigMutable()->capacity.unit = sbufReadU8(src); - if ((batteryConfig()->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (batteryConfig()->capacity.unit != BAT_CAPACITY_UNIT_MWH)) { - batteryConfigMutable()->capacity.unit = BAT_CAPACITY_UNIT_MAH; + currentBatteryProfileMutable->capacity.value = sbufReadU32(src); + currentBatteryProfileMutable->capacity.warning = sbufReadU32(src); + currentBatteryProfileMutable->capacity.critical = sbufReadU32(src); + currentBatteryProfileMutable->capacity.unit = sbufReadU8(src); + if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) { + batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW; + return MSP_RESULT_ERROR; + } + if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) { + currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH; return MSP_RESULT_ERROR; } } else @@ -2100,7 +2111,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #endif osdConfigMutable()->units = sbufReadU8(src); osdConfigMutable()->rssi_alarm = sbufReadU8(src); - batteryConfigMutable()->capacity.warning = sbufReadU16(src); + currentBatteryProfileMutable->capacity.warning = sbufReadU16(src); osdConfigMutable()->time_alarm = sbufReadU16(src); osdConfigMutable()->alt_alarm = sbufReadU16(src); // Won't be read if they weren't provided @@ -2138,36 +2149,44 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; #endif // USE_OSD -#if defined(VTX_COMMON) +#if defined(USE_VTX_COMMON) case MSP_SET_VTX_CONFIG: - if (dataSize >= 4) { - tmp_u16 = sbufReadU16(src); - const uint8_t band = (tmp_u16 / 8) + 1; - const uint8_t channel = (tmp_u16 % 8) + 1; + if (dataSize >= 2) { + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { + if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) { + uint16_t newFrequency = sbufReadU16(src); + if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel + const uint8_t newBand = (newFrequency / 8) + 1; + const uint8_t newChannel = (newFrequency % 8) + 1; + vtxSettingsConfigMutable()->band = newBand; + vtxSettingsConfigMutable()->channel = newChannel; + vtxSettingsConfigMutable()->freq = vtx58_Bandchan2Freq(newBand, newChannel); + } else if (newFrequency <= VTX_SETTINGS_MAX_FREQUENCY_MHZ) { //value is frequency in MHz. Ignore it if it's invalid + vtxSettingsConfigMutable()->band = 0; + vtxSettingsConfigMutable()->channel = 0; + vtxSettingsConfigMutable()->freq = newFrequency; + } - if (vtxCommonGetDeviceType() != VTXDEV_UNKNOWN) { - uint8_t current_band=0, current_channel=0; - vtxCommonGetBandAndChannel(¤t_band,¤t_channel); - if ((current_band != band) || (current_channel != channel)) - vtxCommonSetBandAndChannel(band,channel); + if (sbufBytesRemaining(src) > 1) { + vtxSettingsConfigMutable()->power = sbufReadU8(src); + // Delegate pitmode to vtx directly + const uint8_t newPitmode = sbufReadU8(src); + uint8_t currentPitmode = 0; + vtxCommonGetPitMode(vtxDevice, ¤tPitmode); + if (currentPitmode != newPitmode) { + vtxCommonSetPitMode(vtxDevice, newPitmode); + } - if (sbufBytesRemaining(src) < 2) - break; - - uint8_t power = sbufReadU8(src); - uint8_t current_power = 0; - vtxCommonGetPowerIndex(¤t_power); - if (current_power != power) - vtxCommonSetPowerByIndex(power); - - uint8_t pitmode = sbufReadU8(src); - uint8_t current_pitmode = 0; - vtxCommonGetPitMode(¤t_pitmode); - if (current_pitmode != pitmode) - vtxCommonSetPitMode(pitmode); + if (sbufBytesRemaining(src) > 0) { + vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src); + } + } + } } - } else + } else { return MSP_RESULT_ERROR; + } break; #endif @@ -2245,20 +2264,20 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_VOLTAGE_METER_CONFIG: if (dataSize >= 4) { - batteryConfigMutable()->voltage.scale = sbufReadU8(src) * 10; - batteryConfigMutable()->voltage.cellMin = sbufReadU8(src) * 10; - batteryConfigMutable()->voltage.cellMax = sbufReadU8(src) * 10; - batteryConfigMutable()->voltage.cellWarning = sbufReadU8(src) * 10; + batteryMetersConfigMutable()->voltage_scale = sbufReadU8(src) * 10; + currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10; + currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10; + currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10; } else return MSP_RESULT_ERROR; break; case MSP_SET_CURRENT_METER_CONFIG: if (dataSize >= 7) { - batteryConfigMutable()->current.scale = sbufReadU16(src); - batteryConfigMutable()->current.offset = sbufReadU16(src); - batteryConfigMutable()->current.type = sbufReadU8(src); - batteryConfigMutable()->capacity.value = sbufReadU16(src); + batteryMetersConfigMutable()->current.scale = sbufReadU16(src); + batteryMetersConfigMutable()->current.offset = sbufReadU16(src); + batteryMetersConfigMutable()->current.type = sbufReadU8(src); + currentBatteryProfileMutable->capacity.value = sbufReadU16(src); } else return MSP_RESULT_ERROR; break; @@ -2342,8 +2361,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) boardAlignmentMutable()->pitchDeciDegrees = sbufReadU16(src); // board_align_pitch boardAlignmentMutable()->yawDeciDegrees = sbufReadU16(src); // board_align_yaw - batteryConfigMutable()->current.scale = sbufReadU16(src); - batteryConfigMutable()->current.offset = sbufReadU16(src); + batteryMetersConfigMutable()->current.scale = sbufReadU16(src); + batteryMetersConfigMutable()->current.offset = sbufReadU16(src); } else return MSP_RESULT_ERROR; break; @@ -2538,6 +2557,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) navConfigMutable()->mc.braking_boost_disengage_speed = sbufReadU16(src); navConfigMutable()->mc.braking_bank_angle = sbufReadU8(src); break; + + case MSP2_INAV_SELECT_BATTERY_PROFILE: + if (!ARMING_FLAG(ARMED)) { + if (sbufReadU8Safe(&tmp_u8, src)) + setConfigBatteryProfileAndWriteEEPROM(tmp_u8); + } + break; default: return MSP_RESULT_ERROR; @@ -2563,7 +2589,7 @@ static const setting_t *mspReadSettingName(sbuf_t *src) return NULL; } } - return setting_find(name); + return settingFind(name); } static bool mspSettingCommand(sbuf_t *dst, sbuf_t *src) @@ -2573,8 +2599,8 @@ static bool mspSettingCommand(sbuf_t *dst, sbuf_t *src) return false; } - const void *ptr = setting_get_value_pointer(setting); - size_t size = setting_get_value_size(setting); + const void *ptr = settingGetValuePointer(setting); + size_t size = settingGetValueSize(setting); sbufWriteDataSafe(dst, ptr, size); return true; } @@ -2588,10 +2614,10 @@ static bool mspSetSettingCommand(sbuf_t *dst, sbuf_t *src) return false; } - setting_min_t min = setting_get_min(setting); - setting_max_t max = setting_get_max(setting); + setting_min_t min = settingGetMin(setting); + setting_max_t max = settingGetMax(setting); - void *ptr = setting_get_value_pointer(setting); + void *ptr = settingGetValuePointer(setting); switch (SETTING_TYPE(setting)) { case VAR_UINT8: { @@ -2665,11 +2691,80 @@ static bool mspSetSettingCommand(sbuf_t *dst, sbuf_t *src) *((float*)ptr) = val; } break; + case VAR_STRING: + { + settingSetString(setting, (const char*)sbufPtr(src), sbufBytesRemaining(src)); + } + break; } return true; } +bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResult_e *ret) +{ + switch (cmdMSP) { + +#if defined(USE_NAV) + case MSP_WP: + mspFcWaypointOutCommand(dst, src); + *ret = MSP_RESULT_ACK; + break; +#endif + +#if defined(USE_FLASHFS) + case MSP_DATAFLASH_READ: + mspFcDataFlashReadCommand(dst, src); + *ret = MSP_RESULT_ACK; + break; +#endif + + case MSP2_COMMON_SETTING: + *ret = mspSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR; + break; + + case MSP2_COMMON_SET_SETTING: + *ret = mspSetSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR; + break; + +#if defined(USE_OSD) + case MSP2_INAV_OSD_LAYOUTS: + if (sbufBytesRemaining(src) >= 1) { + uint8_t layout = sbufReadU8(src); + if (layout >= OSD_LAYOUT_COUNT) { + *ret = MSP_RESULT_ERROR; + break; + } + if (sbufBytesRemaining(src) >= 2) { + // Asking for an specific item in a layout + uint16_t item = sbufReadU16(src); + if (item >= OSD_ITEM_COUNT) { + *ret = MSP_RESULT_ERROR; + break; + } + sbufWriteU16(dst, osdConfig()->item_pos[layout][item]); + } else { + // Asking for an specific layout + for (unsigned ii = 0; ii < OSD_ITEM_COUNT; ii++) { + sbufWriteU16(dst, osdConfig()->item_pos[layout][ii]); + } + } + } else { + // Return the number of layouts and items + sbufWriteU8(dst, OSD_LAYOUT_COUNT); + sbufWriteU8(dst, OSD_ITEM_COUNT); + } + *ret = MSP_RESULT_ACK; + break; +#endif + + default: + // Not handled + return false; + } + return true; +} + static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src) { UNUSED(src); @@ -2696,7 +2791,7 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src) */ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) { - int ret = MSP_RESULT_ACK; + mspResult_e ret = MSP_RESULT_ACK; sbuf_t *dst = &reply->buf; sbuf_t *src = &cmd->buf; const uint16_t cmdMSP = cmd->cmd; @@ -2712,22 +2807,10 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro mspFc4waySerialCommand(dst, src, mspPostProcessFn); ret = MSP_RESULT_ACK; #endif -#ifdef USE_NAV - } else if (cmdMSP == MSP_WP) { - mspFcWaypointOutCommand(dst, src); - ret = MSP_RESULT_ACK; -#endif -#ifdef USE_FLASHFS - } else if (cmdMSP == MSP_DATAFLASH_READ) { - mspFcDataFlashReadCommand(dst, src); - ret = MSP_RESULT_ACK; -#endif - } else if (cmdMSP == MSP2_COMMON_SETTING) { - ret = mspSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR; - } else if (cmdMSP == MSP2_COMMON_SET_SETTING) { - ret = mspSetSettingCommand(dst, src) ? MSP_RESULT_ACK : MSP_RESULT_ERROR; } else { - ret = mspFcProcessInCommand(cmdMSP, src); + if (!mspFCProcessInOutCommand(cmdMSP, dst, src, &ret)) { + ret = mspFcProcessInCommand(cmdMSP, src); + } } // Process DONT_REPLY flag diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 4d8cfb088c..b5b9b945be 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -79,7 +79,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXOSDALT1, "OSD ALT 1", 42 }, { BOXOSDALT2, "OSD ALT 2", 43 }, { BOXOSDALT3, "OSD ALT 3", 44 }, - { BOXBRAKING, "MC BRAKING", 45 }, + { BOXNAVCRUISE, "NAV CRUISE", 45 }, + { BOXBRAKING, "MC BRAKING", 46 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -184,12 +185,24 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXSURFACE; } - if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) { + const bool navReadyQuads = !STATE(FIXED_WING) && (getHwCompassStatus() != HW_SENSOR_NONE) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); + const bool navReadyPlanes = STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS); + const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; + if (navFlowDeadReckoning || navReadyQuads || navReadyPlanes) { activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; + } + + if (navReadyQuads || navReadyPlanes) { activeBoxIds[activeBoxIdCount++] = BOXNAVRTH; activeBoxIds[activeBoxIdCount++] = BOXNAVWP; activeBoxIds[activeBoxIdCount++] = BOXHOMERESET; - activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; + + if (feature(FEATURE_GPS)) { + activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; + if (STATE(FIXED_WING)) { + activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; + } + } } if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) { @@ -292,6 +305,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) 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); @@ -331,7 +345,7 @@ uint16_t packSensorStatus(void) IS_ENABLED(sensors(SENSOR_MAG)) << 2 | IS_ENABLED(sensors(SENSOR_GPS)) << 3 | IS_ENABLED(sensors(SENSOR_RANGEFINDER)) << 4 | - //IS_ENABLED(sensors(SENSOR_OPFLOW)) << 5 | + IS_ENABLED(sensors(SENSOR_OPFLOW)) << 5 | IS_ENABLED(sensors(SENSOR_PITOT)) << 6; // Hardware failure indication bit diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 95eeed53f1..2bebfb1a05 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -32,7 +32,6 @@ #include "drivers/sensor.h" #include "drivers/serial.h" #include "drivers/stack_check.h" -#include "drivers/vtx_common.h" #include "fc/cli.h" #include "fc/config.h" @@ -58,6 +57,7 @@ #include "io/pwmdriver_i2c.h" #include "io/serial.h" #include "io/rcdevice_cam.h" +#include "io/vtx.h" #include "msp/msp_serial.h" @@ -108,7 +108,7 @@ void taskUpdateBattery(timeUs_t currentTimeUs) batteryUpdate(BatMonitoringTimeSinceLastServiced); if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) { powerMeterUpdate(BatMonitoringTimeSinceLastServiced); - sagCompensatedVBatUpdate(currentTimeUs); + sagCompensatedVBatUpdate(currentTimeUs, BatMonitoringTimeSinceLastServiced); } #endif batMonitoringLastServiced = currentTimeUs; @@ -273,19 +273,6 @@ void taskUpdateOsd(timeUs_t currentTimeUs) } #endif -#ifdef VTX_CONTROL -// Everything that listens to VTX devices -void taskVtxControl(timeUs_t currentTimeUs) -{ - if (ARMING_FLAG(ARMED)) - return; - -#ifdef VTX_COMMON - vtxCommonProcess(currentTimeUs); -#endif -} -#endif - void fcTasksInit(void) { schedulerInit(); @@ -369,8 +356,8 @@ void fcTasksInit(void) #ifdef USE_OPTICAL_FLOW setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW)); #endif -#ifdef VTX_CONTROL -#if defined(VTX_SMARTAUDIO) || defined(VTX_TRAMP) +#ifdef USE_VTX_CONTROL +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif @@ -616,10 +603,10 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#ifdef VTX_CONTROL +#ifdef USE_VTX_CONTROL [TASK_VTXCTRL] = { .taskName = "VTXCTRL", - .taskFunc = taskVtxControl, + .taskFunc = vtxUpdate, .desiredPeriod = TASK_PERIOD_HZ(5), // 5Hz @200msec .staticPriority = TASK_PRIORITY_IDLE, }, diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 01eb4541ad..675c702f8a 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -41,6 +41,7 @@ #include "navigation/navigation.h" +#include "flight/mixer.h" #include "flight/pid.h" #include "io/beeper.h" @@ -58,6 +59,15 @@ static uint8_t adjustmentStateMask = 0; #define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex)) +#define RC_ADJUSTMENT_EXPO_MIN 0 +#define RC_ADJUSTMENT_EXPO_MAX 100 + +#define RC_ADJUSTMENT_MANUAL_RATE_MIN 0 +#define RC_ADJUSTMENT_MANUAL_RATE_MAX 100 + +#define RC_ADJUSTMENT_PID_MIN 0 +#define RC_ADJUSTMENT_PID_MAX 200 + // sync with adjustmentFunction_e static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = { { @@ -184,6 +194,74 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_PITCH_BOARD_ALIGNMENT, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 5 }} + }, { + .adjustmentFunction = ADJUSTMENT_LEVEL_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_LEVEL_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_LEVEL_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_POS_XY_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_POS_XY_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_POS_XY_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_POS_Z_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_POS_Z_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_POS_Z_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_HEADING_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_VEL_XY_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_VEL_XY_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_VEL_XY_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_VEL_Z_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_VEL_Z_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_VEL_Z_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} + }, { + .adjustmentFunction = ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 5 }} #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT }, { .adjustmentFunction = ADJUSTMENT_PROFILE, @@ -246,6 +324,35 @@ static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustm } #endif +static void applyAdjustmentU8(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta, int low, int high) +{ + int newValue = constrain((int)(*val) + delta, low, high); + *val = newValue; + blackboxLogInflightAdjustmentEvent(adjustmentFunction, newValue); +} + +static void applyAdjustmentU16(adjustmentFunction_e adjustmentFunction, uint16_t *val, int delta, int low, int high) +{ + int newValue = constrain((int)(*val) + delta, low, high); + *val = newValue; + blackboxLogInflightAdjustmentEvent(adjustmentFunction, newValue); +} + +static void applyAdjustmentExpo(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta) +{ + applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_EXPO_MIN, RC_ADJUSTMENT_EXPO_MAX); +} + +static void applyAdjustmentManualRate(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta) +{ + return applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_MANUAL_RATE_MIN, RC_ADJUSTMENT_MANUAL_RATE_MAX); +} + +static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta) +{ + applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX); +} + static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { if (delta > 0) { @@ -253,39 +360,25 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t } else { beeperConfirmationBeeps(1); } - int newValue; switch (adjustmentFunction) { case ADJUSTMENT_RC_EXPO: - newValue = constrain((int)controlRateConfig->stabilized.rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c - controlRateConfig->stabilized.rcExpo8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue); + applyAdjustmentExpo(ADJUSTMENT_RC_EXPO, &controlRateConfig->stabilized.rcExpo8, delta); break; case ADJUSTMENT_RC_YAW_EXPO: - newValue = constrain((int)controlRateConfig->stabilized.rcYawExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c - controlRateConfig->stabilized.rcYawExpo8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_YAW_EXPO, newValue); + applyAdjustmentExpo(ADJUSTMENT_RC_YAW_EXPO, &controlRateConfig->stabilized.rcYawExpo8, delta); break; case ADJUSTMENT_MANUAL_RC_EXPO: - newValue = constrain((int)controlRateConfig->manual.rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c - controlRateConfig->manual.rcExpo8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_RC_EXPO, newValue); + applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_EXPO, &controlRateConfig->manual.rcExpo8, delta); break; case ADJUSTMENT_MANUAL_RC_YAW_EXPO: - newValue = constrain((int)controlRateConfig->manual.rcYawExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c - controlRateConfig->manual.rcYawExpo8 = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_RC_YAW_EXPO, newValue); + applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_YAW_EXPO, &controlRateConfig->manual.rcYawExpo8, delta); break; case ADJUSTMENT_THROTTLE_EXPO: - newValue = constrain((int)controlRateConfig->throttle.rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c - controlRateConfig->throttle.rcExpo8 = newValue; - generateThrottleCurve(controlRateConfig); - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); + applyAdjustmentExpo(ADJUSTMENT_THROTTLE_EXPO, &controlRateConfig->throttle.rcExpo8, delta); break; case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_RATE: - newValue = constrain((int)controlRateConfig->stabilized.rates[FD_PITCH] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); - controlRateConfig->stabilized.rates[FD_PITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); + applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { schedulePidGainsUpdate(); break; @@ -294,41 +387,29 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t FALLTHROUGH; case ADJUSTMENT_ROLL_RATE: - newValue = constrain((int)controlRateConfig->stabilized.rates[FD_ROLL] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); - controlRateConfig->stabilized.rates[FD_ROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); + applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); schedulePidGainsUpdate(); break; case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE: case ADJUSTMENT_MANUAL_ROLL_RATE: - newValue = constrain((int)controlRateConfig->manual.rates[FD_ROLL] + delta, 0, 100); - controlRateConfig->manual.rates[FD_ROLL] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_ROLL_RATE, newValue); + applyAdjustmentManualRate(ADJUSTMENT_MANUAL_ROLL_RATE, &controlRateConfig->manual.rates[FD_ROLL], delta); if (adjustmentFunction == ADJUSTMENT_MANUAL_ROLL_RATE) break; // follow though for combined ADJUSTMENT_MANUAL_PITCH_ROLL_RATE FALLTHROUGH; case ADJUSTMENT_MANUAL_PITCH_RATE: - newValue = constrain((int)controlRateConfig->manual.rates[FD_PITCH] + delta, 0, 100); - controlRateConfig->manual.rates[FD_PITCH] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_PITCH_RATE, newValue); + applyAdjustmentManualRate(ADJUSTMENT_MANUAL_PITCH_RATE, &controlRateConfig->manual.rates[FD_PITCH], delta); break; case ADJUSTMENT_YAW_RATE: - newValue = constrain((int)controlRateConfig->stabilized.rates[FD_YAW] + delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); - controlRateConfig->stabilized.rates[FD_YAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); + applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); schedulePidGainsUpdate(); break; case ADJUSTMENT_MANUAL_YAW_RATE: - newValue = constrain((int)controlRateConfig->manual.rates[FD_YAW] + delta, 0, 100); - controlRateConfig->manual.rates[FD_YAW] = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_MANUAL_YAW_RATE, newValue); + applyAdjustmentManualRate(ADJUSTMENT_MANUAL_YAW_RATE, &controlRateConfig->manual.rates[FD_YAW], delta); break; case ADJUSTMENT_PITCH_ROLL_P: case ADJUSTMENT_PITCH_P: - newValue = constrain((int)pidBank()->pid[PID_PITCH].P + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidBankMutable()->pid[PID_PITCH].P = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); + applyAdjustmentPID(ADJUSTMENT_PITCH_P, &pidBankMutable()->pid[PID_PITCH].P, delta); if (adjustmentFunction == ADJUSTMENT_PITCH_P) { schedulePidGainsUpdate(); break; @@ -337,16 +418,12 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t FALLTHROUGH; case ADJUSTMENT_ROLL_P: - newValue = constrain((int)pidBank()->pid[PID_ROLL].P + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidBankMutable()->pid[PID_ROLL].P = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); + applyAdjustmentPID(ADJUSTMENT_ROLL_P, &pidBankMutable()->pid[PID_ROLL].P, delta); schedulePidGainsUpdate(); break; case ADJUSTMENT_PITCH_ROLL_I: case ADJUSTMENT_PITCH_I: - newValue = constrain((int)pidBank()->pid[PID_PITCH].I + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidBankMutable()->pid[PID_PITCH].I = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); + applyAdjustmentPID(ADJUSTMENT_PITCH_I, &pidBankMutable()->pid[PID_PITCH].I, delta); if (adjustmentFunction == ADJUSTMENT_PITCH_I) { schedulePidGainsUpdate(); break; @@ -355,16 +432,12 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t FALLTHROUGH; case ADJUSTMENT_ROLL_I: - newValue = constrain((int)pidBank()->pid[PID_ROLL].I + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidBankMutable()->pid[PID_ROLL].I = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); + applyAdjustmentPID(ADJUSTMENT_ROLL_I, &pidBankMutable()->pid[PID_ROLL].I, delta); schedulePidGainsUpdate(); break; case ADJUSTMENT_PITCH_ROLL_D: case ADJUSTMENT_PITCH_D: - newValue = constrain((int)pidBank()->pid[PID_PITCH].D + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidBankMutable()->pid[PID_PITCH].D = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); + applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_PITCH].D, delta); if (adjustmentFunction == ADJUSTMENT_PITCH_D) { schedulePidGainsUpdate(); break; @@ -373,38 +446,26 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t FALLTHROUGH; case ADJUSTMENT_ROLL_D: - newValue = constrain((int)pidBank()->pid[PID_ROLL].D + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidBankMutable()->pid[PID_ROLL].D = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); + applyAdjustmentPID(ADJUSTMENT_ROLL_D, &pidBankMutable()->pid[PID_ROLL].D, delta); schedulePidGainsUpdate(); break; case ADJUSTMENT_YAW_P: - newValue = constrain((int)pidBank()->pid[PID_YAW].P + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidBankMutable()->pid[PID_YAW].P = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); + applyAdjustmentPID(ADJUSTMENT_YAW_P, &pidBankMutable()->pid[PID_YAW].P, delta); schedulePidGainsUpdate(); break; case ADJUSTMENT_YAW_I: - newValue = constrain((int)pidBank()->pid[PID_YAW].I + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidBankMutable()->pid[PID_YAW].I = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); + applyAdjustmentPID(ADJUSTMENT_YAW_I, &pidBankMutable()->pid[PID_YAW].I, delta); schedulePidGainsUpdate(); break; case ADJUSTMENT_YAW_D: - newValue = constrain((int)pidBank()->pid[PID_YAW].D + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c - pidBankMutable()->pid[PID_YAW].D = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); + applyAdjustmentPID(ADJUSTMENT_YAW_D, &pidBankMutable()->pid[PID_YAW].D, delta); schedulePidGainsUpdate(); break; case ADJUSTMENT_NAV_FW_CRUISE_THR: - newValue = constrain((int16_t)navConfig()->fw.cruise_throttle + delta, 1000, 2000); - navConfigMutable()->fw.cruise_throttle = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_NAV_FW_CRUISE_THR, newValue); + applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, 1000, 2000); break; case ADJUSTMENT_NAV_FW_PITCH2THR: - newValue = constrain((int8_t)navConfig()->fw.pitch_to_throttle + delta, 0, 100); - navConfigMutable()->fw.pitch_to_throttle = newValue; - blackboxLogInflightAdjustmentEvent(ADJUSTMENT_NAV_FW_PITCH2THR, newValue); + applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, 0, 100); break; case ADJUSTMENT_ROLL_BOARD_ALIGNMENT: updateBoardAlignment(delta, 0); @@ -414,6 +475,73 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t updateBoardAlignment(0, delta); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_BOARD_ALIGNMENT, boardAlignment()->pitchDeciDegrees); break; + case ADJUSTMENT_LEVEL_P: + applyAdjustmentPID(ADJUSTMENT_LEVEL_P, &pidBankMutable()->pid[PID_LEVEL].P, delta); + // TODO: Need to call something to take it into account? + break; + case ADJUSTMENT_LEVEL_I: + applyAdjustmentPID(ADJUSTMENT_LEVEL_I, &pidBankMutable()->pid[PID_LEVEL].I, delta); + // TODO: Need to call something to take it into account? + break; + case ADJUSTMENT_LEVEL_D: + applyAdjustmentPID(ADJUSTMENT_LEVEL_D, &pidBankMutable()->pid[PID_LEVEL].D, delta); + // TODO: Need to call something to take it into account? + break; + case ADJUSTMENT_POS_XY_P: + applyAdjustmentPID(ADJUSTMENT_POS_XY_P, &pidBankMutable()->pid[PID_POS_XY].P, delta); + navigationUsePIDs(); + break; + case ADJUSTMENT_POS_XY_I: + applyAdjustmentPID(ADJUSTMENT_POS_XY_I, &pidBankMutable()->pid[PID_POS_XY].I, delta); + navigationUsePIDs(); + break; + case ADJUSTMENT_POS_XY_D: + applyAdjustmentPID(ADJUSTMENT_POS_XY_D, &pidBankMutable()->pid[PID_POS_XY].D, delta); + navigationUsePIDs(); + break; + case ADJUSTMENT_POS_Z_P: + applyAdjustmentPID(ADJUSTMENT_POS_Z_P, &pidBankMutable()->pid[PID_POS_Z].P, delta); + navigationUsePIDs(); + break; + case ADJUSTMENT_POS_Z_I: + applyAdjustmentPID(ADJUSTMENT_POS_Z_I, &pidBankMutable()->pid[PID_POS_Z].I, delta); + navigationUsePIDs(); + break; + case ADJUSTMENT_POS_Z_D: + applyAdjustmentPID(ADJUSTMENT_POS_Z_D, &pidBankMutable()->pid[PID_POS_Z].D, delta); + navigationUsePIDs(); + break; + case ADJUSTMENT_HEADING_P: + applyAdjustmentPID(ADJUSTMENT_HEADING_P, &pidBankMutable()->pid[PID_HEADING].P, delta); + // TODO: navigationUsePIDs()? + break; + case ADJUSTMENT_VEL_XY_P: + applyAdjustmentPID(ADJUSTMENT_VEL_XY_P, &pidBankMutable()->pid[PID_VEL_XY].P, delta); + navigationUsePIDs(); + break; + case ADJUSTMENT_VEL_XY_I: + applyAdjustmentPID(ADJUSTMENT_VEL_XY_I, &pidBankMutable()->pid[PID_VEL_XY].I, delta); + navigationUsePIDs(); + break; + case ADJUSTMENT_VEL_XY_D: + applyAdjustmentPID(ADJUSTMENT_VEL_XY_D, &pidBankMutable()->pid[PID_VEL_XY].D, delta); + navigationUsePIDs(); + break; + case ADJUSTMENT_VEL_Z_P: + applyAdjustmentPID(ADJUSTMENT_VEL_Z_P, &pidBankMutable()->pid[PID_VEL_Z].P, delta); + navigationUsePIDs(); + break; + case ADJUSTMENT_VEL_Z_I: + applyAdjustmentPID(ADJUSTMENT_VEL_Z_I, &pidBankMutable()->pid[PID_VEL_Z].I, delta); + navigationUsePIDs(); + break; + case ADJUSTMENT_VEL_Z_D: + applyAdjustmentPID(ADJUSTMENT_VEL_Z_D, &pidBankMutable()->pid[PID_VEL_Z].D, delta); + navigationUsePIDs(); + break; + 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; default: break; }; @@ -442,12 +570,10 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) #define RESET_FREQUENCY_2HZ (1000 / 2) -void processRcAdjustments(controlRateConfig_t *controlRateConfig) +void processRcAdjustments(controlRateConfig_t *controlRateConfig, bool canUseRxData) { const uint32_t now = millis(); - const bool canUseRxData = rxIsReceivingSignal(); - for (int adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) { adjustmentState_t * const adjustmentState = &adjustmentStates[adjustmentIndex]; @@ -508,16 +634,28 @@ void resetAdjustmentStates(void) memset(adjustmentStates, 0, sizeof(adjustmentStates)); } -void updateAdjustmentStates(void) +void updateAdjustmentStates(bool canUseRxData) { for (int index = 0; index < MAX_ADJUSTMENT_RANGE_COUNT; index++) { const adjustmentRange_t * const adjustmentRange = adjustmentRanges(index); + const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET]; - if (isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) { - - const adjustmentConfig_t *adjustmentConfig = &defaultAdjustmentConfigs[adjustmentRange->adjustmentFunction - ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET]; - + if (canUseRxData && isRangeActive(adjustmentRange->auxChannelIndex, &adjustmentRange->range)) { configureAdjustment(adjustmentRange->adjustmentIndex, adjustmentRange->auxSwitchChannelIndex, adjustmentConfig); + } else { + adjustmentState_t * const adjustmentState = &adjustmentStates[index]; + if (adjustmentState->config == adjustmentConfig) { + adjustmentState->config = NULL; + } } } } + +bool isAdjustmentFunctionSelected(uint8_t adjustmentFunction) { + for (uint8_t index = 0; index < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; ++index) { + if (adjustmentStates[index].config && adjustmentStates[index].config->adjustmentFunction == adjustmentFunction) { + return true; + } + } + return false; +} diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index 634ed633a5..b6782e273a 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -57,8 +57,25 @@ typedef enum { ADJUSTMENT_NAV_FW_PITCH2THR = 29, ADJUSTMENT_ROLL_BOARD_ALIGNMENT = 30, ADJUSTMENT_PITCH_BOARD_ALIGNMENT = 31, + ADJUSTMENT_LEVEL_P = 32, + ADJUSTMENT_LEVEL_I = 33, + ADJUSTMENT_LEVEL_D = 34, + ADJUSTMENT_POS_XY_P = 35, + ADJUSTMENT_POS_XY_I = 36, + ADJUSTMENT_POS_XY_D = 37, + ADJUSTMENT_POS_Z_P = 38, + ADJUSTMENT_POS_Z_I = 39, + ADJUSTMENT_POS_Z_D = 40, + ADJUSTMENT_HEADING_P = 41, + ADJUSTMENT_VEL_XY_P = 42, + ADJUSTMENT_VEL_XY_I = 43, + ADJUSTMENT_VEL_XY_D = 44, + ADJUSTMENT_VEL_Z_P = 45, + ADJUSTMENT_VEL_Z_I = 46, + ADJUSTMENT_VEL_Z_D = 47, + ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 48, #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT - ADJUSTMENT_PROFILE = 32, + ADJUSTMENT_PROFILE = 49, #endif ADJUSTMENT_FUNCTION_COUNT // must be last } adjustmentFunction_e; @@ -118,6 +135,7 @@ typedef struct adjustmentState_s { PG_DECLARE_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges); void resetAdjustmentStates(void); -void updateAdjustmentStates(void); +void updateAdjustmentStates(bool canUseRxData); struct controlRateConfig_s; -void processRcAdjustments(struct controlRateConfig_s *controlRateConfig); +void processRcAdjustments(struct controlRateConfig_s *controlRateConfig, bool canUseRxData); +bool isAdjustmentFunctionSelected(uint8_t adjustmentFunction); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 8527996743..f7c3d49b29 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -63,7 +63,7 @@ #define AIRMODE_DEADBAND 25 #define MIN_RC_TICK_INTERVAL_MS 20 -#define DEFAULT_RC_SWITCH_DISARM_DELAY_MS 150 // Wait at least 150ms before disarming via switch +#define DEFAULT_RC_SWITCH_DISARM_DELAY_MS 250 // Wait at least 250ms before disarming via switch stickPositions_e rcStickPositions; @@ -232,7 +232,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } // actions during not armed - int i = 0; // GYRO calibration if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { @@ -257,16 +256,38 @@ void processRcStickPositions(throttleStatus_e throttleStatus) // Multiple configuration profiles if (feature(FEATURE_TX_PROF_SEL)) { + + uint8_t i = 0; + if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1 i = 1; else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2 i = 2; else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3 i = 3; + if (i) { setConfigProfileAndWriteEEPROM(i - 1); return; } + + i = 0; + + // Multiple battery configuration profiles + if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1 + i = 1; + else if (rcSticks == THR_HI + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2 + i = 2; + else if (rcSticks == THR_HI + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3 + i = 3; + + if (i) { + setConfigBatteryProfileAndWriteEEPROM(i - 1); + batteryDisableProfileAutoswitch(); + activateBatteryProfile(); + return; + } + } // Save config @@ -331,4 +352,3 @@ void processRcStickPositions(throttleStatus_e throttleStatus) int32_t getRcStickDeflection(int32_t axis) { return MIN(ABS(rcData[axis] - PWM_RANGE_MIDDLE), 500); } - diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 7b9bb8a889..b523970f9b 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -161,6 +161,7 @@ void updateUsedModeActivationConditionFlags(void) #ifdef USE_NAV isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) || isModeActivationConditionPresent(BOXNAVRTH) || + isModeActivationConditionPresent(BOXNAVCRUISE) || isModeActivationConditionPresent(BOXNAVWP); #endif } diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 8dd2d23ce1..30730ac663 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -59,7 +59,8 @@ typedef enum { BOXOSDALT1 = 32, BOXOSDALT2 = 33, BOXOSDALT3 = 34, - BOXBRAKING = 35, + BOXNAVCRUISE = 35, + BOXBRAKING = 36, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 2c41497199..be86a72eab 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -76,7 +76,7 @@ typedef enum { FAILSAFE_MODE = (1 << 9), AUTO_TUNE = (1 << 10), // old G-Tune NAV_WP_MODE = (1 << 11), - UNUSED_MODE2 = (1 << 12), + NAV_CRUISE_MODE = (1 << 12), FLAPERON = (1 << 13), TURN_ASSISTANT = (1 << 14), } flightModeFlags_e; diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index a816fc1e85..8c66ebb1f3 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -10,7 +10,7 @@ #include "settings_generated.c" -void setting_get_name(const setting_t *val, char *buf) +void settingGetName(const setting_t *val, char *buf) { uint8_t bpos = 0; uint16_t n = 0; @@ -52,24 +52,24 @@ void setting_get_name(const setting_t *val, char *buf) buf[bpos] = '\0'; } -bool setting_name_contains(const setting_t *val, char *buf, const char *cmdline) +bool settingNameContains(const setting_t *val, char *buf, const char *cmdline) { - setting_get_name(val, buf); + settingGetName(val, buf); return strstr(buf, cmdline) != NULL; } -bool setting_name_exact_match(const setting_t *val, char *buf, const char *cmdline, uint8_t var_name_length) +bool settingNameIsExactMatch(const setting_t *val, char *buf, const char *cmdline, uint8_t var_name_length) { - setting_get_name(val, buf); + settingGetName(val, buf); return sl_strncasecmp(cmdline, buf, strlen(buf)) == 0 && var_name_length == strlen(buf); } -const setting_t *setting_find(const char *name) +const setting_t *settingFind(const char *name) { char buf[SETTING_MAX_NAME_LENGTH]; for (int ii = 0; ii < SETTINGS_TABLE_COUNT; ii++) { const setting_t *setting = &settingsTable[ii]; - setting_get_name(setting, buf); + settingGetName(setting, buf); if (strcmp(buf, name) == 0) { return setting; } @@ -77,7 +77,7 @@ const setting_t *setting_find(const char *name) return NULL; } -size_t setting_get_value_size(const setting_t *val) +size_t settingGetValueSize(const setting_t *val) { switch (SETTING_TYPE(val)) { case VAR_UINT8: @@ -92,11 +92,13 @@ size_t setting_get_value_size(const setting_t *val) FALLTHROUGH; case VAR_FLOAT: return 4; + case VAR_STRING: + return settingGetMax(val); } return 0; // Unreachable } -pgn_t setting_get_pgn(const setting_t *val) +pgn_t settingGetPgn(const setting_t *val) { uint16_t pos = val - (const setting_t *)settingsTable; uint16_t acc = 0; @@ -118,23 +120,25 @@ static uint16_t getValueOffset(const setting_t *value) return value->offset + sizeof(pidProfile_t) * getConfigProfile(); case CONTROL_RATE_VALUE: return value->offset + sizeof(controlRateConfig_t) * getConfigProfile(); + case BATTERY_CONFIG_VALUE: + return value->offset + sizeof(batteryProfile_t) * getConfigBatteryProfile(); } return 0; } -void *setting_get_value_pointer(const setting_t *val) +void *settingGetValuePointer(const setting_t *val) { - const pgRegistry_t *pg = pgFind(setting_get_pgn(val)); + const pgRegistry_t *pg = pgFind(settingGetPgn(val)); return pg->address + getValueOffset(val); } -const void * setting_get_copy_value_pointer(const setting_t *val) +const void * settingGetCopyValuePointer(const setting_t *val) { - const pgRegistry_t *pg = pgFind(setting_get_pgn(val)); + const pgRegistry_t *pg = pgFind(settingGetPgn(val)); return pg->copy + getValueOffset(val); } -setting_min_t setting_get_min(const setting_t *val) +setting_min_t settingGetMin(const setting_t *val) { if (SETTING_MODE(val) == MODE_LOOKUP) { return 0; @@ -142,10 +146,37 @@ setting_min_t setting_get_min(const setting_t *val) return settingMinMaxTable[SETTING_INDEXES_GET_MIN(val)]; } -setting_max_t setting_get_max(const setting_t *val) +setting_max_t settingGetMax(const setting_t *val) { if (SETTING_MODE(val) == MODE_LOOKUP) { return settingLookupTables[val->config.lookup.tableIndex].valueCount - 1; } return settingMinMaxTable[SETTING_INDEXES_GET_MAX(val)]; } + +const char * settingGetString(const setting_t *val) +{ + if (SETTING_TYPE(val) == VAR_STRING) { + return settingGetValuePointer(val); + } + return NULL; +} + +void settingSetString(const setting_t *val, const char *s, size_t size) +{ + if (SETTING_TYPE(val) == VAR_STRING) { + char *p = settingGetValuePointer(val); + size_t copySize = MIN(size, settingGetMax(val)); + memcpy(p, s, copySize); + p[copySize] = '\0'; + } +} + +setting_max_t settingGetStringMaxLength(const setting_t *val) +{ + if (SETTING_TYPE(val) == VAR_STRING) { + // Max string length is stored as its max + return settingGetMax(val); + } + return 0; +} diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index 3c05d123c5..6b26ee6445 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -27,6 +27,7 @@ typedef enum { VAR_INT16 = (3 << SETTING_TYPE_OFFSET), VAR_UINT32 = (4 << SETTING_TYPE_OFFSET), VAR_FLOAT = (5 << SETTING_TYPE_OFFSET), // 0x05 + VAR_STRING = (6 << SETTING_TYPE_OFFSET) // 0x06 } setting_type_e; typedef enum { @@ -34,6 +35,7 @@ typedef enum { MASTER_VALUE = (0 << SETTING_SECTION_OFFSET), PROFILE_VALUE = (1 << SETTING_SECTION_OFFSET), CONTROL_RATE_VALUE = (2 << SETTING_SECTION_OFFSET), // 0x20 + BATTERY_CONFIG_VALUE = (3 << SETTING_SECTION_OFFSET), } setting_section_e; typedef enum { @@ -73,28 +75,38 @@ static inline setting_type_e SETTING_TYPE(const setting_t *s) { return s->type & static inline setting_section_e SETTING_SECTION(const setting_t *s) { return s->type & SETTING_SECTION_MASK; } static inline setting_mode_e SETTING_MODE(const setting_t *s) { return s->type & SETTING_MODE_MASK; } -void setting_get_name(const setting_t *val, char *buf); -bool setting_name_contains(const setting_t *val, char *buf, const char *cmdline); -bool setting_name_exact_match(const setting_t *val, char *buf, const char *cmdline, uint8_t var_name_length); +void settingGetName(const setting_t *val, char *buf); +bool settingNameContains(const setting_t *val, char *buf, const char *cmdline); +bool settingNameIsExactMatch(const setting_t *val, char *buf, const char *cmdline, uint8_t var_name_length); // Returns a setting_t with the exact name (case sensitive), or // NULL if no setting with that name exists. -const setting_t *setting_find(const char *name); +const setting_t *settingFind(const char *name); // Returns the size in bytes of the setting value. -size_t setting_get_value_size(const setting_t *val); -pgn_t setting_get_pgn(const setting_t *val); +size_t settingGetValueSize(const setting_t *val); +pgn_t settingGetPgn(const setting_t *val); // Returns a pointer to the actual value stored by // the setting_t. The returned value might be modified. -void * setting_get_value_pointer(const setting_t *val); +void * settingGetValuePointer(const setting_t *val); // Returns a pointer to the backed up copy of the value. Note that // this will contain random garbage unless a copy of the parameter // group for the value has been manually performed. Currently, this // is only used by cli.c during config dumps. -const void * setting_get_copy_value_pointer(const setting_t *val); +const void * settingGetCopyValuePointer(const setting_t *val); // Returns the minimum valid value for the given setting_t. setting_min_t // depends on the target and build options, but will always be a signed // integer (e.g. intxx_t,) -setting_min_t setting_get_min(const setting_t *val); +setting_min_t settingGetMin(const setting_t *val); // Returns the maximum valid value for the given setting_t. setting_max_t // depends on the target and build options, but will always be an unsigned // integer (e.g. uintxx_t,) -setting_max_t setting_get_max(const setting_t *val); \ No newline at end of file +setting_max_t settingGetMax(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); +// Sets the value for the given string setting. Size indicates the number of +// bytes in the string without the '\0' terminator (i.e. its strlen()). +// If the setting is not of type VAR_STRING, this function does nothing. +void settingSetString(const setting_t *val, const char *s, size_t size); +// Returns the max string length (without counting the '\0' terminator) +// for setting of type VAR_STRING. Otherwise it returns 0. +setting_max_t settingGetStringMaxLength(const setting_t *val); \ No newline at end of file diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 34dc677e3d..059cc627f3 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -59,6 +59,9 @@ tables: - name: osd_stats_energy_unit values: ["MAH", "WH"] enum: osd_stats_energy_unit_e + - name: osd_video_system + values: ["AUTO", "PAL", "NTSC"] + enum: videoSystem_e - name: frsky_unit values: ["METRIC", "IMPERIAL"] enum: frskyUnit_e @@ -67,7 +70,9 @@ tables: - name: i2c_speed values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"] - name: debug_modes - values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE"] + 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"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator @@ -85,6 +90,9 @@ tables: - name: bat_capacity_unit values: ["MAH", "MWH"] enum: batCapacityUnit_e + - name: bat_voltage_source + values: ["RAW", "SAG_COMP"] + enum: batVoltageSource_e - name: smartport_fuel_unit values: ["PERCENT", "MAH", "MWH"] enum: smartportFuelUnit_e @@ -94,6 +102,9 @@ tables: - name: tz_automatic_dst values: ["OFF", "EU", "USA"] enum: tz_automatic_dst_e + - name: vtx_low_power_disarm + values: ["OFF", "ON", "UNTIL_FIRST_ARM"] + enum: vtxLowerPowerDisarm_e groups: - name: PG_GYRO_CONFIG @@ -423,9 +434,6 @@ groups: - name: motor_pwm_protocol field: motorPwmProtocol table: motor_pwm_protocol - - name: throttle_vbat_compensation - field: throttleVBatCompensation - type: bool - name: PG_FAILSAFE_CONFIG type: failsafeConfig_t @@ -500,16 +508,54 @@ groups: field: yawDeciDegrees min: -1800 max: 3600 - - - name: PG_BATTERY_CONFIG - type: batteryConfig_t + + - name: PG_BATTERY_METERS_CONFIG + type: batteryMetersConfig_t headers: ["sensors/battery.h"] members: - name: vbat_scale - field: voltage.scale + field: voltage_scale condition: USE_ADC min: VBAT_SCALE_MIN max: VBAT_SCALE_MAX + - name: current_meter_scale + field: current.scale + min: -10000 + max: 10000 + - name: current_meter_offset + field: current.offset + min: -32768 + max: 32767 + - name: current_meter_type + field: current.type + table: current_sensor + type: uint8_t + - name: bat_voltage_src + field: voltageSource + table: bat_voltage_source + type: uint8_t + - name: cruise_power + field: cruise_power + min: 0 + max: 4294967295 + - name: idle_power + field: idle_power + min: 0 + max: 65535 + - name: rth_energy_margin + min: 0 + max: 100 + + - name: PG_BATTERY_PROFILES + type: batteryProfile_t + headers: ["sensors/battery.h"] + value_type: BATTERY_CONFIG_VALUE + members: + - name: bat_cells + field: cells + condition: USE_ADC + min: 0 + max: 8 - name: vbat_cell_detect_voltage field: voltage.cellDetect condition: USE_ADC @@ -546,18 +592,6 @@ groups: field: capacity.unit table: bat_capacity_unit type: uint8_t - - name: current_meter_scale - field: current.scale - min: -10000 - max: 10000 - - name: current_meter_offset - field: current.offset - min: -3300 - max: 3300 - - name: current_meter_type - field: current.type - table: current_sensor - type: uint8_t - name: PG_MIXER_CONFIG type: mixerConfig_t @@ -579,6 +613,10 @@ groups: field: appliedMixerPreset min: -1 max: INT16_MAX + - name: fw_min_throttle_down_pitch + field: fwMinThrottleDownPitchAngle + min: 0 + max: 450 - name: PG_MOTOR_3D_CONFIG type: flight3DConfig_t @@ -1082,6 +1120,9 @@ groups: - name: inav_use_gps_velned field: use_gps_velned type: bool + - name: inav_allow_dead_reckoning + field: allow_dead_reckoning + type: bool - name: inav_reset_altitude field: reset_altitude_type table: reset_type @@ -1100,6 +1141,14 @@ groups: field: w_z_surface_v min: 0 max: 10 + - name: inav_w_xy_flow_p + field: w_xy_flow_p + min: 0 + max: 10 + - name: inav_w_xy_flow_v + field: w_xy_flow_v + min: 0 + max: 10 - name: inav_w_z_baro_p field: w_z_baro_p min: 0 @@ -1229,6 +1278,9 @@ groups: - name: nav_rth_altitude field: general.rth_altitude max: 65000 + - name: nav_rth_home_altitude + field: general.rth_home_altitude + max: 65000 - name: nav_mc_bank_angle field: mc.max_bank_angle @@ -1306,6 +1358,10 @@ groups: field: fw.loiter_radius min: 0 max: 10000 + - name: nav_fw_cruise_speed + field: fw.cruise_speed + min: 0 + max: 65535 - name: nav_fw_land_dive_angle field: fw.land_dive_angle @@ -1360,10 +1416,17 @@ groups: field: fw.launch_climb_angle min: 5 max: 45 + - name: nav_fw_cruise_yaw_rate + field: fw.cruise_yaw_rate + min: 0 + max: 60 + - name: nav_fw_allow_manual_thr_increase + field: fw.allow_manual_thr_increase + type: bool - name: PG_TELEMETRY_CONFIG type: telemetryConfig_t - headers: ["io/serial.h", "telemetry/telemetry.h", "telemetry/frsky.h"] + headers: ["io/serial.h", "telemetry/telemetry.h", "telemetry/frsky_d.h"] condition: USE_TELEMETRY members: - name: telemetry_switch @@ -1454,13 +1517,13 @@ groups: - name: PG_OSD_CONFIG type: osdConfig_t - headers: ["io/osd.h"] + headers: ["io/osd.h", "drivers/vcd.h"] condition: USE_OSD members: - name: osd_video_system + table: osd_video_system field: video_system - min: 0 - max: 2 + type: uint8_t - name: osd_row_shiftdown field: row_shiftdown min: 0 @@ -1522,6 +1585,10 @@ groups: min: 0 max: 1 + - name: osd_estimations_wind_compensation + field: estimations_wind_compensation + type: bool + - name: PG_SYSTEM_CONFIG type: systemConfig_t headers: ["fc/config.h"] @@ -1556,6 +1623,7 @@ groups: - name: input_filtering_mode field: pwmRxInputFilteringMode type: bool + - name: name - name: PG_MODE_ACTIVATION_OPERATOR_CONFIG type: modeActivationOperatorConfig_t @@ -1600,3 +1668,43 @@ groups: - name: display_force_sw_blink field: force_sw_blink type: bool + + - name: PG_VTX_CONFIG + type: vtxConfig_t + headers: ["io/vtx_control.h"] + condition: USE_VTX_CONTROL && USE_VTX_COMMON + members: + - name: vtx_halfduplex + field: halfDuplex + type: bool + + - name: PG_VTX_SETTINGS_CONFIG + type: vtxSettingsConfig_t + headers: ["drivers/vtx_common.h", "io/vtx.h"] + members: + - name: vtx_band + field: band + min: VTX_SETTINGS_NO_BAND + max: VTX_SETTINGS_MAX_BAND + - name: vtx_channel + field: channel + min: VTX_SETTINGS_MIN_CHANNEL + max: VTX_SETTINGS_MAX_CHANNEL + - name: vtx_power + field: power + min: VTX_SETTINGS_MIN_POWER + max: VTX_SETTINGS_MAX_POWER + - name: vtx_low_power_disarm + field: lowPowerDisarm + table: vtx_low_power_disarm + type: uint8_t + - name: vtx_freq + field: freq + min: VTX_SETTINGS_MIN_FREQUENCY_MHZ + max: VTX_SETTINGS_MAX_FREQUENCY_MHZ + condition: VTX_SETTINGS_FREQCMD + - name: vtx_pit_mode_freq + field: pitModeFreq + min: VTX_SETTINGS_MIN_FREQUENCY_MHZ + max: VTX_SETTINGS_MAX_FREQUENCY_MHZ + condition: VTX_SETTINGS_FREQCMD diff --git a/src/main/fc/stats.c b/src/main/fc/stats.c index f2f471e003..6ce2c0cb8e 100644 --- a/src/main/fc/stats.c +++ b/src/main/fc/stats.c @@ -30,8 +30,14 @@ PG_RESET_TEMPLATE(statsConfig_t, statsConfig, static uint32_t arm_millis; static uint32_t arm_distance_cm; + #ifdef USE_ADC static uint32_t arm_mWhDrawn; +static uint32_t flyingEnergy; // energy drawn during flying up to last disarm (ARMED) mWh + +uint32_t getFlyingEnergy() { + return flyingEnergy; +} #endif void statsOnArm(void) @@ -51,8 +57,11 @@ void statsOnDisarm(void) statsConfigMutable()->stats_total_time += dt; //[s] statsConfigMutable()->stats_total_dist += (getTotalTravelDistance() - arm_distance_cm) / 100; //[m] #ifdef USE_ADC - if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) - statsConfigMutable()->stats_total_energy += getMWhDrawn() - arm_mWhDrawn; + if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) { + const uint32_t energy = getMWhDrawn() - arm_mWhDrawn; + statsConfigMutable()->stats_total_energy += energy; + flyingEnergy += energy; + } #endif writeEEPROM(); } diff --git a/src/main/fc/stats.h b/src/main/fc/stats.h index c77025b81c..95bf8c2bca 100644 --- a/src/main/fc/stats.h +++ b/src/main/fc/stats.h @@ -11,6 +11,7 @@ typedef struct statsConfig_s { uint8_t stats_enabled; } statsConfig_t; +uint32_t getFlyingEnergy(); void statsOnArm(void); void statsOnDisarm(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 3b659c8bb0..e6ef933bed 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -548,6 +548,19 @@ void imuUpdateAccelerometer(void) #endif } +void imuCheckVibrationLevels(void) +{ + fpVector3_t accVibeLevels; + + accGetVibrationLevels(&accVibeLevels); + const uint32_t accClipCount = accGetClipCount(); + + DEBUG_SET(DEBUG_VIBE, 0, accVibeLevels.x * 100); + DEBUG_SET(DEBUG_VIBE, 1, accVibeLevels.y * 100); + DEBUG_SET(DEBUG_VIBE, 2, accVibeLevels.z * 100); + DEBUG_SET(DEBUG_VIBE, 3, accClipCount); +} + void imuUpdateAttitude(timeUs_t currentTimeUs) { /* Calculate dT */ @@ -560,6 +573,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) if (!hilActive) { gyroGetMeasuredRotationRate(&imuMeasuredRotationBF); // Calculate gyro rate in body frame in rad/s accGetMeasuredAcceleration(&imuMeasuredAccelBF); // Calculate accel in body frame in cm/s/s + imuCheckVibrationLevels(); imuCalculateEstimatedAttitude(dT); // Update attitude estimate } else { @@ -569,6 +583,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) #else gyroGetMeasuredRotationRate(&imuMeasuredRotationBF); // Calculate gyro rate in body frame in rad/s accGetMeasuredAcceleration(&imuMeasuredAccelBF); // Calculate accel in body frame in cm/s/s + imuCheckVibrationLevels(); imuCalculateEstimatedAttitude(dT); // Update attitude estimate #endif } else { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index e483cbbfbd..a5a85ffc13 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -38,6 +38,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/failsafe.h" @@ -76,7 +77,8 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, .yaw_jump_prevention_limit = 200, .platformType = PLATFORM_MULTIROTOR, .hasFlaps = false, - .appliedMixerPreset = -1 //This flag is not available in CLI and used by Configurator only + .appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only + .fwMinThrottleDownPitchAngle = 0 ); #ifdef BRUSHED_MOTORS @@ -99,7 +101,6 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .mincommand = 1000, .motorAccelTimeMs = 0, .motorDecelTimeMs = 0, - .throttleVBatCompensation = false ); static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; @@ -310,7 +311,7 @@ void mixTable(const float dT) throttleMax = motorConfig()->maxthrottle; // Throttle compensation based on battery voltage - if (motorConfig()->throttleVBatCompensation && STATE(FIXED_WING) && isAmperageConfigured() && feature(FEATURE_VBAT)) + if (feature(FEATURE_THR_VBAT_COMP) && feature(FEATURE_VBAT) && isAmperageConfigured()) throttleCommand = MIN(throttleCommand * calculateThrottleCompensationFactor(), throttleMax); } @@ -350,17 +351,11 @@ void mixTable(const float dT) } // Motor stop handling - if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED)) { - bool failsafeMotorStop = failsafeRequiresMotorStop(); - bool navMotorStop = !failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE); - bool userMotorStop = !navigationIsFlyingAutonomousMode() && !failsafeIsActive() && (rcData[THROTTLE] < rxConfig()->mincheck); - if (failsafeMotorStop || navMotorStop || userMotorStop) { - if (feature(FEATURE_3D)) { - motor[i] = PWM_RANGE_MIDDLE; - } - else { - motor[i] = motorConfig()->mincommand; - } + if (ARMING_FLAG(ARMED) && (getMotorStatus() != MOTOR_RUNNING)) { + if (feature(FEATURE_MOTOR_STOP)) { + motor[i] = (feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : motorConfig()->mincommand); + } else { + motor[i] = motorConfig()->minthrottle; } } } @@ -373,3 +368,14 @@ void mixTable(const float dT) /* Apply motor acceleration/deceleration limit */ applyMotorRateLimiting(dT); } + +motorStatus_e getMotorStatus(void) +{ + if (failsafeRequiresMotorStop() || (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE))) + return MOTOR_STOPPED_AUTO; + + if ((STATE(FIXED_WING) || !isAirmodeActive()) && (!navigationIsFlyingAutonomousMode()) && (!failsafeIsActive()) && (rcData[THROTTLE] < rxConfig()->mincheck)) + return MOTOR_STOPPED_USER; + + return MOTOR_RUNNING; +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index d153a37fb7..97bf57cde9 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -28,6 +28,8 @@ #define YAW_JUMP_PREVENTION_LIMIT_LOW 80 #define YAW_JUMP_PREVENTION_LIMIT_HIGH 500 +#define FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX 450 + typedef enum { PLATFORM_MULTIROTOR = 0, PLATFORM_AIRPLANE = 1, @@ -59,6 +61,7 @@ typedef struct mixerConfig_s { uint8_t platformType; bool hasFlaps; int16_t appliedMixerPreset; + uint16_t fwMinThrottleDownPitchAngle; } mixerConfig_t; PG_DECLARE(mixerConfig_t, mixerConfig); @@ -80,17 +83,23 @@ typedef struct motorConfig_s { uint8_t motorPwmProtocol; uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms] uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms] - bool throttleVBatCompensation; } motorConfig_t; PG_DECLARE(motorConfig_t, motorConfig); +typedef enum { + MOTOR_STOPPED_USER, + MOTOR_STOPPED_AUTO, + MOTOR_RUNNING +} motorStatus_e; + extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; uint8_t getMotorCount(void); float getMotorMixRange(void); bool mixerIsOutputSaturated(void); +motorStatus_e getMotorStatus(void); void writeAllMotors(int16_t mc); void mixerUsePWMIOConfiguration(void); @@ -100,4 +109,4 @@ void mixTable(const float dT); void writeMotors(void); void processServoAutotrim(void); void stopMotors(void); -void stopPwmAllMotors(void); \ No newline at end of file +void stopPwmAllMotors(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ee561229e6..acd972f436 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -413,7 +413,12 @@ static float calcHorizonRateMagnitude(void) static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude) { // This is ROLL/PITCH, run ANGLE/HORIZON controllers - const float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]); + float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]); + + // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle + if ((axis == FD_PITCH) && STATE(FIXED_WING) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) + angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle); + const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f); diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c new file mode 100644 index 0000000000..a873d360b3 --- /dev/null +++ b/src/main/flight/rth_estimator.c @@ -0,0 +1,201 @@ + +#include "build/debug.h" + +#include "common/maths.h" + +#include "fc/config.h" +#include "fc/fc_core.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/wind_estimator.h" + +#include "navigation/navigation.h" + +#include "sensors/battery.h" + +#include + +#if defined(USE_ADC) && defined(USE_GPS) + +/* INPUTS: + * - forwardSpeed (same unit as horizontalWindSpeed) + * - heading degrees + * - horizontalWindSpeed (same unit as forwardSpeed) + * - windHeading degrees + * OUTPUT: + * returns degrees + */ +static float windDriftCompensationAngle(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) { + return RADIANS_TO_DEGREES(asin_approx(-horizontalWindSpeed * sin_approx(DEGREES_TO_RADIANS(windHeading - heading)) / forwardSpeed)); +} + +/* INPUTS: + * - forwardSpeed (same unit as horizontalWindSpeed) + * - heading degrees + * - horizontalWindSpeed (same unit as forwardSpeed) + * - windHeading degrees + * OUTPUT: + * returns (same unit as forwardSpeed and horizontalWindSpeed) + */ +static float windDriftCorrectedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) { + 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 + * - horizontalWindSpeed (same unit as forwardSpeed) + * - windHeading degrees + * OUTPUT: + * returns (same unit as forwardSpeed and horizontalWindSpeed) + */ +static float windCompensatedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) { + return windDriftCorrectedForwardSpeed(forwardSpeed, heading, horizontalWindSpeed, windHeading) + forwardWindSpeed(heading, horizontalWindSpeed, windHeading); +} + +// returns degrees +static int8_t RTHAltitudeChangePitchAngle(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; + 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; +} + +// altitudeChange is in m +// verticalWindSpeed is in m/s +// cruise_speed is in cm/s +// 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; + return altitudeChange / estimatedVerticalSpeed; +} + +// altitudeChange is in m +// horizontalWindSpeed is in m/s +// windHeading is in degrees +// verticalWindSpeed is in m/s +// cruise_speed is in cm/s +// 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); + 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; +} + +// returns distance in m +// *heading is in degrees +static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed, float *heading) { + float estimatedAltitudeChangeGroundDistance = estimateRTHAltitudeChangeGroundDistance(altitudeChange, horizontalWindSpeed, windHeading, verticalWindSpeed); + if (altitudeChange > 0) { + float headingDiff = DEGREES_TO_RADIANS(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw) - GPS_directionToHome); + float triangleAltitude = GPS_distanceToHome * sin_approx(headingDiff); + float triangleAltitudeToReturnStart = estimatedAltitudeChangeGroundDistance - GPS_distanceToHome * cos_approx(headingDiff); + *heading = RADIANS_TO_DEGREES(atan2_approx(triangleAltitude, triangleAltitudeToReturnStart)); + return sqrt(sq(triangleAltitude) + sq(triangleAltitudeToReturnStart)); + } else { + *heading = GPS_directionToHome; + return GPS_distanceToHome - estimatedAltitudeChangeGroundDistance; + } +} + +// returns mWh +static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { + // Fixed wing only for now + if (!STATE(FIXED_WING)) + return -1; + + if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && navigationPositionEstimateIsHealthy() && (batteryMetersConfig()->cruise_power > 0) && (ARMING_FLAG(ARMED)) && ((!STATE(FIXED_WING)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected())) && (navConfig()->fw.cruise_speed > 0) && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) && batteryWasFullWhenPluggedIn() && isEstimatedWindSpeedValid() && isImuHeadingValid())) + return -1; + + 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_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, 1, lrintf(RTH_distance * 100)); + DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 2, lrintf(RTH_speed * 100)); + DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 3, lrintf(horizontalWindSpeed * 100)); + + if (RTH_speed <= 0) + return -2; // wind is too strong + + 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 + + if (remaining_energy_before_rth < 0) // No energy left = No time left + return 0; + + return remaining_energy_before_rth; +} + +// returns seconds +int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) { + + const int32_t remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount); + + // error: return error code directly + if (remainingEnergyBeforeRTH < 0) + return remainingEnergyBeforeRTH; + + const int32_t averagePower = calculateAveragePower(); + + if (averagePower == 0) + return -1; + + const uint32_t time_before_rth = remainingEnergyBeforeRTH * 360 / averagePower; + + if (time_before_rth > 0x7FFFFFFF) // int32 overflow + return -1; + + return time_before_rth; +} + +// returns meters +int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) { + + const int32_t remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount); + + // error: return error code directly + if (remainingFlightTimeBeforeRTH < 0) + return remainingFlightTimeBeforeRTH; + + return remainingFlightTimeBeforeRTH * calculateAverageSpeed(); +} + +#endif diff --git a/src/main/flight/rth_estimator.h b/src/main/flight/rth_estimator.h new file mode 100644 index 0000000000..035bc0d644 --- /dev/null +++ b/src/main/flight/rth_estimator.h @@ -0,0 +1,5 @@ + +#if defined(USE_ADC) && defined(USE_GPS) +int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount); +int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount); +#endif diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index bceeadf479..90a33ed4fe 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -173,4 +173,4 @@ void updateWindEstimator(timeUs_t currentTimeUs) } } -#endif \ No newline at end of file +#endif diff --git a/src/main/flight/wind_estimator.h b/src/main/flight/wind_estimator.h index 776582b0cc..e1bad1556c 100644 --- a/src/main/flight/wind_estimator.h +++ b/src/main/flight/wind_estimator.h @@ -23,6 +23,7 @@ #endif #endif +#include "common/axis.h" #include "common/time.h" bool isEstimatedWindSpeedValid(void); diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c index 5789a78e1c..d086b36f32 100644 --- a/src/main/io/displayport_max7456.c +++ b/src/main/io/displayport_max7456.c @@ -158,10 +158,10 @@ static const displayPortVTable_t max7456VTable = { .supportedTextAttributes = supportedTextAttributes, }; -displayPort_t *max7456DisplayPortInit(const vcdProfile_t *vcdProfile) +displayPort_t *max7456DisplayPortInit(const videoSystem_e videoSystem) { displayInit(&max7456DisplayPort, &max7456VTable); - max7456Init(vcdProfile); + max7456Init(videoSystem); resync(&max7456DisplayPort); return &max7456DisplayPort; } diff --git a/src/main/io/displayport_max7456.h b/src/main/io/displayport_max7456.h index 9a582ead72..a88a3631f4 100644 --- a/src/main/io/displayport_max7456.h +++ b/src/main/io/displayport_max7456.h @@ -19,5 +19,6 @@ #include "config/parameter_group.h" #include "drivers/display.h" +#include "drivers/vcd.h" -displayPort_t *max7456DisplayPortInit(const struct vcdProfile_s *vcdProfile); +displayPort_t *max7456DisplayPortInit(const videoSystem_e videoSystem); diff --git a/src/main/io/opflow_cxof.c b/src/main/io/opflow_cxof.c index fae1e65b51..a6d1bab314 100755 --- a/src/main/io/opflow_cxof.c +++ b/src/main/io/opflow_cxof.c @@ -113,6 +113,7 @@ static bool cxofOpflowUpdate(opflowData_t * data) tmpData.deltaTime += (currentTimeUs - previousTimeUs); tmpData.flowRateRaw[0] += pkt->motionX; tmpData.flowRateRaw[1] += pkt->motionY; + tmpData.flowRateRaw[2] = 0; tmpData.quality = (constrain(pkt->squal, 64, 78) - 64) * 100 / 14; previousTimeUs = currentTimeUs; diff --git a/src/main/io/opflow_msp.c b/src/main/io/opflow_msp.c index 92af18b993..029fa09573 100644 --- a/src/main/io/opflow_msp.c +++ b/src/main/io/opflow_msp.c @@ -81,6 +81,7 @@ void mspOpflowReceiveNewData(uint8_t * bufferPtr) sensorData.deltaTime = currentTimeUs - updatedTimeUs; sensorData.flowRateRaw[0] = pkt->motionX; sensorData.flowRateRaw[1] = pkt->motionY; + sensorData.flowRateRaw[2] = 0; sensorData.quality = (int)pkt->quality * 100 / 255; hasNewData = true; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index dde9150c59..8f4e383247 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -64,12 +64,17 @@ #include "fc/fc_core.h" #include "fc/config.h" +#include "fc/controlrate_profile.h" +#include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/fc_tasks.h" #include "flight/imu.h" +#include "flight/mixer.h" #include "flight/pid.h" +#include "flight/rth_estimator.h" #include "flight/wind_estimator.h" #include "navigation/navigation.h" @@ -77,6 +82,7 @@ #include "rx/rx.h" #include "sensors/battery.h" +#include "sensors/boardalignment.h" #include "sensors/diagnostics.h" #include "sensors/sensors.h" #include "sensors/pitotmeter.h" @@ -116,7 +122,6 @@ x; \ }) -static timeUs_t flyTime = 0; static unsigned currentLayout = 0; static int layoutOverride = -1; @@ -160,7 +165,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, 2); +PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 3); static int digitCount(int32_t value) { @@ -460,7 +465,7 @@ static inline void osdFormatOnTime(char *buff) static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr) { - uint32_t seconds = flyTime / 1000000; + uint32_t seconds = getFlightTime(); osdFormatTime(buff, seconds, SYM_FLY_M, SYM_FLY_H); if (attr && osdConfig()->time_alarm > 0) { if (seconds / 60 >= osdConfig()->time_alarm && ARMING_FLAG(ARMED)) { @@ -722,7 +727,7 @@ static void osdFormatBatteryChargeSymbol(char *buff) static void osdUpdateBatteryCapacityOrVoltageTextAttributes(textAttributes_t *attr) { - if ((getBatteryState() != BATTERY_NOT_PRESENT) && ((batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= batteryConfig()->capacity.warning - batteryConfig()->capacity.critical)) || ((!batteryUsesCapacityThresholds()) && (getBatteryVoltage() <= getBatteryWarningVoltage())))) + if ((getBatteryState() != BATTERY_NOT_PRESENT) && ((batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= currentBatteryProfile->capacity.warning - currentBatteryProfile->capacity.critical)) || ((!batteryUsesCapacityThresholds()) && (getBatteryVoltage() <= getBatteryWarningVoltage())))) TEXT_ATTRIBUTES_ADD_BLINK(*attr); } @@ -749,7 +754,7 @@ static void osdCrosshairsBounds(uint8_t *x, uint8_t *y, uint8_t *length) * uses the THR value applied by the system rather than the * input value received by the sticks. **/ -static void osdFormatThrottlePosition(char *buff, bool autoThr) +static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr) { buff[0] = SYM_THR; buff[1] = SYM_THR1; @@ -758,6 +763,8 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr) buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; thr = rcCommand[THROTTLE]; + if (isFixedWingAutoThrottleManuallyIncreased()) + TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } tfp_sprintf(buff + 2, "%3d", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); } @@ -871,7 +878,7 @@ static int osdGetHeadingAngle(int angle) */ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t centerSym, uint32_t poiDistance, int16_t poiDirection, uint8_t poiSymbol, - uint16_t *drawn) + uint16_t *drawn, uint32_t *usedScale) { // TODO: These need to be tested with several setups. We might // need to make them configurable. @@ -905,16 +912,20 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente *drawn = 0; } - uint32_t scale; + uint32_t initialScale; float scaleToUnit; int scaleUnitDivisor; char symUnscaled; char symScaled; int maxDecimals; + const int 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 + const int scaleReductionMultiplier = MIN(midX - hMargin, midY - vMargin) / 2; switch (osdConfig()->units) { case OSD_UNIT_IMPERIAL: - scale = 16; // 16m ~= 0.01miles + initialScale = 16; // 16m ~= 0.01miles scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber() scaleUnitDivisor = 0; symUnscaled = SYM_MI; @@ -924,7 +935,7 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_METRIC: - scale = 10; // 10m as initial scale + 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; @@ -933,6 +944,15 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente break; } + // Try to keep the same scale when getting closer until we draw over the center point + uint32_t scale = initialScale; + if (*usedScale) { + scale = *usedScale; + if (scale > initialScale && poiDistance < *usedScale * scaleReductionMultiplier) { + scale /= scaleMultiplier; + } + } + if (STATE(GPS_FIX)) { int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading); @@ -941,7 +961,7 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente float poiCos = cos_approx(poiAngle); // Now start looking for a valid scale that lets us draw everything - for (int ii = 0; ii < 50; ii++, scale *= 2) { + for (int ii = 0; ii < 50; ii++, scale *= scaleMultiplier) { // Calculate location of the aircraft in map int points = poiDistance / (float)(scale / charHeight); @@ -993,28 +1013,99 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente buf[3] = scaled ? symScaled : symUnscaled; buf[4] = '\0'; displayWrite(osdDisplayPort, minX + 1, maxY, buf); + *usedScale = scale; } /* Draws a map with the home in the center and the craft moving around. * See osdDrawMap() for reference. */ -static void osdDrawHomeMap(int referenceHeading, uint8_t referenceSym, uint16_t *drawn) +static void osdDrawHomeMap(int referenceHeading, uint8_t referenceSym, uint16_t *drawn, uint32_t *usedScale) { - osdDrawMap(referenceHeading, referenceSym, SYM_HOME, GPS_distanceToHome, GPS_directionToHome, SYM_ARROW_UP, drawn); + osdDrawMap(referenceHeading, referenceSym, SYM_HOME, GPS_distanceToHome, GPS_directionToHome, SYM_ARROW_UP, drawn, usedScale); } /* Draws a map with the aircraft in the center and the home moving around. * See osdDrawMap() for reference. */ -static void osdDrawRadar(uint16_t *drawn) +static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale) { int16_t reference = DECIDEGREES_TO_DEGREES(osdGetHeading()); int16_t poiDirection = osdGetHeadingAngle(GPS_directionToHome + 180); - osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn); + osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale); } #endif +void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController) { + strcpy(buff, label); + uint8_t label_len = strlen(label); + for (uint8_t i = label_len; i < 5; ++i) buff[i] = ' '; + osdFormatCentiNumber(buff + 5, pidController->proportional, 0, 1, 0, 4); + buff[9] = ' '; + osdFormatCentiNumber(buff + 10, pidController->integrator, 0, 1, 0, 4); + buff[14] = ' '; + osdFormatCentiNumber(buff + 15, pidController->derivative, 0, 1, 0, 4); + buff[19] = ' '; + osdFormatCentiNumber(buff + 20, pidController->output_constrained, 0, 1, 0, 4); + buff[24] = '\0'; +} + +static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_t voltage, uint8_t digits, uint8_t decimals) +{ + char buff[6]; + textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; + + osdFormatBatteryChargeSymbol(buff); + buff[1] = '\0'; + osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + + elemAttr = TEXT_ATTRIBUTES_NONE; + osdFormatCentiNumber(buff, voltage, 0, decimals, 0, MIN(digits, 4)); + strcat(buff, "V"); + if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getBatteryVoltage() <= getBatteryWarningVoltage())) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); +} + +static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, const pid8_t *pid, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD) +{ + textAttributes_t elemAttr; + char buff[4]; + + displayWrite(osdDisplayPort, elemPosX, elemPosY, str); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pid->P); + if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P)))) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pid->I); + if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I)))) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pid->D); + if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D)))) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); +} + +static void osdDisplayAdjustableDecimalValue(uint8_t elemPosX, uint8_t elemPosY, const char *str, const uint8_t valueOffset, const float value, const uint8_t valueLength, const uint8_t maxDecimals, adjustmentFunction_e adjFunc) { + char buff[8]; + textAttributes_t elemAttr; + displayWrite(osdDisplayPort, elemPosX, elemPosY, str); + + elemAttr = TEXT_ATTRIBUTES_NONE; + osdFormatCentiNumber(buff, value * 100, 0, maxDecimals, 0, MIN(valueLength, 8)); + if (isAdjustmentFunctionSelected(adjFunc)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr); +} + static bool osdDrawSingleElement(uint8_t item) { uint16_t pos = osdConfig()->item_pos[currentLayout][item]; @@ -1040,29 +1131,11 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_MAIN_BATT_VOLTAGE: - osdFormatBatteryChargeSymbol(buff); - buff[1] = '\0'; - osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); - elemAttr = TEXT_ATTRIBUTES_NONE; - osdFormatCentiNumber(buff, getBatteryVoltage(), 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); - strcat(buff, "V"); - if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getBatteryVoltage() <= getBatteryWarningVoltage())) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); + osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawVoltage(), 2 + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); return true; case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE: - osdFormatBatteryChargeSymbol(buff); - buff[1] = '\0'; - osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); - elemAttr = TEXT_ATTRIBUTES_NONE; - osdFormatCentiNumber(buff, getSagCompensatedBatteryVoltage(), 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); - strcat(buff, "V"); - if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getSagCompensatedBatteryVoltage() <= getBatteryWarningVoltage())) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); + osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedVoltage(), 2 + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); return true; case OSD_CURRENT_DRAW: @@ -1083,18 +1156,18 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_BATTERY_REMAINING_CAPACITY: - buff[0] = (batteryConfig()->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH); + buff[0] = (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH); - if (batteryConfig()->capacity.value == 0) + if (currentBatteryProfile->capacity.value == 0) tfp_sprintf(buff + 1, "NA"); else if (!batteryWasFullWhenPluggedIn()) tfp_sprintf(buff + 1, "NF"); - else if (batteryConfig()->capacity.unit == BAT_CAPACITY_UNIT_MAH) + else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) tfp_sprintf(buff + 1, "%-4lu", getBatteryRemainingCapacity()); - else // batteryConfig()->capacity.unit == BAT_CAPACITY_UNIT_MWH + else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3); - if ((getBatteryState() != BATTERY_NOT_PRESENT) && batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= batteryConfig()->capacity.warning - batteryConfig()->capacity.critical)) + if ((getBatteryState() != BATTERY_NOT_PRESENT) && batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= currentBatteryProfile->capacity.warning - currentBatteryProfile->capacity.critical)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); break; @@ -1104,6 +1177,15 @@ static bool osdDrawSingleElement(uint8_t item) osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); break; + case OSD_POWER_SUPPLY_IMPEDANCE: + if (isPowerSupplyImpedanceValid()) + tfp_sprintf(buff, "%3d", getPowerSupplyImpedance()); + else + strcpy(buff, "---"); + buff[3] = SYM_MILLIOHM; + buff[4] = '\0'; + break; + #ifdef USE_GPS case OSD_GPS_SATS: buff[0] = SYM_SAT_L; @@ -1128,14 +1210,35 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HOME_DIR: { - int16_t h = osdGetHeadingAngle(GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading())); - h = h * 2 / 45; - - buff[0] = SYM_ARROW_UP + h; + // There are 16 orientations for the home direction arrow. + // so we use 22.5deg per image, where the 1st image is used + // for [349, 11], the 2nd for [12, 33], etc... + int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()); + // Add 11 to the angle, so first character maps to [349, 11] + int homeArrowDir = osdGetHeadingAngle(homeDirection + 11); + unsigned arrowOffset = homeArrowDir * 2 / 45; + buff[0] = SYM_ARROW_UP + arrowOffset; buff[1] = 0; break; } + case OSD_HOME_HEADING_ERROR: + { + buff[0] = SYM_HOME; + buff[1] = SYM_HEADING; + + if (isImuHeadingValid() && navigationPositionEstimateIsHealthy()) { + int16_t h = lrintf(CENTIDEGREES_TO_DEGREES((float)wrap_18000(DEGREES_TO_CENTIDEGREES((int32_t)GPS_directionToHome) - DECIDEGREES_TO_CENTIDEGREES((int32_t)osdGetHeading())))); + tfp_sprintf(buff + 2, "%4d", h); + } else { + strcpy(buff + 2, "----"); + } + + buff[6] = SYM_DEGREES; + buff[7] = '\0'; + break; + } + case OSD_HOME_DIST: { buff[0] = SYM_HOME; @@ -1168,6 +1271,53 @@ static bool osdDrawSingleElement(uint8_t item) buff[5] = '\0'; break; } + + case OSD_CRUISE_HEADING_ERROR: + { + if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_CRUISE_MODE)) { + displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); + return true; + } + + buff[0] = SYM_HEADING; + + if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_CRUISE_MODE) && isAdjustingPosition())) { + buff[1] = buff[2] = buff[3] = '-'; + } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) { + int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError())); + if (ABS(herr) > 99) + strcpy(buff + 1, ">99"); + else + tfp_sprintf(buff + 1, "%3d", herr); + } + + buff[4] = SYM_DEGREES; + buff[5] = '\0'; + break; + } + + case OSD_CRUISE_HEADING_ADJUSTMENT: + { + int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment())); + + if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_CRUISE_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) { + displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); + return true; + } + + buff[0] = SYM_HEADING; + + if (!ARMING_FLAG(ARMED)) { + buff[1] = buff[2] = buff[3] = buff[4] = '-'; + } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) { + tfp_sprintf(buff + 1, "%4d", heading_adjust); + } + + buff[5] = SYM_DEGREES; + buff[6] = '\0'; + break; + } + case OSD_GPS_HDOP: { buff[0] = SYM_HDP_L; @@ -1180,19 +1330,22 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_MAP_NORTH: { static uint16_t drawn = 0; - osdDrawHomeMap(0, 'N', &drawn); + static uint32_t scale = 0; + osdDrawHomeMap(0, 'N', &drawn, &scale); return true; } case OSD_MAP_TAKEOFF: { static uint16_t drawn = 0; - osdDrawHomeMap(CENTIDEGREES_TO_DEGREES(navigationGetHomeHeading()), 'T', &drawn); + static uint32_t scale = 0; + osdDrawHomeMap(CENTIDEGREES_TO_DEGREES(navigationGetHomeHeading()), 'T', &drawn, &scale); return true; } case OSD_RADAR: { static uint16_t drawn = 0; - osdDrawRadar(&drawn); + static uint32_t scale = 0; + osdDrawRadar(&drawn, &scale); return true; } #endif // GPS @@ -1233,6 +1386,60 @@ static bool osdDrawSingleElement(uint8_t item) break; } + case OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH: + { + static timeUs_t updatedTimestamp = 0; + /*static int32_t updatedTimeSeconds = 0;*/ + timeUs_t currentTimeUs = micros(); + static int32_t timeSeconds = -1; + if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { + timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation); + updatedTimestamp = currentTimeUs; + } + if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) { + buff[0] = SYM_FLY_M; + strcpy(buff + 1, "--:--"); + updatedTimestamp = 0; + } else if (timeSeconds == -2) { + // Wind is too strong to come back with cruise throttle + buff[0] = SYM_FLY_M; + buff[1] = buff[2] = buff[4] = buff[5] = SYM_WIND_HORIZONTAL; + buff[3] = ':'; + buff[6] = '\0'; + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else { + osdFormatTime(buff, timeSeconds, SYM_FLY_M, SYM_FLY_H); + if (timeSeconds == 0) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + break; + + case OSD_REMAINING_DISTANCE_BEFORE_RTH:; + static timeUs_t updatedTimestamp = 0; + timeUs_t currentTimeUs = micros(); + static int32_t distanceMeters = -1; + if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { + distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation); + updatedTimestamp = currentTimeUs; + } + buff[0] = SYM_TRIP_DIST; + if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) { + buff[1] = SYM_DIST_M; + strcpy(buff + 2, "---"); + } 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[5] = '\0'; + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else { + osdFormatDistanceSymbol(buff + 1, distanceMeters * 100); + if (distanceMeters == 0) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + case OSD_FLYMODE: { char *p = "ACRO"; @@ -1254,17 +1461,22 @@ static bool osdDrawSingleElement(uint8_t item) } else { p = " PH "; } + } else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) { + p = "3CRS"; + } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) { + p = "CRS "; } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) { // If navigationRequiresAngleMode() returns false when ALTHOLD is active, // it means it can be combined with ANGLE, HORIZON, ACRO, etc... // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE. p = " AH "; - } else if (FLIGHT_MODE(NAV_WP_MODE)) + } else if (FLIGHT_MODE(NAV_WP_MODE)) { p = " WP "; - else if (FLIGHT_MODE(ANGLE_MODE)) + } else if (FLIGHT_MODE(ANGLE_MODE)) { p = "ANGL"; - else if (FLIGHT_MODE(HORIZON_MODE)) + } else if (FLIGHT_MODE(HORIZON_MODE)) { p = "HOR "; + } displayWrite(osdDisplayPort, elemPosX, elemPosY, p); return true; @@ -1284,11 +1496,11 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_THROTTLE_POS: { - osdFormatThrottlePosition(buff, false); + osdFormatThrottlePosition(buff, false, NULL); break; } -#if defined(VTX) || defined(VTX_COMMON) +#if defined(VTX) || defined(USE_VTX_COMMON) case OSD_VTX_CHANNEL: #if defined(VTX) // FIXME: This doesn't actually work. It's for boards with @@ -1298,8 +1510,21 @@ static bool osdDrawSingleElement(uint8_t item) { uint8_t band = 0; uint8_t channel = 0; - vtxCommonGetBandAndChannel(&band, &channel); - tfp_sprintf(buff, "CH:%c%s", vtx58BandLetter[band], vtx58ChannelNames[channel]); + 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; + } + } + tfp_sprintf(buff, "CH:%c%s:%c", bandChr, channelStr, powerChr); } #endif break; @@ -1327,13 +1552,13 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_ATTITUDE_ROLL: buff[0] = SYM_ROLL_LEVEL; - if (attitude.values.roll) + if (ABS(attitude.values.roll) >= (osdConfig()->attitude_angle_decimals ? 1 : 10)) buff[0] += (attitude.values.roll < 0 ? -1 : 1); osdFormatCentiNumber(buff + 1, ABS(attitude.values.roll) * 10, 0, osdConfig()->attitude_angle_decimals, 0, 2 + osdConfig()->attitude_angle_decimals); break; case OSD_ATTITUDE_PITCH: - if (attitude.values.pitch == 0) + if (ABS(attitude.values.pitch) < (osdConfig()->attitude_angle_decimals ? 1 : 10)) buff[0] = 'P'; else if (attitude.values.pitch > 0) buff[0] = SYM_PITCH_DOWN; @@ -1535,20 +1760,176 @@ static bool osdDrawSingleElement(uint8_t item) #endif case OSD_ROLL_PIDS: - { - tfp_sprintf(buff, "ROL %3d %3d %3d", pidBank()->pid[PID_ROLL].P, pidBank()->pid[PID_ROLL].I, pidBank()->pid[PID_ROLL].D); - break; - } + osdDisplayPIDValues(elemPosX, elemPosY, "ROL", &pidBank()->pid[PID_ROLL], ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D); + return true; case OSD_PITCH_PIDS: + osdDisplayPIDValues(elemPosX, elemPosY, "PIT", &pidBank()->pid[PID_PITCH], ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D); + return true; + + case OSD_YAW_PIDS: + osdDisplayPIDValues(elemPosX, elemPosY, "YAW", &pidBank()->pid[PID_YAW], ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D); + return true; + + case OSD_LEVEL_PIDS: + osdDisplayPIDValues(elemPosX, elemPosY, "LEV", &pidBank()->pid[PID_LEVEL], ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D); + return true; + + case OSD_POS_XY_PIDS: + osdDisplayPIDValues(elemPosX, elemPosY, "PXY", &pidBank()->pid[PID_POS_XY], ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D); + return true; + + case OSD_POS_Z_PIDS: + osdDisplayPIDValues(elemPosX, elemPosY, "PZ", &pidBank()->pid[PID_POS_Z], ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D); + return true; + + case OSD_VEL_XY_PIDS: + 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)) + osdDisplayPIDValues(elemPosX, elemPosY, "VZ", &pidBank()->pid[PID_VEL_Z], ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D); + return true; + + case OSD_HEADING_P: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "HP", 0, pidBank()->pid[PID_HEADING].P, 3, 0, ADJUSTMENT_HEADING_P); + return true; + + case OSD_BOARD_ALIGN_ROLL: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AR", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->rollDeciDegrees), 4, 1, ADJUSTMENT_ROLL_BOARD_ALIGNMENT); + return true; + + case OSD_BOARD_ALIGN_PITCH: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AP", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->pitchDeciDegrees), 4, 1, ADJUSTMENT_PITCH_BOARD_ALIGNMENT); + return true; + + case OSD_RC_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "EXP", 0, currentControlRateProfile->stabilized.rcExpo8, 3, 0, ADJUSTMENT_RC_EXPO); + return true; + + case OSD_RC_YAW_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "YEX", 0, currentControlRateProfile->stabilized.rcYawExpo8, 3, 0, ADJUSTMENT_RC_YAW_EXPO); + return true; + + case OSD_THROTTLE_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TEX", 0, currentControlRateProfile->throttle.rcExpo8, 3, 0, ADJUSTMENT_THROTTLE_EXPO); + return true; + + case OSD_PITCH_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "SPR"); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_PITCH]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; + + case OSD_ROLL_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "SRR"); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_ROLL]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; + + case OSD_YAW_RATE: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "SYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); + return true; + + case OSD_MANUAL_RC_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MEX", 0, currentControlRateProfile->manual.rcExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_EXPO); + return true; + + case OSD_MANUAL_RC_YAW_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYX", 0, currentControlRateProfile->manual.rcYawExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_YAW_EXPO); + return true; + + case OSD_MANUAL_PITCH_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "MPR"); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_PITCH]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; + + case OSD_MANUAL_ROLL_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "MRR"); + + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_ROLL]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; + + case OSD_MANUAL_YAW_RATE: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); + return true; + + case OSD_NAV_FW_CRUISE_THR: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRS", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR); + return true; + + case OSD_NAV_FW_PITCH2THR: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, navConfig()->fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR); + return true; + + case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)mixerConfig()->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE); + return true; + + case OSD_FW_ALT_PID_OUTPUTS: { - tfp_sprintf(buff, "PIT %3d %3d %3d", pidBank()->pid[PID_PITCH].P, pidBank()->pid[PID_PITCH].I, pidBank()->pid[PID_PITCH].D); + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + osdFormatPidControllerOutput(buff, "PZO", &nav_pids->fw_alt); break; } - case OSD_YAW_PIDS: + case OSD_FW_POS_PID_OUTPUTS: { - tfp_sprintf(buff, "YAW %3d %3d %3d", pidBank()->pid[PID_YAW].P, pidBank()->pid[PID_YAW].I, pidBank()->pid[PID_YAW].D); + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + osdFormatPidControllerOutput(buff, "PXYO", &nav_pids->fw_nav); + break; + } + + case OSD_MC_VEL_Z_PID_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + osdFormatPidControllerOutput(buff, "VZO", &nav_pids->vel[Z]); + break; + } + + case OSD_MC_VEL_X_PID_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + osdFormatPidControllerOutput(buff, "VXO", &nav_pids->vel[X]); + break; + } + + case OSD_MC_VEL_Y_PID_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + osdFormatPidControllerOutput(buff, "VYO", &nav_pids->vel[Y]); + break; + } + + case OSD_MC_POS_XYZ_P_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + strcpy(buff, "POSO "); + osdFormatCentiNumber(buff + 5, nav_pids->pos[X].output_constrained, 0, 1, 0, 4); + buff[9] = ' '; + osdFormatCentiNumber(buff + 10, nav_pids->pos[Y].output_constrained, 0, 1, 0, 4); + buff[14] = ' '; + osdFormatCentiNumber(buff + 15, nav_pids->pos[Z].output_constrained, 0, 1, 0, 4); + buff[19] = '\0'; break; } @@ -1664,23 +2045,19 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_MAIN_BATT_CELL_VOLTAGE: { - uint16_t cellBattCentiVolts = getBatteryAverageCellVoltage(); - osdFormatBatteryChargeSymbol(buff); - buff[1] = '\0'; - osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); - elemAttr = TEXT_ATTRIBUTES_NONE; - osdFormatCentiNumber(buff, cellBattCentiVolts, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 1); - strcat(buff, "V"); - if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getBatteryVoltage() <= getBatteryWarningVoltage())) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); + osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), 3, 2); + return true; + } + + case OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE: + { + osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedAverageCellVoltage(), 3, 2); return true; } case OSD_THROTTLE_POS_AUTO_THR: { - osdFormatThrottlePosition(buff, true); + osdFormatThrottlePosition(buff, true, &elemAttr); break; } @@ -1862,11 +2239,19 @@ static bool osdDrawSingleElement(uint8_t item) static uint8_t osdIncElementIndex(uint8_t elementIndex) { ++elementIndex; + if (!sensors(SENSOR_ACC)) { if (elementIndex == OSD_CROSSHAIRS) { elementIndex = OSD_ONTIME; } } + + if (!feature(FEATURE_VBAT)) { + if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) { + elementIndex = OSD_LEVEL_PIDS; + } + } + if (!feature(FEATURE_CURRENT_METER)) { if (elementIndex == OSD_CURRENT_DRAW) { elementIndex = OSD_GPS_SPEED; @@ -1874,7 +2259,14 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex) if (elementIndex == OSD_EFFICIENCY_MAH_PER_KM) { elementIndex = OSD_TRIP_DIST; } + if (elementIndex == OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH) { + elementIndex = OSD_HOME_HEADING_ERROR; + } + if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) { + elementIndex = OSD_LEVEL_PIDS; + } } + if (!feature(FEATURE_GPS)) { if (elementIndex == OSD_GPS_SPEED) { elementIndex = OSD_ALTITUDE; @@ -1888,6 +2280,9 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex) if (elementIndex == OSD_TRIP_DIST) { elementIndex = OSD_ATTITUDE_PITCH; } + if (elementIndex == OSD_MAP_NORTH) { + elementIndex = OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE; + } } if (elementIndex == OSD_ITEM_COUNT) { @@ -1917,17 +2312,21 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1); osdConfig->item_pos[0][OSD_TRIP_DIST] = OSD_POS(1, 2); osdConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1); + osdConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1); osdConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1); osdConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG; osdConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2); osdConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2); + 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_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); osdConfig->item_pos[0][OSD_BATTERY_REMAINING_PERCENT] = OSD_POS(1, 7); + osdConfig->item_pos[0][OSD_POWER_SUPPLY_IMPEDANCE] = OSD_POS(1, 8); osdConfig->item_pos[0][OSD_EFFICIENCY_MAH_PER_KM] = OSD_POS(1, 5); osdConfig->item_pos[0][OSD_EFFICIENCY_WH_PER_KM] = OSD_POS(1, 5); @@ -1950,6 +2349,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[0][OSD_FLYTIME] = OSD_POS(23, 9); osdConfig->item_pos[0][OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | OSD_VISIBLE_FLAG; osdConfig->item_pos[0][OSD_RTC_TIME] = OSD_POS(23, 12); + osdConfig->item_pos[0][OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH] = OSD_POS(23, 7); + osdConfig->item_pos[0][OSD_REMAINING_DISTANCE_BEFORE_RTH] = OSD_POS(23, 6); osdConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG; osdConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10); @@ -1961,6 +2362,35 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[0][OSD_ROLL_PIDS] = OSD_POS(2, 10); osdConfig->item_pos[0][OSD_PITCH_PIDS] = OSD_POS(2, 11); osdConfig->item_pos[0][OSD_YAW_PIDS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_LEVEL_PIDS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_POS_XY_PIDS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_POS_Z_PIDS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_VEL_XY_PIDS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_VEL_Z_PIDS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_HEADING_P] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_BOARD_ALIGN_ROLL] = OSD_POS(2, 10); + osdConfig->item_pos[0][OSD_BOARD_ALIGN_PITCH] = OSD_POS(2, 11); + osdConfig->item_pos[0][OSD_RC_EXPO] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_RC_YAW_EXPO] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_THROTTLE_EXPO] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_PITCH_RATE] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_ROLL_RATE] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_YAW_RATE] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MANUAL_RC_EXPO] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MANUAL_RC_YAW_EXPO] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MANUAL_PITCH_RATE] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MANUAL_ROLL_RATE] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MANUAL_YAW_RATE] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_NAV_FW_CRUISE_THR] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_NAV_FW_PITCH2THR] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_FW_ALT_PID_OUTPUTS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_FW_POS_PID_OUTPUTS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MC_VEL_X_PID_OUTPUTS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MC_VEL_Y_PID_OUTPUTS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MC_VEL_Z_PID_OUTPUTS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_MC_POS_XYZ_P_OUTPUTS] = OSD_POS(2, 12); + osdConfig->item_pos[0][OSD_POWER] = OSD_POS(15, 1); osdConfig->item_pos[0][OSD_AIR_SPEED] = OSD_POS(3, 5); @@ -1982,7 +2412,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->dist_alarm = 1000; osdConfig->neg_alt_alarm = 5; - osdConfig->video_system = 0; + osdConfig->video_system = VIDEO_SYSTEM_AUTO; osdConfig->ahi_reverse_roll = 0; osdConfig->crosshairs_style = OSD_CROSSHAIRS_STYLE_DEFAULT; @@ -1993,6 +2423,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->units = OSD_UNIT_METRIC; osdConfig->main_voltage_decimals = 1; osdConfig->attitude_angle_decimals = 0; + + osdConfig->estimations_wind_compensation = true; } static void osdSetNextRefreshIn(uint32_t timeMs) { @@ -2194,7 +2626,7 @@ static void osdShowStats(void) } displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); - uint32_t flySeconds = flyTime / 1000000; + uint16_t flySeconds = getFlightTime(); uint16_t flyMinutes = flySeconds / 60; flySeconds %= 60; uint16_t flyHours = flyMinutes / 60; @@ -2239,8 +2671,6 @@ static void osdShowArmed(void) static void osdRefresh(timeUs_t currentTimeUs) { - static timeUs_t lastTimeUs = 0; - if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu)) { displayClearScreen(osdDisplayPort); armState = ARMING_FLAG(ARMED); @@ -2261,13 +2691,6 @@ static void osdRefresh(timeUs_t currentTimeUs) armState = ARMING_FLAG(ARMED); } - if (ARMING_FLAG(ARMED)) { - timeUs_t deltaT = currentTimeUs - lastTimeUs; - flyTime += deltaT; - } - - lastTimeUs = currentTimeUs; - if (resumeRefreshAt) { // If we already reached he time for the next refresh, // or THR is high or PITCH is high, resume refreshing. diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 1a6aed8a9f..a47c1d4280 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -20,6 +20,8 @@ #include "common/time.h" #include "config/parameter_group.h" +#include "drivers/vcd.h" + #ifndef OSD_ALTERNATE_LAYOUT_COUNT #define OSD_ALTERNATE_LAYOUT_COUNT 3 #endif @@ -80,10 +82,45 @@ typedef enum { OSD_MAP_NORTH, OSD_MAP_TAKEOFF, OSD_RADAR, - OSD_DEBUG, // Number 46. Intentionally absent from configurator and CMS. Set it from CLI. OSD_WIND_SPEED_HORIZONTAL, OSD_WIND_SPEED_VERTICAL, + OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH, + OSD_REMAINING_DISTANCE_BEFORE_RTH, + OSD_HOME_HEADING_ERROR, + OSD_CRUISE_HEADING_ERROR, + OSD_CRUISE_HEADING_ADJUSTMENT, OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE, + OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE, + OSD_POWER_SUPPLY_IMPEDANCE, + OSD_LEVEL_PIDS, + OSD_POS_XY_PIDS, + OSD_POS_Z_PIDS, + OSD_VEL_XY_PIDS, + OSD_VEL_Z_PIDS, + OSD_HEADING_P, + OSD_BOARD_ALIGN_ROLL, + OSD_BOARD_ALIGN_PITCH, + OSD_RC_EXPO, + OSD_RC_YAW_EXPO, + OSD_THROTTLE_EXPO, + OSD_PITCH_RATE, + OSD_ROLL_RATE, + OSD_YAW_RATE, + OSD_MANUAL_RC_EXPO, + OSD_MANUAL_RC_YAW_EXPO, + OSD_MANUAL_PITCH_RATE, + OSD_MANUAL_ROLL_RATE, + OSD_MANUAL_YAW_RATE, + OSD_NAV_FW_CRUISE_THR, + OSD_NAV_FW_PITCH2THR, + OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, + OSD_DEBUG, // Intentionally absent from configurator and CMS. Set it from CLI. + OSD_FW_ALT_PID_OUTPUTS, + OSD_FW_POS_PID_OUTPUTS, + OSD_MC_VEL_X_PID_OUTPUTS, + OSD_MC_VEL_Y_PID_OUTPUTS, + OSD_MC_VEL_Z_PID_OUTPUTS, + OSD_MC_POS_XYZ_P_OUTPUTS, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -121,7 +158,7 @@ typedef struct osdConfig_s { uint16_t dist_alarm; // home distance in m uint16_t neg_alt_alarm; // abs(negative altitude) in m - uint8_t video_system; + videoSystem_e video_system; uint8_t row_shiftdown; // Preferences @@ -135,6 +172,8 @@ typedef struct osdConfig_s { uint8_t units; // from osd_unit_e uint8_t stats_energy_unit; // from osd_stats_energy_unit_e + + bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c new file mode 100644 index 0000000000..1dc0e0d50a --- /dev/null +++ b/src/main/io/vtx.c @@ -0,0 +1,279 @@ +/* + * 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 "platform.h" + +#if defined(USE_VTX_COMMON) + +#include "common/maths.h" +#include "common/time.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/vtx_common.h" + +#include "fc/cli.h" +#include "fc/config.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" + +#include "flight/failsafe.h" + +#include "io/vtx.h" +#include "io/vtx_string.h" +#include "io/vtx_control.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 0); + +PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, + .band = VTX_SETTINGS_DEFAULT_BAND, + .channel = VTX_SETTINGS_DEFAULT_CHANNEL, + .power = VTX_SETTINGS_DEFAULT_POWER, + .freq = VTX_SETTINGS_DEFAULT_FREQ, + .pitModeFreq = VTX_SETTINGS_DEFAULT_PITMODE_FREQ, + .lowPowerDisarm = VTX_LOW_POWER_DISARM_OFF, +); + +typedef enum { + VTX_PARAM_POWER = 0, + VTX_PARAM_BANDCHAN, + VTX_PARAM_PITMODE, + VTX_PARAM_CONFIRM, + VTX_PARAM_COUNT +} vtxScheduleParams_e; + +void vtxInit(void) +{ + bool settingsUpdated = false; + + // sync frequency in parameter group when band/channel are specified + const uint16_t freq = vtx58_Bandchan2Freq(vtxSettingsConfig()->band, vtxSettingsConfig()->channel); + if (vtxSettingsConfig()->band && freq != vtxSettingsConfig()->freq) { + vtxSettingsConfigMutable()->freq = freq; + settingsUpdated = true; + } + +#if defined(VTX_SETTINGS_FREQCMD) + // constrain pit mode frequency + if (vtxSettingsConfig()->pitModeFreq) { + const uint16_t constrainedPitModeFreq = MAX(vtxSettingsConfig()->pitModeFreq, VTX_SETTINGS_MIN_USER_FREQ); + if (constrainedPitModeFreq != vtxSettingsConfig()->pitModeFreq) { + vtxSettingsConfigMutable()->pitModeFreq = constrainedPitModeFreq; + settingsUpdated = true; + } + } +#endif + + if (settingsUpdated) { + saveConfigAndNotify(); + } +} + +static vtxSettingsConfig_t vtxGetSettings(void) +{ + vtxSettingsConfig_t settings = { + .band = vtxSettingsConfig()->band, + .channel = vtxSettingsConfig()->channel, + .power = vtxSettingsConfig()->power, + .freq = vtxSettingsConfig()->freq, + .pitModeFreq = vtxSettingsConfig()->pitModeFreq, + .lowPowerDisarm = vtxSettingsConfig()->lowPowerDisarm, + }; + +#if 0 +#if defined(VTX_SETTINGS_FREQCMD) + if (IS_RC_MODE_ACTIVE(BOXVTXPITMODE) && isModeActivationConditionPresent(BOXVTXPITMODE) && settings.pitModeFreq) { + settings.band = 0; + settings.freq = settings.pitModeFreq; + settings.power = VTX_SETTINGS_DEFAULT_POWER; + } +#endif +#endif + + if (!ARMING_FLAG(ARMED) && !failsafeIsActive() && + ((settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS) || + (settings.lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED)))) { + + settings.power = VTX_SETTINGS_DEFAULT_POWER; + } + + return settings; +} + +static bool vtxProcessBandAndChannel(vtxDevice_t *vtxDevice) +{ + if(!ARMING_FLAG(ARMED)) { + uint8_t vtxBand; + uint8_t vtxChan; + if (vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) { + const vtxSettingsConfig_t settings = vtxGetSettings(); + if (vtxBand != settings.band || vtxChan != settings.channel) { + vtxCommonSetBandAndChannel(vtxDevice, settings.band, settings.channel); + return true; + } + } + } + return false; +} + +#if defined(VTX_SETTINGS_FREQCMD) +static bool vtxProcessFrequency(vtxDevice_t *vtxDevice) +{ + if(!ARMING_FLAG(ARMED)) { + uint16_t vtxFreq; + if (vtxCommonGetFrequency(vtxDevice, &vtxFreq)) { + const vtxSettingsConfig_t settings = vtxGetSettings(); + if (vtxFreq != settings.freq) { + vtxCommonSetFrequency(vtxDevice, settings.freq); + return true; + } + } + } + return false; +} +#endif + +static bool vtxProcessPower(vtxDevice_t *vtxDevice) +{ + uint8_t vtxPower; + if (vtxCommonGetPowerIndex(vtxDevice, &vtxPower)) { + const vtxSettingsConfig_t settings = vtxGetSettings(); + if (vtxPower != settings.power) { + vtxCommonSetPowerByIndex(vtxDevice, settings.power); + return true; + } + } + return false; +} + +static bool vtxProcessPitMode(vtxDevice_t *vtxDevice) +{ + uint8_t pitOnOff; + + bool currPmSwitchState = false; + static bool prevPmSwitchState = false; + + if (!ARMING_FLAG(ARMED) && vtxCommonGetPitMode(vtxDevice, &pitOnOff)) { + + // Not supported on INAV yet. It might not be that useful. +#if 0 + currPmSwitchState = IS_RC_MODE_ACTIVE(BOXVTXPITMODE); +#endif + + if (currPmSwitchState != prevPmSwitchState) { + prevPmSwitchState = currPmSwitchState; + + if (currPmSwitchState) { +#if defined(VTX_SETTINGS_FREQCMD) + if (vtxSettingsConfig()->pitModeFreq) { + return false; + } +#endif + +#if 0 + if (isModeActivationConditionPresent(BOXVTXPITMODE)) { +#endif + if (0) { + if (!pitOnOff) { + vtxCommonSetPitMode(vtxDevice, true); + return true; + } + } + } else { + if (pitOnOff) { + vtxCommonSetPitMode(vtxDevice, false); + return true; + } + } + } + } + return false; +} + +static bool vtxProcessStateUpdate(vtxDevice_t *vtxDevice) +{ + const vtxSettingsConfig_t vtxSettingsState = vtxGetSettings(); + vtxSettingsConfig_t vtxState = vtxSettingsState; + + if (vtxSettingsState.band) { + vtxCommonGetBandAndChannel(vtxDevice, &vtxState.band, &vtxState.channel); +#if defined(VTX_SETTINGS_FREQCMD) + } else { + vtxCommonGetFrequency(vtxDevice, &vtxState.freq); +#endif + } + + vtxCommonGetPowerIndex(vtxDevice, &vtxState.power); + + return (bool)memcmp(&vtxSettingsState, &vtxState, sizeof(vtxSettingsConfig_t)); +} + +void vtxUpdate(timeUs_t currentTimeUs) +{ + static uint8_t currentSchedule = 0; + + if (cliMode) { + return; + } + + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { + // Check input sources for config updates + vtxControlInputPoll(); + + const uint8_t startingSchedule = currentSchedule; + bool vtxUpdatePending = false; + do { + switch (currentSchedule) { + case VTX_PARAM_POWER: + vtxUpdatePending = vtxProcessPower(vtxDevice); + break; + case VTX_PARAM_BANDCHAN: + if (vtxGetSettings().band) { + vtxUpdatePending = vtxProcessBandAndChannel(vtxDevice); +#if defined(VTX_SETTINGS_FREQCMD) + } else { + vtxUpdatePending = vtxProcessFrequency(vtxDevice); +#endif + } + break; + case VTX_PARAM_PITMODE: + vtxUpdatePending = vtxProcessPitMode(vtxDevice); + break; + case VTX_PARAM_CONFIRM: + vtxUpdatePending = vtxProcessStateUpdate(vtxDevice); + break; + default: + break; + } + currentSchedule = (currentSchedule + 1) % VTX_PARAM_COUNT; + } while (!vtxUpdatePending && currentSchedule != startingSchedule); + + if (!ARMING_FLAG(ARMED) || vtxUpdatePending) { + vtxCommonProcess(vtxDevice, currentTimeUs); + } + } +} + +#endif diff --git a/src/main/io/vtx.h b/src/main/io/vtx.h new file mode 100644 index 0000000000..35500d799f --- /dev/null +++ b/src/main/io/vtx.h @@ -0,0 +1,49 @@ +/* + * 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 + +#include + +#include "platform.h" + +#include "common/time.h" + +#include "config/parameter_group.h" + +typedef enum { + VTX_LOW_POWER_DISARM_OFF = 0, + VTX_LOW_POWER_DISARM_ALWAYS = 1, + VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM = 2, // Set low power until arming for the first time +} vtxLowerPowerDisarm_e; + +typedef struct vtxSettingsConfig_s { + uint8_t band; // 1=A, 2=B, 3=E, 4=F(Airwaves/Fatshark), 5=Racebande + uint8_t channel; // 1-8 + uint8_t power; // 0 = lowest + uint16_t freq; // sets freq in MHz if band=0 + uint16_t pitModeFreq; // sets out-of-range pitmode frequency + uint8_t lowPowerDisarm; // min power while disarmed, from vtxLowerPowerDisarm_e +} vtxSettingsConfig_t; + +PG_DECLARE(vtxSettingsConfig_t, vtxSettingsConfig); + +void vtxInit(void); +void vtxUpdate(timeUs_t currentTimeUs); diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index cb410be1c7..6daebbe8e7 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -37,9 +37,14 @@ #include "io/vtx_control.h" -#if defined(VTX_CONTROL) && defined(VTX_COMMON) +#if defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON) -PG_REGISTER(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 2); + +PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, +// .vtxChannelActivationConditions = { 0 }, + .halfDuplex = true, +); static uint8_t locked = 0; @@ -48,6 +53,13 @@ void vtxControlInit(void) // NOTHING TO DO } +void vtxControlInputPoll(void) +{ + // Check variuos input sources for VTX config updates + + // XXX: None supported in INAV +} + static void vtxUpdateBandAndChannel(uint8_t bandStep, uint8_t channelStep) { if (ARMING_FLAG(ARMED)) { @@ -55,9 +67,12 @@ static void vtxUpdateBandAndChannel(uint8_t bandStep, uint8_t channelStep) } if (!locked) { - uint8_t band = 0, channel = 0; - vtxCommonGetBandAndChannel(&band, &channel); - vtxCommonSetBandAndChannel(band + bandStep, channel + channelStep); + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { + uint8_t band = 0, channel = 0; + vtxCommonGetBandAndChannel(vtxDevice, &band, &channel); + vtxCommonSetBandAndChannel(vtxDevice, band + bandStep, channel + channelStep); + } } } @@ -88,17 +103,20 @@ void vtxUpdateActivatedChannel(void) } if (!locked) { - static uint8_t lastIndex = -1; + vtxDevice_t *vtxDevice = vtxCommonDevice(); + if (vtxDevice) { + static uint8_t lastIndex = -1; - for (uint8_t index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) { - const vtxChannelActivationCondition_t *vtxChannelActivationCondition = &vtxConfig()->vtxChannelActivationConditions[index]; + for (uint8_t index = 0; index < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; index++) { + const vtxChannelActivationCondition_t *vtxChannelActivationCondition = &vtxConfig()->vtxChannelActivationConditions[index]; - if (isRangeActive(vtxChannelActivationCondition->auxChannelIndex, &vtxChannelActivationCondition->range) - && index != lastIndex) { - lastIndex = index; + if (isRangeActive(vtxChannelActivationCondition->auxChannelIndex, &vtxChannelActivationCondition->range) + && index != lastIndex) { + lastIndex = index; - vtxCommonSetBandAndChannel(vtxChannelActivationCondition->band, vtxChannelActivationCondition->channel); - break; + vtxCommonSetBandAndChannel(vtxDevice, vtxChannelActivationCondition->band, vtxChannelActivationCondition->channel); + break; + } } } } @@ -106,10 +124,16 @@ void vtxUpdateActivatedChannel(void) void vtxCycleBandOrChannel(const uint8_t bandStep, const uint8_t channelStep) { + vtxDevice_t *vtxDevice = vtxCommonDevice(); + + if (!vtxDevice) { + return; + } + uint8_t band = 0, channel = 0; vtxDeviceCapability_t capability; - bool haveAllNeededInfo = vtxCommonGetBandAndChannel(&band, &channel) && vtxCommonGetDeviceCapability(&capability); + bool haveAllNeededInfo = vtxCommonGetBandAndChannel(vtxDevice, &band, &channel) && vtxCommonGetDeviceCapability(vtxDevice, &capability); if (!haveAllNeededInfo) { return; } @@ -128,15 +152,21 @@ void vtxCycleBandOrChannel(const uint8_t bandStep, const uint8_t channelStep) newBand = capability.bandCount; } - vtxCommonSetBandAndChannel(newBand, newChannel); + vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel); } void vtxCyclePower(const uint8_t powerStep) { + vtxDevice_t *vtxDevice = vtxCommonDevice(); + + if (!vtxDevice) { + return; + } + uint8_t power = 0; vtxDeviceCapability_t capability; - bool haveAllNeededInfo = vtxCommonGetPowerIndex(&power) && vtxCommonGetDeviceCapability(&capability); + bool haveAllNeededInfo = vtxCommonGetPowerIndex(vtxDevice, &power) && vtxCommonGetDeviceCapability(vtxDevice, &capability); if (!haveAllNeededInfo) { return; } @@ -148,7 +178,7 @@ void vtxCyclePower(const uint8_t powerStep) newPower = capability.powerCount; } - vtxCommonSetPowerByIndex(newPower); + vtxCommonSetPowerByIndex(vtxDevice, newPower); } #endif diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index 3715b60e00..e39e447a9f 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -30,11 +30,13 @@ typedef struct vtxChannelActivationCondition_s { typedef struct vtxConfig_s { vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT]; + uint8_t halfDuplex; } vtxConfig_t; PG_DECLARE(vtxConfig_t, vtxConfig); void vtxControlInit(void); +void vtxControlInputPoll(void); void vtxIncrementBand(void); void vtxDecrementBand(void); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index b03c995196..f9722affd1 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -1,18 +1,21 @@ /* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * 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 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 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. + * 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 Cleanflight. If not, see . + * along with this software. + * + * If not, see . */ /* Created by jflyper */ @@ -20,64 +23,64 @@ #include #include #include +#include #include "platform.h" -#if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL) -#include "build/build_config.h" +#if defined(USE_VTX_SMARTAUDIO) && defined(USE_VTX_CONTROL) + #include "build/debug.h" #include "cms/cms.h" -#include "cms/cms_types.h" #include "cms/cms_menu_vtx_smartaudio.h" -#include "common/axis.h" +#include "common/maths.h" #include "common/printf.h" #include "common/utils.h" -#include "config/parameter_group.h" -#include "config/parameter_group_ids.h" - -#include "drivers/system.h" -#include "drivers/serial.h" #include "drivers/time.h" #include "drivers/vtx_common.h" -#include "fc/rc_controls.h" -#include "fc/runtime_config.h" - -#include "flight/pid.h" -#include "config/config_master.h" - #include "io/serial.h" +#include "io/vtx.h" +#include "io/vtx_control.h" #include "io/vtx_smartaudio.h" #include "io/vtx_string.h" -//#define SMARTAUDIO_DEBUG_MONITOR +// Timing parameters +// Note that vtxSAProcess() is normally called at 200ms interval +#define SMARTAUDIO_CMD_TIMEOUT 120 // Time until the command is considered lost +#define SMARTAUDIO_POLLING_INTERVAL 150 // Minimum time between state polling +#define SMARTAUDIO_POLLING_WINDOW 1000 // Time window after command polling for state change + +//#define USE_SMARTAUDIO_DPRINTF +//#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART1 + +#ifdef USE_SMARTAUDIO_DPRINTF +serialPort_t *debugSerialPort = NULL; +#endif // USE_SMARTAUDIO_DPRINTF static serialPort_t *smartAudioSerialPort = NULL; -#if defined(USE_CMS) || defined(VTX_COMMON) -static const char * const saPowerNames[] = { +#if defined(USE_CMS) || defined(USE_VTX_COMMON) +const char * const saPowerNames[VTX_SMARTAUDIO_POWER_COUNT+1] = { "---", "25 ", "200", "500", "800", }; #endif -#ifdef VTX_COMMON +#ifdef USE_VTX_COMMON static const vtxVTable_t saVTable; // Forward static vtxDevice_t vtxSmartAudio = { .vTable = &saVTable, - .capability = { - .bandCount = 5, - .channelCount = 8, - .powerCount = 4, - }, + .capability.bandCount = VTX_SMARTAUDIO_BAND_COUNT, + .capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT, + .capability.powerCount = VTX_SMARTAUDIO_POWER_COUNT, .bandNames = (char **)vtx58BandNames, .channelNames = (char **)vtx58ChannelNames, .powerNames = (char **)saPowerNames, }; -#endif // VTX_COMMON +#endif // SmartAudio command and response codes enum { @@ -93,10 +96,18 @@ enum { // This is not a good design; can't distinguish command from response this way. #define SACMD(cmd) (((cmd) << 1) | 1) + #define SA_IS_PITMODE(n) ((n) & SA_MODE_GET_PITMODE) #define SA_IS_PIRMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_IN_RANGE_PITMODE)) #define SA_IS_PORMODE(n) (((n) & SA_MODE_GET_PITMODE) && ((n) & SA_MODE_GET_OUT_RANGE_PITMODE)) + +// convert between 'saDevice.channel' and band/channel values +#define SA_DEVICE_CHVAL_TO_BAND(val) ((val) / (uint8_t)8) +#define SA_DEVICE_CHVAL_TO_CHANNEL(val) ((val) % (uint8_t)8) +#define SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel) ((band) * (uint8_t)8 + (channel)) + + // Statistical counters, for user side trouble shooting. smartAudioStat_t saStat = { @@ -109,7 +120,7 @@ smartAudioStat_t saStat = { .badcode = 0, }; -saPowerTable_t saPowerTable[] = { +saPowerTable_t saPowerTable[VTX_SMARTAUDIO_POWER_COUNT] = { { 25, 7, 0 }, { 200, 16, 1 }, { 500, 25, 2 }, @@ -169,9 +180,9 @@ static uint8_t CRC8(const uint8_t *data, const int8_t len) } +#ifdef USE_SMARTAUDIO_DPRINTF static void saPrintSettings(void) { -#ifdef SMARTAUDIO_DPRINTF dprintf(("Current status: version: %d\r\n", saDevice.version)); dprintf((" mode(0x%x): fmode=%s", saDevice.mode, (saDevice.mode & 1) ? "freq" : "chan")); dprintf((" pit=%s ", (saDevice.mode & 2) ? "on " : "off")); @@ -184,18 +195,17 @@ static void saPrintSettings(void) dprintf(("power: %d ", saDevice.power)); dprintf(("pitfreq: %d ", saDevice.orfreq)); dprintf(("\r\n")); -#endif } +#endif int saDacToPowerIndex(int dac) { - int idx; - - for (idx = 3 ; idx >= 0 ; idx--) { - if (saPowerTable[idx].valueV1 <= dac) - return(idx); + for (int idx = 3 ; idx >= 0 ; idx--) { + if (saPowerTable[idx].valueV1 <= dac) { + return idx; + } } - return(0); + return 0; } // @@ -208,13 +218,12 @@ uint16_t sa_smartbaud = SMARTBAUD_MIN; static int sa_adjdir = 1; // -1=going down, 1=going up static int sa_baudstep = 50; -#define SMARTAUDIO_CMD_TIMEOUT 120 - static void saAutobaud(void) { - if (saStat.pktsent < 10) + if (saStat.pktsent < 10) { // Not enough samples collected return; + } #if 0 dprintf(("autobaud: %d rcvd %d/%d (%d)\r\n", @@ -250,7 +259,7 @@ static void saAutobaud(void) // Transport level variables -static uint32_t sa_lastTransmission = 0; +static timeUs_t sa_lastTransmissionMs = 0; static uint8_t sa_outstanding = SA_CMD_NONE; // Outstanding command static uint8_t sa_osbuf[32]; // Outstanding comamnd frame for retransmission static int sa_oslen; // And associate length @@ -268,11 +277,12 @@ static void saProcessResponse(uint8_t *buf, int len) dprintf(("processResponse: outstanding %d got %d\r\n", sa_outstanding, resp)); } - switch(resp) { + switch (resp) { case SA_CMD_GET_SETTINGS_V2: // Version 2 Get Settings case SA_CMD_GET_SETTINGS: // Version 1 Get Settings - if (len < 7) + if (len < 7) { break; + } saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2; saDevice.channel = buf[2]; @@ -280,12 +290,10 @@ static void saProcessResponse(uint8_t *buf, int len) saDevice.mode = buf[4]; saDevice.freq = (buf[5] << 8)|buf[6]; -#ifdef SMARTAUDIO_DEBUG_MONITOR - debug[0] = saDevice.version * 100 + saDevice.mode; - debug[1] = saDevice.channel; - debug[2] = saDevice.freq; - debug[3] = saDevice.power; -#endif + DEBUG_SET(DEBUG_SMARTAUDIO, 0, saDevice.version * 100 + saDevice.mode); + DEBUG_SET(DEBUG_SMARTAUDIO, 1, saDevice.channel); + DEBUG_SET(DEBUG_SMARTAUDIO, 2, saDevice.freq); + DEBUG_SET(DEBUG_SMARTAUDIO, 3, saDevice.power); break; case SA_CMD_SET_POWER: // Set Power @@ -295,10 +303,11 @@ static void saProcessResponse(uint8_t *buf, int len) break; case SA_CMD_SET_FREQ: // Set Frequency - if (len < 5) + if (len < 5) { break; + } - uint16_t freq = (buf[2] << 8)|buf[3]; + const uint16_t freq = (buf[2] << 8)|buf[3]; if (freq & SA_FREQ_GETPIT) { saDevice.orfreq = freq & SA_FREQ_MASK; @@ -321,12 +330,17 @@ static void saProcessResponse(uint8_t *buf, int len) return; } - // Debug - if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) + if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) { +#ifdef USE_CMS //if changes then trigger saCms update + saCmsResetOpmodel(); +#endif +#ifdef USE_SMARTAUDIO_DPRINTF // Debug saPrintSettings(); +#endif + } saDevicePrev = saDevice; -#ifdef VTX_COMMON +#ifdef USE_VTX_COMMON // Todo: Update states in saVtxDevice? #endif @@ -355,7 +369,7 @@ static void saReceiveFramer(uint8_t c) static int len; static int dlen; - switch(state) { + switch (state) { case S_WAITPRE1: if (c == 0xAA) { state = S_WAITPRE2; @@ -420,16 +434,20 @@ static void saReceiveFramer(uint8_t c) static void saSendFrame(uint8_t *buf, int len) { - int i; + switch (smartAudioSerialPort->identifier) { + case SERIAL_PORT_SOFTSERIAL1: + case SERIAL_PORT_SOFTSERIAL2: + break; + default: + serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit + break; + } - serialWrite(smartAudioSerialPort, 0x00); // Generate 1st start bit - - for (i = 0 ; i < len ; i++) + for (int i = 0 ; i < len ; i++) { serialWrite(smartAudioSerialPort, buf[i]); + } - serialWrite(smartAudioSerialPort, 0x00); // XXX Probably don't need this - - sa_lastTransmission = millis(); + sa_lastTransmissionMs = millis(); saStat.pktsent++; } @@ -460,10 +478,9 @@ static void saResendCmd(void) static void saSendCmd(uint8_t *buf, int len) { - int i; - - for (i = 0 ; i < len ; i++) + for (int i = 0 ; i < len ; i++) { sa_osbuf[i] = buf[i]; + } sa_oslen = len; sa_outstanding = (buf[2] >> 1); @@ -478,22 +495,11 @@ typedef struct saCmdQueue_s { int len; } saCmdQueue_t; -#define SA_QSIZE 4 // 1 heartbeat (GetSettings) + 2 commands + 1 slack +#define SA_QSIZE 6 // 1 heartbeat (GetSettings) + 2 commands + 1 slack static saCmdQueue_t sa_queue[SA_QSIZE]; static uint8_t sa_qhead = 0; static uint8_t sa_qtail = 0; -#ifdef DPRINTF_SMARTAUDIO -static int saQueueLength(void) -{ - if (sa_qhead >= sa_qtail) { - return sa_qhead - sa_qtail; - } else { - return SA_QSIZE + sa_qhead - sa_qtail; - } -} -#endif - static bool saQueueEmpty(void) { return sa_qhead == sa_qtail; @@ -506,8 +512,9 @@ static bool saQueueFull(void) static void saQueueCmd(uint8_t *buf, int len) { - if (saQueueFull()) + if (saQueueFull()) { return; + } sa_queue[sa_qhead].buf = buf; sa_queue[sa_qhead].len = len; @@ -516,8 +523,9 @@ static void saQueueCmd(uint8_t *buf, int len) static void saSendQueue(void) { - if (saQueueEmpty()) + if (saQueueEmpty()) { return; + } saSendCmd(sa_queue[sa_qtail].buf, sa_queue[sa_qtail].len); sa_qtail = (sa_qtail + 1) % SA_QSIZE; @@ -532,9 +540,15 @@ static void saGetSettings(void) saQueueCmd(bufGetSettings, 5); } -void saSetFreq(uint16_t freq) +static bool saValidateFreq(uint16_t freq) +{ + return (freq >= VTX_SMARTAUDIO_MIN_FREQUENCY_MHZ && freq <= VTX_SMARTAUDIO_MAX_FREQUENCY_MHZ); +} + +static void saDoDevSetFreq(uint16_t freq) { static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 }; + static uint8_t switchBuf[7]; if (freq & SA_FREQ_GETPIT) { dprintf(("smartAudioSetFreq: GETPIT\r\n")); @@ -548,31 +562,60 @@ void saSetFreq(uint16_t freq) buf[5] = freq & 0xff; buf[6] = CRC8(buf, 6); + // Need to work around apparent SmartAudio bug when going from 'channel' + // to 'user-freq' mode, where the set-freq command will fail if the freq + // value is unchanged from the previous 'user-freq' mode + if ((saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) == 0 && freq == saDevice.freq) { + memcpy(&switchBuf, &buf, sizeof(buf)); + const uint16_t switchFreq = freq + ((freq == VTX_SMARTAUDIO_MAX_FREQUENCY_MHZ) ? -1 : 1); + switchBuf[4] = (switchFreq >> 8); + switchBuf[5] = switchFreq & 0xff; + switchBuf[6] = CRC8(switchBuf, 6); + + saQueueCmd(switchBuf, 7); + } + saQueueCmd(buf, 7); } -#if 0 -static void saSetPitFreq(uint16_t freq) +void saSetFreq(uint16_t freq) { - saSetFreq(freq | SA_FREQ_SETPIT); + saDoDevSetFreq(freq); } +void saSetPitFreq(uint16_t freq) +{ + saDoDevSetFreq(freq | SA_FREQ_SETPIT); +} + +#if 0 static void saGetPitFreq(void) { - saSetFreq(SA_FREQ_GETPIT); + saDoDevSetFreq(SA_FREQ_GETPIT); } #endif -void saSetBandAndChannel(uint8_t band, uint8_t channel) +static bool saValidateBandAndChannel(uint8_t band, uint8_t channel) +{ + return (band >= VTX_SMARTAUDIO_MIN_BAND && band <= VTX_SMARTAUDIO_MAX_BAND && + channel >= VTX_SMARTAUDIO_MIN_CHANNEL && channel <= VTX_SMARTAUDIO_MAX_CHANNEL); +} + +static void saDevSetBandAndChannel(uint8_t band, uint8_t channel) { static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_CHAN), 1 }; - buf[4] = band * 8 + channel; + buf[4] = SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel); buf[5] = CRC8(buf, 5); saQueueCmd(buf, 6); } +void saSetBandAndChannel(uint8_t band, uint8_t channel) +{ + saDevSetBandAndChannel(band, channel); +} + void saSetMode(int mode) { static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 }; @@ -583,7 +626,7 @@ void saSetMode(int mode) saQueueCmd(buf, 6); } -void saSetPowerByIndex(uint8_t index) +static void saDevSetPowerByIndex(uint8_t index) { static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 }; @@ -594,17 +637,23 @@ void saSetPowerByIndex(uint8_t index) return; } - if (index > 3) + if (index >= VTX_SMARTAUDIO_POWER_COUNT) { return; + } buf[4] = (saDevice.version == 1) ? saPowerTable[index].valueV1 : saPowerTable[index].valueV2; buf[5] = CRC8(buf, 5); saQueueCmd(buf, 6); } +void saSetPowerByIndex(uint8_t index) +{ + saDevSetPowerByIndex(index); +} + bool vtxSmartAudioInit(void) { -#ifdef SMARTAUDIO_DPRINTF +#ifdef USE_SMARTAUDIO_DPRINTF // Setup debugSerialPort debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, NULL, 115200, MODE_RXTX, 0); @@ -616,24 +665,40 @@ bool vtxSmartAudioInit(void) serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO); if (portConfig) { - smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, SERIAL_BIDIR|SERIAL_BIDIR_PP); + portOptions_t portOptions = SERIAL_STOPBITS_2 | SERIAL_BIDIR_NOPULL; +#if defined(USE_VTX_COMMON) + portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP : SERIAL_UNIDIR); +#else + portOptions = SERIAL_BIDIR; +#endif + + smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, portOptions); } if (!smartAudioSerialPort) { return false; } - vtxCommonRegisterDevice(&vtxSmartAudio); + vtxCommonSetDevice(&vtxSmartAudio); return true; } -void vtxSAProcess(uint32_t now) -{ - static bool initialSent = false; +#define SA_INITPHASE_START 0 +#define SA_INITPHASE_WAIT_SETTINGS 1 // SA_CMD_GET_SETTINGS was sent and waiting for reply. +#define SA_INITPHASE_WAIT_PITFREQ 2 // SA_FREQ_GETPIT sent and waiting for reply. +#define SA_INITPHASE_DONE 3 - if (smartAudioSerialPort == NULL) +static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) +{ + UNUSED(vtxDevice); + UNUSED(currentTimeUs); + + static char initPhase = SA_INITPHASE_START; + + if (smartAudioSerialPort == NULL) { return; + } while (serialRxBytesWaiting(smartAudioSerialPort) > 0) { uint8_t c = serialRead(smartAudioSerialPort); @@ -643,72 +708,84 @@ void vtxSAProcess(uint32_t now) // Re-evaluate baudrate after each frame reception saAutobaud(); - if (!initialSent) { + switch (initPhase) { + case SA_INITPHASE_START: saGetSettings(); - saSetFreq(SA_FREQ_GETPIT); - saSendQueue(); - initialSent = true; - return; + //saSendQueue(); + initPhase = SA_INITPHASE_WAIT_SETTINGS; + break; + + case SA_INITPHASE_WAIT_SETTINGS: + // Don't send SA_FREQ_GETPIT to V1 device; it act as plain SA_CMD_SET_FREQ, + // and put the device into user frequency mode with uninitialized freq. + if (saDevice.version) { + if (saDevice.version == 2) { + saDoDevSetFreq(SA_FREQ_GETPIT); + initPhase = SA_INITPHASE_WAIT_PITFREQ; + } else { + initPhase = SA_INITPHASE_DONE; + } + } + break; + + case SA_INITPHASE_WAIT_PITFREQ: + if (saDevice.orfreq) { + initPhase = SA_INITPHASE_DONE; + } + break; + + case SA_INITPHASE_DONE: + break; } - if ((sa_outstanding != SA_CMD_NONE) - && (now - sa_lastTransmission > SMARTAUDIO_CMD_TIMEOUT)) { + // Command queue control + + timeMs_t nowMs = millis(); // Don't substitute with "currentTimeUs / 1000"; sa_lastTransmissionMs is based on millis(). + static timeMs_t lastCommandSentMs = 0; // Last non-GET_SETTINGS sent + + if ((sa_outstanding != SA_CMD_NONE) && (nowMs - sa_lastTransmissionMs > SMARTAUDIO_CMD_TIMEOUT)) { // Last command timed out // dprintf(("process: resending 0x%x\r\n", sa_outstanding)); // XXX Todo: Resend termination and possible offline transition saResendCmd(); + lastCommandSentMs = nowMs; } else if (!saQueueEmpty()) { // Command pending. Send it. // dprintf(("process: sending queue\r\n")); saSendQueue(); - } else if (now - sa_lastTransmission >= 1000) { - // Heart beat for autobauding - //dprintf(("process: sending heartbeat\r\n")); - saGetSettings(); - saSendQueue(); + lastCommandSentMs = nowMs; + } else if ((nowMs - lastCommandSentMs < SMARTAUDIO_POLLING_WINDOW) && (nowMs - sa_lastTransmissionMs >= SMARTAUDIO_POLLING_INTERVAL)) { + //dprintf(("process: sending status change polling\r\n")); + saGetSettings(); + saSendQueue(); } - -#ifdef SMARTAUDIO_TEST_VTX_COMMON - // Testing VTX_COMMON API - { - static uint32_t lastMonitorUs = 0; - if (cmp32(now, lastMonitorUs) < 5 * 1000 * 1000) - return; - - static uint8_t monBand; - static uint8_t monChan; - static uint8_t monPower; - - vtxCommonGetBandAndChannel(&monBand, &monChan); - vtxCommonGetPowerIndex(&monPower); - debug[0] = monBand; - debug[1] = monChan; - debug[2] = monPower; - } -#endif } -#ifdef VTX_COMMON +#ifdef USE_VTX_COMMON // Interface to common VTX API -vtxDevType_e vtxSAGetDeviceType(void) +vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice) { + UNUSED(vtxDevice); return VTXDEV_SMARTAUDIO; } -bool vtxSAIsReady(void) +static bool vtxSAIsReady(const vtxDevice_t *vtxDevice) { - return !(saDevice.version == 0); + return vtxDevice!=NULL && !(saDevice.version == 0); } -void vtxSASetBandAndChannel(uint8_t band, uint8_t channel) +static void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { - if (band && channel) + UNUSED(vtxDevice); + if (saValidateBandAndChannel(band, channel)) { saSetBandAndChannel(band - 1, channel - 1); + } } -void vtxSASetPowerByIndex(uint8_t index) +static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { + UNUSED(vtxDevice); if (index == 0) { // SmartAudio doesn't support power off. return; @@ -717,10 +794,11 @@ void vtxSASetPowerByIndex(uint8_t index) saSetPowerByIndex(index - 1); } -void vtxSASetPitMode(uint8_t onoff) +static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) { - if (!(vtxSAIsReady() && (saDevice.version == 2))) + if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) { return; + } if (onoff) { // SmartAudio can not turn pit mode on by software. @@ -729,45 +807,74 @@ void vtxSASetPitMode(uint8_t onoff) uint8_t newmode = SA_MODE_CLR_PITMODE; - if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) + if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) { newmode |= SA_MODE_SET_IN_RANGE_PITMODE; + } - if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) + if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) { newmode |= SA_MODE_SET_OUT_RANGE_PITMODE; + } saSetMode(newmode); return; } -bool vtxSAGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) +static void vtxSASetFreq(vtxDevice_t *vtxDevice, uint16_t freq) { - if (!vtxSAIsReady()) - return false; + UNUSED(vtxDevice); + if (saValidateFreq(freq)) { + saSetMode(0); //need to be in FREE mode to set freq + saSetFreq(freq); + } +} - *pBand = (saDevice.channel / 8) + 1; - *pChannel = (saDevice.channel % 8) + 1; +static bool vtxSAGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) +{ + if (!vtxSAIsReady(vtxDevice)) { + return false; + } + + // if in user-freq mode then report band as zero + *pBand = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 0 : + (SA_DEVICE_CHVAL_TO_BAND(saDevice.channel) + 1); + *pChannel = SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel) + 1; return true; } -bool vtxSAGetPowerIndex(uint8_t *pIndex) +static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) { - if (!vtxSAIsReady()) + if (!vtxSAIsReady(vtxDevice)) { return false; + } *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1; return true; } -bool vtxSAGetPitMode(uint8_t *pOnOff) +static bool vtxSAGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) { - if (!(vtxSAIsReady() && (saDevice.version == 2))) + if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) { return false; + } *pOnOff = (saDevice.mode & SA_MODE_GET_PITMODE) ? 1 : 0; return true; } +static bool vtxSAGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) +{ + if (!vtxSAIsReady(vtxDevice)) { + return false; + } + + // if not in user-freq mode then convert band/chan to frequency + *pFreq = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? saDevice.freq : + vtx58_Bandchan2Freq(SA_DEVICE_CHVAL_TO_BAND(saDevice.channel) + 1, + SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel) + 1); + return true; +} + static const vtxVTable_t saVTable = { .process = vtxSAProcess, .getDeviceType = vtxSAGetDeviceType, @@ -775,9 +882,11 @@ static const vtxVTable_t saVTable = { .setBandAndChannel = vtxSASetBandAndChannel, .setPowerByIndex = vtxSASetPowerByIndex, .setPitMode = vtxSASetPitMode, + .setFrequency = vtxSASetFreq, .getBandAndChannel = vtxSAGetBandAndChannel, .getPowerIndex = vtxSAGetPowerIndex, .getPitMode = vtxSAGetPitMode, + .getFrequency = vtxSAGetFreq, }; #endif // VTX_COMMON diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index 149aaa1b4a..a0ad9ee504 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -1,18 +1,21 @@ /* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * 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 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 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. + * 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 Cleanflight. If not, see . + * along with this software. + * + * If not, see . */ #pragma once @@ -22,6 +25,19 @@ #include "platform.h" +#define VTX_SMARTAUDIO_MIN_BAND 1 +#define VTX_SMARTAUDIO_MAX_BAND 5 +#define VTX_SMARTAUDIO_MIN_CHANNEL 1 +#define VTX_SMARTAUDIO_MAX_CHANNEL 8 + +#define VTX_SMARTAUDIO_BAND_COUNT (VTX_SMARTAUDIO_MAX_BAND - VTX_SMARTAUDIO_MIN_BAND + 1) +#define VTX_SMARTAUDIO_CHANNEL_COUNT (VTX_SMARTAUDIO_MAX_CHANNEL - VTX_SMARTAUDIO_MIN_CHANNEL + 1) + +#define VTX_SMARTAUDIO_POWER_COUNT 4 +#define VTX_SMARTAUDIO_DEFAULT_POWER 1 + +#define VTX_SMARTAUDIO_MIN_FREQUENCY_MHZ 5000 //min freq in MHz +#define VTX_SMARTAUDIO_MAX_FREQUENCY_MHZ 5999 //max freq in MHz // opmode flags, GET side #define SA_MODE_GET_FREQ_BY_FREQ 1 @@ -73,9 +89,11 @@ typedef struct smartAudioStat_s { extern smartAudioDevice_t saDevice; extern saPowerTable_t saPowerTable[]; +extern const char * const saPowerNames[]; extern smartAudioStat_t saStat; -extern bool saDeferred; + extern uint16_t sa_smartbaud; +extern bool saDeferred; int saDacToPowerIndex(int dac); void saSetBandAndChannel(uint8_t band, uint8_t channel); @@ -85,17 +103,9 @@ void saSetFreq(uint16_t freq); void saSetPitFreq(uint16_t freq); bool vtxSmartAudioInit(void); -#ifdef SMARTAUDIO_DPRINTF -#ifdef OMNIBUSF4 -#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3 -#else -#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART1 -#endif +#ifdef USE_SMARTAUDIO_DPRINTF extern serialPort_t *debugSerialPort; #define dprintf(x) if (debugSerialPort) printf x #else #define dprintf(x) -#endif // SMARTAUDIO_DPRINTF - -int saDacToPowerIndex(int dac); -bool vtxSmartAudioInit(void); +#endif // USE_SMARTAUDIO_DPRINTF diff --git a/src/main/io/vtx_string.c b/src/main/io/vtx_string.c index 71f0e4047a..bbaa3eb7b1 100644 --- a/src/main/io/vtx_string.c +++ b/src/main/io/vtx_string.c @@ -25,9 +25,12 @@ #include "platform.h" #include "build/debug.h" -#if defined(VTX_COMMON) +#if defined(USE_VTX_COMMON) -const uint16_t vtx58frequencyTable[5][8] = +#define VTX_STRING_BAND_COUNT 5 +#define VTX_STRING_CHAN_COUNT 8 + +const uint16_t vtx58frequencyTable[VTX_STRING_BAND_COUNT][VTX_STRING_CHAN_COUNT] = { { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, // Boscam A { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, // Boscam B @@ -36,7 +39,7 @@ const uint16_t vtx58frequencyTable[5][8] = { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, // RaceBand }; -const char * const vtx58BandNames[] = { +const char * const vtx58BandNames[VTX_STRING_BAND_COUNT + 1] = { "--------", "BOSCAM A", "BOSCAM B", @@ -45,9 +48,9 @@ const char * const vtx58BandNames[] = { "RACEBAND", }; -const char vtx58BandLetter[] = "-ABEFR"; +const char vtx58BandLetter[VTX_STRING_BAND_COUNT + 1] = "-ABEFR"; -const char * const vtx58ChannelNames[] = { +const char * const vtx58ChannelNames[VTX_STRING_CHAN_COUNT + 1] = { "-", "1", "2", "3", "4", "5", "6", "7", "8", }; @@ -74,4 +77,17 @@ bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel) return false; } +//Converts band and channel values to a frequency (in MHz) value. +// band: Band value (1 to 5). +// channel: Channel value (1 to 8). +// Returns frequency value (in MHz), or 0 if band/channel out of range. +uint16_t vtx58_Bandchan2Freq(uint8_t band, uint8_t channel) +{ + if (band > 0 && band <= VTX_STRING_BAND_COUNT && + channel > 0 && channel <= VTX_STRING_CHAN_COUNT) { + return vtx58frequencyTable[band - 1][channel - 1]; + } + return 0; +} + #endif diff --git a/src/main/io/vtx_string.h b/src/main/io/vtx_string.h index cd9c40fb41..f7a5618411 100644 --- a/src/main/io/vtx_string.h +++ b/src/main/io/vtx_string.h @@ -8,3 +8,4 @@ extern const char * const vtx58ChannelNames[]; extern const char vtx58BandLetter[]; bool vtx58_Freq2Bandchan(uint16_t freq, uint8_t *pBand, uint8_t *pChannel); +uint16_t vtx58_Bandchan2Freq(uint8_t band, uint8_t channel); diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index f7235be0ee..e6a93eb462 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -1,61 +1,66 @@ /* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * 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 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 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. + * 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 Cleanflight. If not, see . + * along with this software. + * + * If not, see . */ /* Created by jflyper */ #include #include +#include #include #include "platform.h" -#if defined(VTX_TRAMP) && defined(VTX_CONTROL) +#if defined(USE_VTX_TRAMP) && defined(USE_VTX_CONTROL) #include "build/debug.h" +#include "common/maths.h" #include "common/utils.h" +#include "cms/cms_menu_vtx_tramp.h" + #include "drivers/vtx_common.h" #include "io/serial.h" #include "io/vtx_tramp.h" +#include "io/vtx_control.h" +#include "io/vtx.h" #include "io/vtx_string.h" -#define TRAMP_SERIAL_OPTIONS (SERIAL_BIDIR) - -#if defined(USE_CMS) || defined(VTX_COMMON) -const uint16_t trampPowerTable[] = { +#if defined(USE_CMS) || defined(USE_VTX_COMMON) +const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = { 25, 100, 200, 400, 600 }; -const char * const trampPowerNames[] = { +const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1] = { "---", "25 ", "100", "200", "400", "600" }; #endif -#if defined(VTX_COMMON) +#if defined(USE_VTX_COMMON) static const vtxVTable_t trampVTable; // forward static vtxDevice_t vtxTramp = { .vTable = &trampVTable, - .capability = { - .bandCount = 5, - .channelCount = 8, - .powerCount = sizeof(trampPowerTable), - }, + .capability.bandCount = VTX_TRAMP_BAND_COUNT, + .capability.channelCount = VTX_TRAMP_CHANNEL_COUNT, + .capability.powerCount = sizeof(trampPowerTable), .bandNames = (char **)vtx58BandNames, .channelNames = (char **)vtx58ChannelNames, .powerNames = (char **)trampPowerNames, @@ -76,38 +81,42 @@ typedef enum { trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE; -// TODO: These fields are currently removed by the compiler because they're -// never read. Decide if we want to use them for something or remove them. -static uint16_t trampRFFreqMin; -static uint16_t trampRFFreqMax; -static uint16_t trampRFPowerMax; - +uint32_t trampRFFreqMin; +uint32_t trampRFFreqMax; +uint32_t trampRFPowerMax; trampData_t trampData; // Maximum number of requests sent to try a config change #define TRAMP_MAX_RETRIES 2 -uint16_t trampConfFreq = 0; +uint32_t trampConfFreq = 0; uint8_t trampFreqRetries = 0; uint16_t trampConfPower = 0; uint8_t trampPowerRetries = 0; +static void trampWriteBuf(uint8_t *buf) +{ + serialWriteBuf(trampSerialPort, buf, 16); +} + static uint8_t trampChecksum(uint8_t *trampBuf) { uint8_t cksum = 0; - for (int i = 1 ; i < 14 ; i++) + for (int i = 1 ; i < 14 ; i++) { cksum += trampBuf[i]; + } return cksum; } void trampCmdU16(uint8_t cmd, uint16_t param) { - if (!trampSerialPort) + if (!trampSerialPort) { return; + } uint8_t trampReqBuffer[16]; memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer)); @@ -116,14 +125,26 @@ void trampCmdU16(uint8_t cmd, uint16_t param) trampReqBuffer[2] = param & 0xff; trampReqBuffer[3] = (param >> 8) & 0xff; trampReqBuffer[14] = trampChecksum(trampReqBuffer); - serialWriteBuf(trampSerialPort, trampReqBuffer, sizeof(trampReqBuffer)); + trampWriteBuf(trampReqBuffer); +} + +static bool trampValidateFreq(uint16_t freq) +{ + return (freq >= VTX_TRAMP_MIN_FREQUENCY_MHZ && freq <= VTX_TRAMP_MAX_FREQUENCY_MHZ); +} + +static void trampDevSetFreq(uint16_t freq) +{ + trampConfFreq = freq; + if (trampConfFreq != trampData.curFreq) { + trampFreqRetries = TRAMP_MAX_RETRIES; + } } void trampSetFreq(uint16_t freq) { - trampConfFreq = freq; - if(trampConfFreq != trampData.curFreq) - trampFreqRetries = TRAMP_MAX_RETRIES; + trampData.setByFreqFlag = true; //set freq via MHz value + trampDevSetFreq(freq); } void trampSendFreq(uint16_t freq) @@ -131,16 +152,29 @@ void trampSendFreq(uint16_t freq) trampCmdU16('F', freq); } +static bool trampValidateBandAndChannel(uint8_t band, uint8_t channel) +{ + return (band >= VTX_TRAMP_MIN_BAND && band <= VTX_TRAMP_MAX_BAND && + channel >= VTX_TRAMP_MIN_CHANNEL && channel <= VTX_TRAMP_MAX_CHANNEL); +} + +static void trampDevSetBandAndChannel(uint8_t band, uint8_t channel) +{ + trampDevSetFreq(vtx58_Bandchan2Freq(band, channel)); +} + void trampSetBandAndChannel(uint8_t band, uint8_t channel) { - trampSetFreq(vtx58frequencyTable[band - 1][channel - 1]); + trampData.setByFreqFlag = false; //set freq via band/channel + trampDevSetBandAndChannel(band, channel); } void trampSetRFPower(uint16_t level) { trampConfPower = level; - if(trampConfPower != trampData.power) + if (trampConfPower != trampData.power) { trampPowerRetries = TRAMP_MAX_RETRIES; + } } void trampSendRFPower(uint16_t level) @@ -148,21 +182,28 @@ void trampSendRFPower(uint16_t level) trampCmdU16('P', level); } -bool trampIsAvailable(void) -{ - return trampStatus != TRAMP_STATUS_BAD_DEVICE && trampStatus != TRAMP_STATUS_OFFLINE; -} - // return false if error bool trampCommitChanges(void) { - if(trampStatus != TRAMP_STATUS_ONLINE) + if (trampStatus != TRAMP_STATUS_ONLINE) { return false; + } trampStatus = TRAMP_STATUS_SET_FREQ_PW; return true; } +// return false if index out of range +static bool trampDevSetPowerByIndex(uint8_t index) +{ + if (index > 0 && index <= sizeof(trampPowerTable)) { + trampSetRFPower(trampPowerTable[index - 1]); + trampCommitChanges(); + return true; + } + return false; +} + void trampSetPitMode(uint8_t onoff) { trampCmdU16('I', onoff ? 0 : 1); @@ -171,13 +212,13 @@ void trampSetPitMode(uint8_t onoff) // returns completed response code char trampHandleResponse(void) { - uint8_t respCode = trampRespBuffer[1]; + const uint8_t respCode = trampRespBuffer[1]; switch (respCode) { case 'r': { - uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); - if(min_freq != 0) { + const uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + if (min_freq != 0) { trampRFFreqMin = min_freq; trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8); trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8); @@ -190,16 +231,20 @@ char trampHandleResponse(void) case 'v': { - uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); - if(freq != 0) { + const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8); + if (freq != 0) { trampData.curFreq = freq; trampData.configuredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8); trampData.pitMode = trampRespBuffer[7]; trampData.power = trampRespBuffer[8]|(trampRespBuffer[9] << 8); - vtx58_Freq2Bandchan(trampData.curFreq, &trampData.band, &trampData.channel); - if(trampConfFreq == 0) trampConfFreq = trampData.curFreq; - if(trampConfPower == 0) trampConfPower = trampData.power; + // if no band/chan match then make sure set-by-freq mode is flagged + if (!vtx58_Freq2Bandchan(trampData.curFreq, &trampData.band, &trampData.channel)) { + trampData.setByFreqFlag = true; + } + + if (trampConfFreq == 0) trampConfFreq = trampData.curFreq; + if (trampConfPower == 0) trampConfPower = trampData.power; return 'v'; } @@ -209,8 +254,8 @@ char trampHandleResponse(void) case 's': { - uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); - if(temp != 0) { + const uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8)); + if (temp != 0) { trampData.temperature = temp; return 's'; } @@ -238,10 +283,11 @@ static void trampResetReceiver(void) static bool trampIsValidResponseCode(uint8_t code) { - if (code == 'r' || code == 'v' || code == 's') + if (code == 'r' || code == 'v' || code == 's') { return true; - else + } else { return false; + } } // returns completed response code or 0 @@ -249,14 +295,15 @@ static char trampReceive(uint32_t currentTimeUs) { UNUSED(currentTimeUs); - if (!trampSerialPort) + if (!trampSerialPort) { return 0; + } while (serialRxBytesWaiting(trampSerialPort)) { - uint8_t c = serialRead(trampSerialPort); + const uint8_t c = serialRead(trampSerialPort); trampRespBuffer[trampReceivePos++] = c; - switch(trampReceiveState) { + switch (trampReceiveState) { case S_WAIT_LEN: if (c == 0x0F) { trampReceiveState = S_WAIT_CODE; @@ -287,6 +334,7 @@ static char trampReceive(uint32_t currentTimeUs) default: trampResetReceiver(); + break; } } @@ -314,50 +362,63 @@ void trampQueryS(void) trampQuery('s'); } -void vtxTrampProcess(uint32_t currentTimeUs) +static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) { - static uint32_t lastQueryTimeUs = 0; + UNUSED(vtxDevice); + + static timeUs_t lastQueryTimeUs = 0; + static bool initSettingsDoneFlag = false; #ifdef TRAMP_DEBUG static uint16_t debugFreqReqCounter = 0; static uint16_t debugPowReqCounter = 0; #endif - if (trampStatus == TRAMP_STATUS_BAD_DEVICE) + if (trampStatus == TRAMP_STATUS_BAD_DEVICE) { return; + } - char replyCode = trampReceive(currentTimeUs); + const char replyCode = trampReceive(currentTimeUs); #ifdef TRAMP_DEBUG debug[0] = trampStatus; #endif - switch(replyCode) { + switch (replyCode) { case 'r': - if (trampStatus <= TRAMP_STATUS_OFFLINE) + if (trampStatus <= TRAMP_STATUS_OFFLINE) { trampStatus = TRAMP_STATUS_ONLINE; + + // once device is ready enter vtx settings + if (!initSettingsDoneFlag) { + initSettingsDoneFlag = true; + // if vtx_band!=0 then enter 'vtx_band/chan' values (and power) + } + } break; case 'v': - if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) + if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) { trampStatus = TRAMP_STATUS_SET_FREQ_PW; + } break; } - switch(trampStatus) { + switch (trampStatus) { case TRAMP_STATUS_OFFLINE: case TRAMP_STATUS_ONLINE: if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s - if (trampStatus == TRAMP_STATUS_OFFLINE) + if (trampStatus == TRAMP_STATUS_OFFLINE) { trampQueryR(); - else { + } else { static unsigned int cnt = 0; - if(((cnt++) & 1) == 0) + if (((cnt++) & 1) == 0) { trampQueryV(); - else + } else { trampQueryS(); + } } lastQueryTimeUs = currentTimeUs; @@ -374,8 +435,7 @@ void vtxTrampProcess(uint32_t currentTimeUs) debugFreqReqCounter++; #endif done = false; - } - else if (trampConfPower && trampPowerRetries && (trampConfPower != trampData.configuredPower)) { + } else if (trampConfPower && trampPowerRetries && (trampConfPower != trampData.configuredPower)) { trampSendRFPower(trampConfPower); trampPowerRetries--; #ifdef TRAMP_DEBUG @@ -384,13 +444,12 @@ void vtxTrampProcess(uint32_t currentTimeUs) done = false; } - if(!done) { + if (!done) { trampStatus = TRAMP_STATUS_CHECK_FREQ_PW; // delay next status query by 300ms lastQueryTimeUs = currentTimeUs + 300 * 1000; - } - else { + } else { // everything has been done, let's return to original state trampStatus = TRAMP_STATUS_ONLINE; // reset configuration value in case it failed (no more retries) @@ -420,55 +479,68 @@ void vtxTrampProcess(uint32_t currentTimeUs) } -#ifdef VTX_COMMON +#ifdef USE_VTX_COMMON // Interface to common VTX API -vtxDevType_e vtxTrampGetDeviceType(void) +static vtxDevType_e vtxTrampGetDeviceType(const vtxDevice_t *vtxDevice) { + UNUSED(vtxDevice); return VTXDEV_TRAMP; } -bool vtxTrampIsReady(void) +static bool vtxTrampIsReady(const vtxDevice_t *vtxDevice) { - return trampStatus > TRAMP_STATUS_OFFLINE; + return vtxDevice!=NULL && trampStatus > TRAMP_STATUS_OFFLINE; } -void vtxTrampSetBandAndChannel(uint8_t band, uint8_t channel) +static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { - if (band && channel) { + UNUSED(vtxDevice); + if (trampValidateBandAndChannel(band, channel)) { trampSetBandAndChannel(band, channel); trampCommitChanges(); } } -void vtxTrampSetPowerByIndex(uint8_t index) +static void vtxTrampSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { - if (index) { - trampSetRFPower(trampPowerTable[index - 1]); + UNUSED(vtxDevice); + trampDevSetPowerByIndex(index); +} + +static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) +{ + UNUSED(vtxDevice); + trampSetPitMode(onoff); +} + +static void vtxTrampSetFreq(vtxDevice_t *vtxDevice, uint16_t freq) +{ + UNUSED(vtxDevice); + if (trampValidateFreq(freq)) { + trampSetFreq(freq); trampCommitChanges(); } } -void vtxTrampSetPitMode(uint8_t onoff) +static bool vtxTrampGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) { - trampSetPitMode(onoff); -} - -bool vtxTrampGetBandAndChannel(uint8_t *pBand, uint8_t *pChannel) -{ - if (!vtxTrampIsReady()) + if (!vtxTrampIsReady(vtxDevice)) { return false; + } - *pBand = trampData.band; + // if in user-freq mode then report band as zero + *pBand = trampData.setByFreqFlag ? 0 : trampData.band; *pChannel = trampData.channel; return true; } -bool vtxTrampGetPowerIndex(uint8_t *pIndex) +static bool vtxTrampGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) { - if (!vtxTrampIsReady()) + if (!vtxTrampIsReady(vtxDevice)) { return false; + } if (trampData.configuredPower > 0) { for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) { @@ -482,15 +554,26 @@ bool vtxTrampGetPowerIndex(uint8_t *pIndex) return true; } -bool vtxTrampGetPitMode(uint8_t *pOnOff) +static bool vtxTrampGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) { - if (!vtxTrampIsReady()) + if (!vtxTrampIsReady(vtxDevice)) { return false; + } *pOnOff = trampData.pitMode; return true; } +static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) +{ + if (!vtxTrampIsReady(vtxDevice)) { + return false; + } + + *pFreq = trampData.curFreq; + return true; +} + static const vtxVTable_t trampVTable = { .process = vtxTrampProcess, .getDeviceType = vtxTrampGetDeviceType, @@ -498,9 +581,11 @@ static const vtxVTable_t trampVTable = { .setBandAndChannel = vtxTrampSetBandAndChannel, .setPowerByIndex = vtxTrampSetPowerByIndex, .setPitMode = vtxTrampSetPitMode, + .setFrequency = vtxTrampSetFreq, .getBandAndChannel = vtxTrampGetBandAndChannel, .getPowerIndex = vtxTrampGetPowerIndex, .getPitMode = vtxTrampGetPitMode, + .getFrequency = vtxTrampGetFreq, }; #endif @@ -510,17 +595,24 @@ bool vtxTrampInit(void) serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_TRAMP); if (portConfig) { - trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, TRAMP_SERIAL_OPTIONS); + portOptions_t portOptions = 0; +#if defined(USE_VTX_COMMON) + portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR); +#else + portOptions = SERIAL_BIDIR; +#endif + + trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions); } if (!trampSerialPort) { return false; } -#if defined(VTX_COMMON) - vtxCommonRegisterDevice(&vtxTramp); +#if defined(USE_VTX_COMMON) + vtxCommonSetDevice(&vtxTramp); #endif - + return true; } diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h index 48e7a0e928..a9c7cd04b2 100644 --- a/src/main/io/vtx_tramp.h +++ b/src/main/io/vtx_tramp.h @@ -37,6 +37,7 @@ extern const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT]; extern const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1]; typedef struct trampData_s { + bool setByFreqFlag; //false = set via band/channel uint8_t band; uint8_t channel; uint16_t power; // Actual transmitting power @@ -49,7 +50,6 @@ typedef struct trampData_s { extern trampData_t trampData; bool vtxTrampInit(void); -bool trampIsAvailable(void); bool trampCommitChanges(void); void trampSetPitMode(uint8_t onoff); void trampSetBandAndChannel(uint8_t band, uint8_t channel); diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 29340f9bf1..15c200f26a 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -41,3 +41,5 @@ #define MSP2_INAV_OSD_SET_ALARMS 0x2015 #define MSP2_INAV_OSD_PREFERENCES 0x2016 #define MSP2_INAV_OSD_SET_PREFERENCES 0x2017 + +#define MSP2_INAV_SELECT_BATTERY_PROFILE 0x2018 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 30383f0218..28c28cf2ee 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -41,6 +41,7 @@ #include "fc/runtime_config.h" #include "flight/imu.h" +#include "flight/mixer.h" #include "flight/pid.h" #include "io/beeper.h" @@ -107,6 +108,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .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 }, @@ -132,6 +134,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .max_climb_angle = 20, .max_dive_angle = 15, .cruise_throttle = 1400, + .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) @@ -152,7 +155,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .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 + .launch_max_angle = 45, // 45 deg + .cruise_yaw_rate = 20, // 20dps + .allow_manual_thr_increase = false } ); @@ -181,11 +186,11 @@ static void setupAltitudeController(void); static void resetHeadingController(void); void resetGCSFlags(void); -static bool posEstimationHasGlobalReference(void); static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint); 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); void initializeRTHSanityChecker(const fpVector3_t * pos); bool validateRTHSanityChecker(void); @@ -196,6 +201,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState); @@ -235,6 +246,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_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, } }, @@ -269,6 +282,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_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, } }, @@ -303,6 +318,112 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_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, + } + }, + /** CRUISE_2D mode ************************************************/ + [NAV_STATE_CRUISE_2D_INITIALIZE] = { + .persistentId = NAV_PERSISTENT_ID_CRUISE_2D_INITIALIZE, + .onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE, + .timeoutMs = 0, + .stateFlags = NAV_REQUIRE_ANGLE, + .mapToFlightModes = NAV_CRUISE_MODE, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_2D_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + } + }, + + [NAV_STATE_CRUISE_2D_IN_PROGRESS] = { + .persistentId = NAV_PERSISTENT_ID_CRUISE_2D_IN_PROGRESS, + .onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS, + .timeoutMs = 10, + .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW, + .mapToFlightModes = NAV_CRUISE_MODE, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_2D_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ] = NAV_STATE_CRUISE_2D_ADJUSTING, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + } + }, + + [NAV_STATE_CRUISE_2D_ADJUSTING] = { + .persistentId = NAV_PERSISTENT_ID_CRUISE_2D_ADJUSTING, + .onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING, + .timeoutMs = 10, + .stateFlags = NAV_REQUIRE_ANGLE | NAV_RC_POS, + .mapToFlightModes = NAV_CRUISE_MODE, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_2D_IN_PROGRESS, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_2D_ADJUSTING, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + } + }, + + /** CRUISE_3D mode ************************************************/ + [NAV_STATE_CRUISE_3D_INITIALIZE] = { + .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE, + .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE, + .timeoutMs = 0, + .stateFlags = NAV_REQUIRE_ANGLE, + .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_3D_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + } + }, + [NAV_STATE_CRUISE_3D_IN_PROGRESS] = { + .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS, + .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS, + .timeoutMs = 10, + .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT, + .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_3D_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ] = NAV_STATE_CRUISE_3D_ADJUSTING, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + } + }, + + [NAV_STATE_CRUISE_3D_ADJUSTING] = { + .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING, + .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING, + .timeoutMs = 10, + .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_ALT, + .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_3D_IN_PROGRESS, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_3D_ADJUSTING, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -338,6 +459,8 @@ 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_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, } }, @@ -356,6 +479,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [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, } }, @@ -375,6 +500,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [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, } }, @@ -392,6 +519,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [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, } }, @@ -493,6 +622,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_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, } }, @@ -514,6 +645,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_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, } }, @@ -533,6 +666,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_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, } }, @@ -564,6 +699,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_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, } }, @@ -748,7 +885,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset } - if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) { + if ((previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING) && (previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME) && (previousState != NAV_STATE_RTH_LANDING)) { fpVector3_t targetHoldPos; calculateInitialHoldPosition(&targetHoldPos); setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); @@ -772,12 +909,117 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS( return NAV_FSM_EVENT_NONE; } +///////////////// + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(navigationFSMState_t previousState) +{ + const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); + + if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now + + DEBUG_SET(DEBUG_CRUISE, 0, 1); + if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Try the next iteration. + + if (!(prevFlags & NAV_CTL_POS)) { + resetPositionController(); + } + + posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow + posControl.cruise.previousYaw = posControl.cruise.yaw; + posControl.cruise.lastYawAdjustmentTime = 0; + + return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState) +{ + const timeMs_t currentTimeMs = millis(); + + if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration. + + DEBUG_SET(DEBUG_CRUISE, 0, 2); + DEBUG_SET(DEBUG_CRUISE, 2, 0); + + if (posControl.flags.isAdjustingPosition) { + return NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ; + } + + // User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile + if (posControl.flags.isAdjustingHeading) { + timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime; + if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference. + float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate)); + float centidegsPerIteration = rateTarget * timeDifference / 1000.0f; + posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration); + DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw)); + posControl.cruise.lastYawAdjustmentTime = currentTimeMs; + } + + if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000) + posControl.cruise.previousYaw = posControl.cruise.yaw; + + uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm] + + if ((previousState == NAV_STATE_CRUISE_2D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_2D_ADJUSTING) + || (previousState == NAV_STATE_CRUISE_3D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_3D_ADJUSTING) + || posControl.flags.isAdjustingHeading) { + calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance); + DEBUG_SET(DEBUG_CRUISE, 2, 1); + } else if (calculateDistanceToDestination(&posControl.cruise.targetPos) <= (navConfig()->fw.loiter_radius * 1.10f)) { //10% margin + calculateNewCruiseTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance); + DEBUG_SET(DEBUG_CRUISE, 2, 2); + } + + setDesiredPosition(&posControl.cruise.targetPos, posControl.cruise.yaw, NAV_POS_UPDATE_XY); + + return NAV_FSM_EVENT_NONE; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(navigationFSMState_t previousState) +{ + UNUSED(previousState); + DEBUG_SET(DEBUG_CRUISE, 0, 3); + + // User is rolling, changing manually direction. Wait until it is done and then restore CRUISE + if (posControl.flags.isAdjustingPosition) { + posControl.cruise.yaw = posControl.actualState.yaw; //store current heading + posControl.cruise.lastYawAdjustmentTime = millis(); + return NAV_FSM_EVENT_NONE; // reprocess the state + } + + resetPositionController(); + + return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState) +{ + if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now + + navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState); + + return navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(previousState); +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS(navigationFSMState_t previousState) +{ + navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState); + + return navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(previousState); +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(navigationFSMState_t previousState) +{ + navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState); + + return navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(previousState); +} static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState) { navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); - if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME) || !posEstimationHasGlobalReference()) { + if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) { // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing // If we are in dead-reckoning mode - also fail, since coordinates may be unreliable return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; @@ -949,6 +1191,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND // If position ok OR within valid timeout - continue if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) { + + // set altitude to go to when landing is not allowed + if (navConfig()->general.rth_home_altitude && !navigationRTHAllowsLanding()) { + posControl.homeWaypointAbove.pos.z = navConfig()->general.rth_home_altitude; + setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z); + } + // Wait until target heading is reached (with 15 deg margin for error) if (STATE(FIXED_WING)) { resetLandingDetector(); @@ -968,8 +1217,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND return NAV_FSM_EVENT_NONE; } } - } - else { + + } else { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } } @@ -1229,10 +1478,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_IN_PROGRESS } - //allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle. - if(feature(FEATURE_FW_LAUNCH)){ + //allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle. + if (feature(FEATURE_FW_LAUNCH)) { throttleStatus_e throttleStatus = calculateThrottleStatus(); - if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())){ + if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } } @@ -1384,6 +1633,11 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative; const float outValConstrained = constrainf(outVal, outMin, outMax); + pid->proportional = newProportional; + pid->integral = pid->integrator; + pid->derivative = newDerivative; + pid->output_constrained = outValConstrained; + /* Update I-term */ if (!(pidFlags & PID_ZERO_INTEGRATOR)) { const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt); @@ -1411,10 +1665,14 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu void navPidReset(pidController_t *pid) { pid->reset = true; + pid->proportional = 0.0f; + pid->integral = 0.0f; + 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->output_constrained = 0.0f; } void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD) @@ -1475,7 +1733,7 @@ bool checkForPositionSensorTimeout(void) /*----------------------------------------------------------- * Processes an update to XY-position and velocity *-----------------------------------------------------------*/ -void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY) +void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY) { posControl.actualState.abs.pos.x = newX; posControl.actualState.abs.pos.y = newY; @@ -1489,13 +1747,24 @@ void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, f posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY)); - if (estimateValid) { + // CASE 1: POS & VEL valid + if (estPosValid && estVelValid) { posControl.flags.estPosStatus = EST_TRUSTED; + posControl.flags.estVelStatus = EST_TRUSTED; posControl.flags.horizontalPositionDataNew = 1; posControl.lastValidPositionTimeMs = millis(); } + // CASE 1: POS invalid, VEL valid + else if (!estPosValid && estVelValid) { + posControl.flags.estPosStatus = EST_USABLE; // Pos usable, but not trusted + posControl.flags.estVelStatus = EST_TRUSTED; + posControl.flags.horizontalPositionDataNew = 1; + posControl.lastValidPositionTimeMs = millis(); + } + // CASE 3: can't use pos/vel data else { posControl.flags.estPosStatus = EST_NONE; + posControl.flags.estVelStatus = EST_NONE; posControl.flags.horizontalPositionDataNew = 0; } @@ -1657,6 +1926,22 @@ static void updateHomePositionCompatibility(void) 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); + } +} + /*----------------------------------------------------------- * Reset home position to current position *-----------------------------------------------------------*/ @@ -1664,24 +1949,7 @@ static void updateDesiredRTHAltitude(void) { if (ARMING_FLAG(ARMED)) { if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) { - switch (navConfig()->general.flags.rth_alt_control_mode) { - case NAV_RTH_NO_ALT: - posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z; - break; - case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin - posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude; - break; - case NAV_RTH_MAX_ALT: - posControl.homeWaypointAbove.pos.z = MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z); - break; - case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home - posControl.homeWaypointAbove.pos.z = MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z); - break; - case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home - default: - posControl.homeWaypointAbove.pos.z = posControl.homePosition.pos.z + navConfig()->general.rth_altitude; - break; - } + posControl.homeWaypointAbove.pos.z = RTHAltitude(); } } else { @@ -1911,6 +2179,13 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t dista farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z; } +void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance) +{ + origin->x = origin->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw)); + origin->y = origin->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw)); + origin->z = origin->z; +} + /*----------------------------------------------------------- * NAV land detector *-----------------------------------------------------------*/ @@ -2208,7 +2483,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) // WP #255 - special waypoint - directly set desiredPosition // Only valid when armed and in poshold mode else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) && - ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled && + ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled && (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) { // Convert to local coordinates geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE); @@ -2416,7 +2691,7 @@ void applyWaypointNavigationAndAltitudeHold(void) #if defined(NAV_BLACKBOX) navFlags = 0; if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0); - if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1); + if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1); if (posControl.flags.estPosStatus == EST_TRUSTED) navFlags |= (1 << 2); #if defined(NAV_GPS_GLITCH_DETECTION) if (isGPSGlitchDetected()) navFlags |= (1 << 4); @@ -2472,7 +2747,7 @@ void applyWaypointNavigationAndAltitudeHold(void) void switchNavigationFlightModes(void) { const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState); - const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE) & (~enabledNavFlightModes); + const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_CRUISE_MODE) & (~enabledNavFlightModes); DISABLE_FLIGHT_MODE(disabledFlightModes); ENABLE_FLIGHT_MODE(enabledNavFlightModes); } @@ -2487,12 +2762,12 @@ static bool canActivateAltHoldMode(void) static bool canActivatePosHoldMode(void) { - return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE); + return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE); } -static bool posEstimationHasGlobalReference(void) +static bool canActivateNavigationModes(void) { - return true; // For now assume that we always have global coordinates available + return (posControl.flags.estPosStatus == EST_TRUSTED) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE); } static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) @@ -2508,8 +2783,9 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } // Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data) - bool canActivateAltHold = canActivateAltHoldMode(); - bool canActivatePosHold = canActivatePosHoldMode(); + bool canActivateAltHold = canActivateAltHoldMode(); + bool canActivatePosHold = canActivatePosHoldMode(); + bool canActivateNavigation = canActivateNavigationModes(); // LAUNCH mode has priority over any other NAV mode if (STATE(FIXED_WING)) { @@ -2536,7 +2812,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } // RTH/Failsafe_RTH can override MANUAL - if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME))) { + if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) { // If we request forced RTH - attempt to activate it no matter what // This might switch to emergency landing controller if GPS is unavailable canActivateWaypoint = false; // Block WP mode if we switched to RTH for whatever reason @@ -2550,7 +2826,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } if (IS_RC_MODE_ACTIVE(BOXNAVWP)) { - if ((FLIGHT_MODE(NAV_WP_MODE)) || (posEstimationHasGlobalReference() && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0))) + if ((FLIGHT_MODE(NAV_WP_MODE)) || (canActivateNavigation && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0))) return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT; } else { @@ -2563,6 +2839,18 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; } + // PH has priority over CRUISE + // CRUISE has priority on AH + if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) { + + if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) + return NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D; + + if (FLIGHT_MODE(NAV_CRUISE_MODE) || (canActivatePosHold)) + return NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D; + + } + if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) { if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold)) return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD; @@ -2603,7 +2891,7 @@ bool navigationRequiresTurnAssistance(void) const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState); if (STATE(FIXED_WING)) { // For airplanes turn assistant is always required when controlling position - return (currentState & NAV_CTL_POS); + return (currentState & (NAV_CTL_POS | NAV_CTL_ALT)); } else { return false; @@ -2641,8 +2929,8 @@ bool navigationTerrainFollowingEnabled(void) bool navigationBlockArming(void) { - 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)); - const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)); + 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) @@ -2789,6 +3077,7 @@ void navigationInit(void) posControl.flags.estAltStatus = EST_NONE; posControl.flags.estPosStatus = EST_NONE; + posControl.flags.estVelStatus = EST_NONE; posControl.flags.estHeadingStatus = EST_NONE; posControl.flags.estAglStatus = EST_NONE; @@ -2854,7 +3143,7 @@ rthState_e getStateOfForcedRTH(void) bool navigationIsControllingThrottle(void) { navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); - return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)); + return ((stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)) && (getMotorStatus() != MOTOR_STOPPED_USER)); } bool navigationIsFlyingAutonomousMode(void) @@ -2880,6 +3169,27 @@ int32_t navigationGetHomeHeading(void) return posControl.homePosition.yaw; } +// returns m/s +float calculateAverageSpeed() { + return (float)getTotalTravelDistance() / (getFlightTime() * 100); +} + +const navigationPIDControllers_t* getNavigationPIDControllers(void) { + return &posControl.pids; +} + +bool isAdjustingPosition(void) { + return posControl.flags.isAdjustingPosition; +} + +bool isAdjustingHeading(void) { + return posControl.flags.isAdjustingHeading; +} + +int32_t getCruiseHeadingAdjustment(void) { + return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw); +} + #else // NAV #ifdef USE_GPS @@ -2917,7 +3227,7 @@ void onNewGPSData(void) int32_t dir; GPS_distance_cm_bearing(gpsSol.llh.lat, gpsSol.llh.lon, GPS_home.lat, GPS_home.lon, &dist, &dir); GPS_distanceToHome = dist / 100; - GPS_directionToHome = dir / 100; + GPS_directionToHome = lrintf(dir / 100.0f); } else { GPS_distanceToHome = 0; GPS_directionToHome = 0; @@ -2942,6 +3252,7 @@ int32_t getTotalTravelDistance(void) { return lrintf(GPS_totalTravelDistance); } + #endif #endif // NAV diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 89b6b7aa5f..46e7f87b63 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -17,6 +17,8 @@ #pragma once +#include "common/axis.h" +#include "common/filter.h" #include "common/maths.h" #include "common/vector.h" @@ -31,6 +33,8 @@ extern gpsLocation_t GPS_home; extern uint16_t GPS_distanceToHome; // distance to home point in meters extern int16_t GPS_directionToHome; // direction to home point in degrees +extern bool autoThrottleManuallyIncreased; + /* Navigation system updates */ void onNewGPSData(void); @@ -80,6 +84,7 @@ typedef struct positionEstimationConfig_s { uint8_t reset_home_type; // nav_reset_type_e uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s) uint8_t use_gps_velned; + uint8_t allow_dead_reckoning; uint16_t max_surface_altitude; @@ -94,6 +99,9 @@ typedef struct positionEstimationConfig_s { float w_xy_gps_p; // Weight (cutoff frequency) for GPS position measurements float w_xy_gps_v; // Weight (cutoff frequency) for GPS velocity measurements + float w_xy_flow_p; + float w_xy_flow_v; + float w_z_res_v; // When velocity sources lost slowly decrease estimated velocity with this weight float w_xy_res_v; @@ -132,6 +140,7 @@ typedef struct navConfig_s { uint16_t land_slowdown_maxalt; // Altitude to start lowering descent rate during RTH descend uint16_t emerg_descent_rate; // emergency landing descent rate uint16_t rth_altitude; // altitude to maintain when RTH is active (depends on rth_alt_control_mode) (cm) + uint16_t rth_home_altitude; // altitude to go to during RTH after the craft reached home (cm) uint16_t min_rth_distance; // 0 Disables. Minimal distance for RTH in cm, otherwise it will just autoland uint16_t rth_abort_threshold; // Initiate emergency landing if during RTH we get this much [cm] away from home uint16_t max_terrain_follow_altitude; // Max altitude to be used in SURFACE TRACKING mode @@ -156,6 +165,7 @@ typedef struct navConfig_s { uint8_t max_climb_angle; // Fixed wing max banking angle (deg) uint8_t max_dive_angle; // Fixed wing max banking angle (deg) uint16_t cruise_throttle; // Cruise throttle + uint16_t cruise_speed; // Speed at cruise throttle (cm/s), used for time/distance left before RTH uint16_t min_throttle; // Minimum allowed throttle in auto mode uint16_t max_throttle; // Maximum allowed throttle in auto mode uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10) @@ -173,6 +183,8 @@ typedef struct navConfig_s { uint16_t launch_max_altitude; // cm, altitude where to consider launch ended uint8_t launch_climb_angle; // Target climb angle for launch (deg) uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] + uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled + bool allow_manual_thr_increase; } fw; } navConfig_t; @@ -209,6 +221,46 @@ typedef struct { int32_t yaw; // deg * 100 } navWaypointPosition_t; +typedef struct { + float kP; + float kI; + float kD; + float kT; // Tracking gain (anti-windup) +} pidControllerParam_t; + +typedef struct { + float kP; +} pControllerParam_t; + +typedef struct { + bool reset; + pidControllerParam_t param; + pt1Filter_t dterm_filter_state; // last derivative for low-pass filter + 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 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 vel[XYZ_AXIS_COUNT]; + pidController_t surface; + + /* Fixed-wing PIDs */ + pidController_t fw_alt; + pidController_t fw_nav; +} navigationPIDControllers_t; + /* MultiWii-compatible params for telemetry */ typedef enum { MW_GPS_MODE_NONE = 0, @@ -288,9 +340,20 @@ bool navIsCalibrationComplete(void); bool navigationTerrainFollowingEnabled(void); /* Access to estimated position and velocity */ +typedef struct { + uint8_t altStatus; + uint8_t posStatus; + uint8_t velStatus; + uint8_t aglStatus; + fpVector3_t pos; + fpVector3_t vel; + float agl; +} navPositionAndVelocity_t; + float getEstimatedActualVelocity(int axis); float getEstimatedActualPosition(int axis); int32_t getTotalTravelDistance(void); +void getEstimatedPositionAndVelocity(navPositionAndVelocity_t * pos); /* Waypoint list access functions */ int getWaypointCount(void); @@ -301,6 +364,8 @@ void resetWaypointList(void); bool loadNonVolatileWaypointList(void); bool saveNonVolatileWaypointList(void); +float RTHAltitude(); + /* Geodetic functions */ typedef enum { GEO_ALT_ABSOLUTE, @@ -324,6 +389,7 @@ rthState_e getStateOfForcedRTH(void); /* Getter functions which return data about the state of the navigation system */ bool navigationIsControllingThrottle(void); +bool isFixedWingAutoThrottleManuallyIncreased(void); bool navigationIsFlyingAutonomousMode(void); /* Returns true iff navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active. @@ -331,6 +397,16 @@ bool navigationIsFlyingAutonomousMode(void); bool navigationRTHAllowsLanding(void); bool isNavLaunchEnabled(void); +bool isFixedWingLaunchDetected(void); + +float calculateAverageSpeed(); + +const navigationPIDControllers_t* getNavigationPIDControllers(void); + +int32_t navigationGetHeadingError(void); +int32_t getCruiseHeadingAdjustment(void); +bool isAdjustingPosition(void); +bool isAdjustingHeading(void); /* Returns the heading recorded when home position was acquired. * Note that the navigation system uses deg*100 as unit and angles diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 0e874c70a4..dde97449ef 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -48,6 +48,9 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "rx/rx.h" + + // If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind #define NAV_FW_THROTTLE_SPEED_BOOST_GAIN 1.5f #define NAV_FW_MIN_VEL_SPEED_BOOST 700.0f // 7 m/s @@ -58,6 +61,8 @@ static bool isPitchAdjustmentValid = false; static bool isRollAdjustmentValid = false; static float throttleSpeedAdjustment = 0; +static bool isAutoThrottleManuallyIncreased = false; +static int32_t navHeadingError; /*----------------------------------------------------------- @@ -182,6 +187,10 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs) *-----------------------------------------------------------*/ bool adjustFixedWingHeadingFromRCInput(void) { + if (ABS(rcCommand[YAW]) > rcControlsConfig()->pos_hold_deadband) { + return true; + } + return false; } @@ -218,7 +227,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) #define TAN_15DEG 0.26795f bool needToCalculateCircularLoiter = isApproachingLastWaypoint() && (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG)) - && (distanceToActualTarget > 50.0f); + && (distanceToActualTarget > 50.0f) + && !FLIGHT_MODE(NAV_CRUISE_MODE); // Calculate virtual position for straight movement if (needToCalculateCircularLoiter) { @@ -271,29 +281,29 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); // Calculate NAV heading error - int32_t headingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw); + navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw); // Forced turn direction // If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg - if (ABS(headingError) > 17000) { + if (ABS(navHeadingError) > 17000) { forceTurnDirection = true; } - else if (ABS(headingError) < 9000 && forceTurnDirection) { + else if (ABS(navHeadingError) < 9000 && forceTurnDirection) { forceTurnDirection = false; } // If forced turn direction flag is enabled we fix the sign of the direction if (forceTurnDirection) { - headingError = ABS(headingError); + navHeadingError = ABS(navHeadingError); } // Slow error monitoring (2Hz rate) if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) { // Check if error is decreasing over time - errorIsDecreasing = (ABS(previousHeadingError) > ABS(headingError)); + errorIsDecreasing = (ABS(previousHeadingError) > ABS(navHeadingError)); // Save values for next iteration - previousHeadingError = headingError; + previousHeadingError = navHeadingError; previousTimeMonitoringUpdate = currentTimeUs; } @@ -301,7 +311,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0); // Input error in (deg*100), output pitch angle (deg*100) - float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.yaw + headingError, posControl.actualState.yaw, US2S(deltaMicros), + float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.yaw + navHeadingError, posControl.actualState.yaw, US2S(deltaMicros), -DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle), pidFlags); @@ -445,6 +455,18 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat } uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); + + // Manual throttle increase + if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) { + if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95) + correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - navConfig()->fw.cruise_throttle); + else + correctedThrottleValue = motorConfig()->maxthrottle; + isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > navConfig()->fw.cruise_throttle); + } else { + isAutoThrottleManuallyIncreased = false; + } + rcCommand[THROTTLE] = constrain(correctedThrottleValue, motorConfig()->minthrottle, motorConfig()->maxthrottle); } @@ -471,6 +493,11 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat #endif } +bool isFixedWingAutoThrottleManuallyIncreased() +{ + return isAutoThrottleManuallyIncreased; +} + /*----------------------------------------------------------- * FixedWing land detector *-----------------------------------------------------------*/ @@ -527,23 +554,28 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, #ifdef NAV_FW_LIMIT_MIN_FLY_VELOCITY // Don't apply anything if ground speed is too low (<3m/s) if (posControl.actualState.velXY > 300) { - if (navStateFlags & NAV_CTL_ALT) - applyFixedWingAltitudeAndThrottleController(currentTimeUs); +#else + if (true) { +#endif + if (navStateFlags & NAV_CTL_ALT) { + if (getMotorStatus() == MOTOR_STOPPED_USER) { + // Motor has been stopped by user. Update target altitude and bypass navigation pitch/throttle control + resetFixedWingAltitudeController(); + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); + } else + applyFixedWingAltitudeAndThrottleController(currentTimeUs); + } if (navStateFlags & NAV_CTL_POS) applyFixedWingPositionController(currentTimeUs); - } - else { + + } else { posControl.rcAdjustment[PITCH] = 0; posControl.rcAdjustment[ROLL] = 0; } -#else - if (navStateFlags & NAV_CTL_ALT) - applyFixedWingAltitudeAndThrottleController(currentTimeUs); - if (navStateFlags & NAV_CTL_POS) - applyFixedWingPositionController(currentTimeUs); -#endif + if (FLIGHT_MODE(NAV_CRUISE_MODE) && posControl.flags.isAdjustingPosition) + rcCommand[ROLL] = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); //if (navStateFlags & NAV_CTL_YAW) if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) @@ -551,4 +583,9 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, } } +int32_t navigationGetHeadingError(void) +{ + return navHeadingError; +} + #endif // NAV diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index dee448093c..603866ee09 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -72,6 +72,8 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) targetVel = constrainf(targetVel, -navConfig()->general.max_auto_climb_rate, navConfig()->general.max_auto_climb_rate); } + posControl.pids.pos[Z].output_constrained = targetVel; + // limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing velocity. // if we are decelerating - don't limit (allow better recovery from falling) if (ABS(targetVel) > ABS(posControl.desiredState.vel.z)) { @@ -349,6 +351,9 @@ static void updatePositionVelocityController_MC(void) newVelTotal = maxSpeed; } + posControl.pids.pos[X].output_constrained = newVelX; + posControl.pids.pos[Y].output_constrained = newVelY; + // Apply expo & attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode) const float velHeadFactor = getVelocityHeadingAttenuationFactor(); const float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed); diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 1a7ece4d59..2c46baa74f 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -36,146 +36,22 @@ #include "drivers/time.h" #include "fc/config.h" -#include "fc/rc_controls.h" -#include "fc/runtime_config.h" #include "flight/imu.h" -#include "flight/pid.h" #include "io/gps.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "navigation/navigation_pos_estimator_private.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" -#include "sensors/boardalignment.h" #include "sensors/compass.h" #include "sensors/pitotmeter.h" -#include "sensors/rangefinder.h" #include "sensors/opflow.h" -#include "sensors/sensors.h" - -/** - * Model-identification based position estimator - * Based on INAV position estimator for PX4 by Anton Babushkin - * @author Konstantin Sharlaimov - */ -#define INAV_GPS_DEFAULT_EPH 200.0f // 2m GPS HDOP (gives about 1.6s of dead-reckoning if GPS is temporary lost) -#define INAV_GPS_DEFAULT_EPV 500.0f // 5m GPS VDOP - -#define INAV_GPS_ACCEPTANCE_EPE 500.0f // 5m acceptance radius - -#define INAV_ACC_BIAS_ACCEPTANCE_VALUE (GRAVITY_CMSS * 0.25f) // Max accepted bias correction of 0.25G - unlikely we are going to be that much off anyway - -#define INAV_GPS_GLITCH_RADIUS 250.0f // 2.5m GPS glitch radius -#define INAV_GPS_GLITCH_ACCEL 1000.0f // 10m/s/s max possible acceleration for GPS glitch detection - -#define INAV_POSITION_PUBLISH_RATE_HZ 50 // Publish position updates at this rate -#define INAV_PITOT_UPDATE_RATE 10 - -#define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout -#define INAV_BARO_TIMEOUT_MS 200 // Baro timeout -#define INAV_SURFACE_TIMEOUT_MS 300 // Surface timeout (missed 3 readings in a row) - -#define INAV_HISTORY_BUF_SIZE (INAV_POSITION_PUBLISH_RATE_HZ / 2) // Enough to hold 0.5 sec historical data - -typedef struct { - timeUs_t lastTriggeredTime; - timeUs_t deltaTime; -} navigationTimer_t; - -typedef struct { - timeUs_t lastUpdateTime; // Last update time (us) -#if defined(NAV_GPS_GLITCH_DETECTION) - bool glitchDetected; - bool glitchRecovery; -#endif - fpVector3_t pos; // GPS position in NEU coordinate system (cm) - fpVector3_t vel; // GPS velocity (cms) - float eph; - float epv; -} navPositionEstimatorGPS_t; - -typedef struct { - timeUs_t lastUpdateTime; // Last update time (us) - float alt; // Raw barometric altitude (cm) - float epv; -} navPositionEstimatorBARO_t; - -typedef struct { - timeUs_t lastUpdateTime; // Last update time (us) - float airspeed; // airspeed (cm/s) -} navPositionEstimatorPITOT_t; - -typedef enum { - SURFACE_QUAL_LOW, // Surface sensor signal lost long ago - most likely surface distance is incorrect - SURFACE_QUAL_MID, // Surface sensor is not available but we can somewhat trust the estimate - SURFACE_QUAL_HIGH // All good -} navAGLEstimateQuality_e; - -typedef struct { - timeUs_t lastUpdateTime; // Last update time (us) - float alt; // Raw altitude measurement (cm) - float reliability; -} navPositionEstimatorSURFACE_t; - -typedef struct { - timeUs_t lastUpdateTime; // Last update time (us) - bool isValid; - float quality; - float flowRate[2]; - float bodyRate[2]; -} navPositionEstimatorFLOW_t; - -typedef struct { - timeUs_t lastUpdateTime; // Last update time (us) - - // 3D position, velocity and confidence - fpVector3_t pos; - fpVector3_t vel; - float eph; - float epv; - - // AGL - navAGLEstimateQuality_e aglQual; - float aglOffset; // Offset between surface and pos.Z - float aglAlt; - float aglVel; -} navPositionEstimatorESTIMATE_t; - -typedef struct { - timeUs_t baroGroundTimeout; - float baroGroundAlt; - bool isBaroGroundValid; -} navPositionEstimatorSTATE_t; - -typedef struct { - fpVector3_t accelNEU; - fpVector3_t accelBias; - bool gravityCalibrationComplete; -} navPosisitonEstimatorIMU_t; - -typedef struct { - // Data sources - navPositionEstimatorGPS_t gps; - navPositionEstimatorBARO_t baro; - navPositionEstimatorSURFACE_t surface; - navPositionEstimatorPITOT_t pitot; - navPositionEstimatorFLOW_t flow; - - // IMU data - navPosisitonEstimatorIMU_t imu; - - // Estimate - navPositionEstimatorESTIMATE_t est; - - // Extra state variables - navPositionEstimatorSTATE_t state; -} navigationPosEstimator_t; - -static navigationPosEstimator_t posEstimator; +navigationPosEstimator_t posEstimator; PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 3); @@ -186,6 +62,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .reset_home_type = NAV_RESET_ON_EACH_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, @@ -195,11 +72,14 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .w_z_surface_v = 6.100f, .w_z_gps_p = 0.2f, - .w_z_gps_v = 0.5f, + .w_z_gps_v = 0.1f, .w_xy_gps_p = 1.0f, .w_xy_gps_v = 2.0f, + .w_xy_flow_p = 1.0f, + .w_xy_flow_v = 2.0f, + .w_z_res_v = 0.5f, .w_xy_res_v = 0.5f, @@ -209,25 +89,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .baro_epv = 100.0f ); -/* Inertial filter, implementation taken from PX4 implementation by Anton Babushkin */ -static void inavFilterPredict(int axis, float dt, float acc) -{ - posEstimator.est.pos.v[axis] += posEstimator.est.vel.v[axis] * dt + acc * dt * dt / 2.0f; - posEstimator.est.vel.v[axis] += acc * dt; -} - -static void inavFilterCorrectPos(int axis, float dt, float e, float w) -{ - float ewdt = e * w * dt; - posEstimator.est.pos.v[axis] += ewdt; - posEstimator.est.vel.v[axis] += w * ewdt; -} - -static void inavFilterCorrectVel(int axis, float dt, float e, float w) -{ - posEstimator.est.vel.v[axis] += e * w * dt; -} - #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; } #define getTimerDeltaMicros(tim) ((tim)->deltaTime) static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t currentTimeUs) @@ -480,64 +341,6 @@ static void updatePitotTopic(timeUs_t currentTimeUs) } #endif -#ifdef USE_RANGEFINDER -/** - * Read surface and update alt/vel topic - * Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available - */ -#define RANGEFINDER_RELIABILITY_RC_CONSTANT (0.47802f) -#define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f) -#define RANGEFINDER_RELIABILITY_LOW_THRESHOLD (0.33f) -#define RANGEFINDER_RELIABILITY_HIGH_THRESHOLD (0.75f) - -void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt) -{ - const float dt = US2S(currentTimeUs - posEstimator.surface.lastUpdateTime); - float newReliabilityMeasurement = 0; - - posEstimator.surface.lastUpdateTime = currentTimeUs; - - if (newSurfaceAlt >= 0) { - if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) { - newReliabilityMeasurement = 1.0f; - posEstimator.surface.alt = newSurfaceAlt; - } - else { - newReliabilityMeasurement = 0.0f; - } - } - else { - // Negative values - out of range or failed hardware - newReliabilityMeasurement = 0.0f; - } - - /* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */ - if (dt > 0.5f) { - posEstimator.surface.reliability = 0.0f; - } - else { - const float relAlpha = dt / (dt + RANGEFINDER_RELIABILITY_RC_CONSTANT); - posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha; - } -} -#endif - -#ifdef USE_OPTICAL_FLOW -/** - * Read optical flow topic - * Function is called by OPFLOW task as soon as new update is available - */ -void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs) -{ - posEstimator.flow.lastUpdateTime = currentTimeUs; - posEstimator.flow.isValid = opflow.isHwHealty && (opflow.flowQuality == OPFLOW_QUALITY_VALID); - posEstimator.flow.flowRate[X] = opflow.flowRate[X]; - posEstimator.flow.flowRate[Y] = opflow.flowRate[Y]; - posEstimator.flow.bodyRate[X] = opflow.bodyRate[X]; - posEstimator.flow.bodyRate[Y] = opflow.bodyRate[Y]; -} -#endif - /** * Update IMU topic * Function is called at main loop rate @@ -611,62 +414,97 @@ static void updateIMUTopic(void) } } -static float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w) +float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w) { return oldEPE + (newEPE - oldEPE) * w * dt; } -static bool navIsHeadingUsable(void) +static bool navIsAccelerationUsable(void) { - // If we have GPS - we need true IMU north (valid heading) - return isImuHeadingValid(); + return true; } -/** - * Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc) - * Function is called at main loop rate - */ -static void updateEstimatedTopic(timeUs_t currentTimeUs) +static bool navIsHeadingUsable(void) { - /* Calculate dT */ - float dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime); - posEstimator.est.lastUpdateTime = currentTimeUs; - - /* If IMU is not ready we can't estimate anything */ - if (!isImuReady()) { - posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f; - posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f; - return; + if (sensors(SENSOR_GPS)) { + // If we have GPS - we need true IMU north (valid heading) + return isImuHeadingValid(); } - - /* Calculate new EPH and EPV for the case we didn't update postion */ - float newEPH = posEstimator.est.eph; - float newEPV = posEstimator.est.epv; - - if (newEPH <= positionEstimationConfig()->max_eph_epv) { - newEPH *= 1.0f + dt; - } - - if (newEPV <= positionEstimationConfig()->max_eph_epv) { - newEPV *= 1.0f + dt; + else { + // If we don't have GPS - we may use whatever we have, other sensors are operating in body frame + return isImuHeadingValid() || positionEstimationConfig()->allow_dead_reckoning; } +} +static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) +{ /* Figure out if we have valid position data from our data sources */ -#if defined(NAV_GPS_GLITCH_DETECTION) - //isGPSValid = isGPSValid && !posEstimator.gps.glitchDetected; -#endif - const bool isGPSValid = sensors(SENSOR_GPS) && - posControl.gpsOrigin.valid && - ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && - (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv); - const bool isGPSZValid = isGPSValid && (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv); - const bool isBaroValid = sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS)); -#if defined(USE_BARO) || defined(USE_RANGEFINDER) - const bool isSurfaceValid = sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.surface.lastUpdateTime) <= MS2US(INAV_SURFACE_TIMEOUT_MS)); -#endif + uint32_t newFlags = 0; + + if (sensors(SENSOR_GPS) && posControl.gpsOrigin.valid && + ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && + (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) { + if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) { + newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID; + } + else { + newFlags |= EST_GPS_XY_VALID; + } + } + + if (sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS))) { + newFlags |= EST_BARO_VALID; + } + + if (sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.surface.lastUpdateTime) <= MS2US(INAV_SURFACE_TIMEOUT_MS))) { + newFlags |= EST_SURFACE_VALID; + } + + if (sensors(SENSOR_OPFLOW) && posEstimator.flow.isValid && ((currentTimeUs - posEstimator.flow.lastUpdateTime) <= MS2US(INAV_FLOW_TIMEOUT_MS))) { + newFlags |= EST_FLOW_VALID; + } + + if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) { + newFlags |= EST_XY_VALID; + } + + if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) { + newFlags |= EST_Z_VALID; + } + + return newFlags; +} + +static void estimationPredict(estimationContext_t * ctx) +{ + /* 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; + } + + /* Prediction step: XY-axis */ + if ((ctx->newFlags & EST_XY_VALID)) { + // Predict based on known velocity + posEstimator.est.pos.x += posEstimator.est.vel.x * ctx->dt; + posEstimator.est.pos.y += posEstimator.est.vel.y * ctx->dt; + + // 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; + } + } +} + +static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) +{ + if (ctx->newFlags & EST_BARO_VALID) { + timeUs_t currentTimeUs = micros(); - /* Do some preparations to data */ - if (isBaroValid) { if (!ARMING_FLAG(ARMED)) { posEstimator.state.baroGroundAlt = posEstimator.est.pos.z; posEstimator.state.isBaroGroundValid = true; @@ -682,261 +520,189 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) posEstimator.state.baroGroundTimeout = currentTimeUs + 250000; // 0.25 sec } } + + // We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it + bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) && + (((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) || + ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt)); + + // Altitude + const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z; + ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; + ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt; + + // If GPS is available - also use GPS climb rate + if (ctx->newFlags & EST_GPS_Z_VALID) { + // Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution + const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z; + const float gpsRocScaler = bellCurve(gpsRocResidual, 2.5f); + ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt; + } + + ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p); + + // Accelerometer bias + if (!isAirCushionEffectDetected) { + ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); + } + + return true; } - else { - posEstimator.state.isBaroGroundValid = false; - } - -#if defined(USE_BARO) - /* We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it */ - bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) && - ((isSurfaceValid && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) || - (isBaroValid && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt)); -#endif - - /* Validate EPV for GPS and calculate altitude/climb rate correction flags */ - const bool useGpsZPos = STATE(FIXED_WING) && !sensors(SENSOR_BARO) && isGPSValid && isGPSZValid; - const bool useGpsZVel = isGPSValid && isGPSZValid; - - /* Estimate validity */ - const bool isEstXYValid = (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv); - const bool isEstZValid = (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv); - - /* Prediction step: Z-axis */ - if (isEstZValid) { - inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.z); - } - - /* Prediction step: XY-axis */ - if (isEstXYValid) { - if (navIsHeadingUsable()) { - inavFilterPredict(X, dt, posEstimator.imu.accelNEU.x); - inavFilterPredict(Y, dt, posEstimator.imu.accelNEU.y); + else if (STATE(FIXED_WING) && (ctx->newFlags & EST_GPS_Z_VALID)) { + // If baro is not available - use GPS Z for correction on a plane + // Reset current estimate to GPS altitude if estimate not valid + if (!(ctx->newFlags & EST_Z_VALID)) { + ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z; + ctx->estVelCorr.z += posEstimator.gps.vel.z - posEstimator.est.vel.z; + ctx->newEPV = posEstimator.gps.epv; } else { - inavFilterPredict(X, dt, 0.0f); - inavFilterPredict(Y, dt, 0.0f); + // Altitude + const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z; + + ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt; + ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt; + ctx->estVelCorr.z += (posEstimator.gps.vel.z - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_gps_v * ctx->dt; + ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p); + + // Accelerometer bias + ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p); } + + return true; } - /* Accelerometer bias correction */ - const bool updateAccBias = (positionEstimationConfig()->w_acc_bias > 0); - fpVector3_t accelBiasCorr = { { 0, 0, 0} }; + return false; +} - /* Correction step: Z-axis */ - if (useGpsZPos || isBaroValid) { - float gpsWeightScaler = 1.0f; - - /* Handle Z-axis reset */ - if (!isEstZValid && useGpsZPos) { - posEstimator.est.pos.z = posEstimator.gps.pos.z; - posEstimator.est.vel.z = posEstimator.gps.vel.z; - newEPV = posEstimator.gps.epv; - } - else { -#if defined(USE_BARO) - /* Apply BARO correction to altitude */ - if (isBaroValid) { - const float baroResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z; - inavFilterCorrectPos(Z, dt, baroResidual, positionEstimationConfig()->w_z_baro_p); - newEPV = updateEPE(posEstimator.est.epv, dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p); - - /* accelerometer bias correction for baro */ - if (updateAccBias && !isAirCushionEffectDetected) { - accelBiasCorr.z -= baroResidual * sq(positionEstimationConfig()->w_z_baro_p); - } - } -#endif - - /* Apply GPS correction to altitude */ - if (useGpsZPos) { - const float gpsResidualZ = posEstimator.gps.pos.z - posEstimator.est.pos.z; - inavFilterCorrectPos(Z, dt, gpsResidualZ, positionEstimationConfig()->w_z_gps_p * gpsWeightScaler); - newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, gpsResidualZ), positionEstimationConfig()->w_z_gps_p); - - if (updateAccBias) { - accelBiasCorr.z -= gpsResidualZ * sq(positionEstimationConfig()->w_z_gps_p); - } - } - - /* Apply GPS correction to climb rate */ - if (useGpsZVel) { - const float gpsResidualZVel = posEstimator.gps.vel.z - posEstimator.est.vel.z; - inavFilterCorrectVel(Z, dt, gpsResidualZVel, positionEstimationConfig()->w_z_gps_v * sq(gpsWeightScaler)); - } - } - } - else { - inavFilterCorrectVel(Z, dt, 0.0f - posEstimator.est.vel.z, positionEstimationConfig()->w_z_res_v); - } - - /* Correction step: XY-axis */ - /* GPS */ - if (isGPSValid) { +static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) +{ + if (ctx->newFlags & EST_GPS_XY_VALID) { /* If GPS is valid and our estimate is NOT valid - reset it to GPS coordinates and velocity */ - if (!isEstXYValid) { - posEstimator.est.pos.x = posEstimator.gps.pos.x; - posEstimator.est.pos.y = posEstimator.gps.pos.y; - posEstimator.est.vel.x = posEstimator.gps.vel.x; - posEstimator.est.vel.y = posEstimator.gps.vel.y; - newEPH = posEstimator.gps.eph; + if (!(ctx->newFlags & EST_XY_VALID)) { + ctx->estPosCorr.x += posEstimator.gps.pos.x - posEstimator.est.pos.x; + ctx->estPosCorr.y += posEstimator.gps.pos.y - posEstimator.est.pos.y; + ctx->estVelCorr.x += posEstimator.gps.vel.x - posEstimator.est.vel.x; + ctx->estVelCorr.y += posEstimator.gps.vel.y - posEstimator.est.vel.y; + ctx->newEPH = posEstimator.gps.epv; } else { - const float gpsResidualX = posEstimator.gps.pos.x - posEstimator.est.pos.x; - const float gpsResidualY = posEstimator.gps.pos.y - posEstimator.est.pos.y; - const float gpsResidualXVel = posEstimator.gps.vel.x - posEstimator.est.vel.x; - const float gpsResidualYVel = posEstimator.gps.vel.y - posEstimator.est.vel.y; - const float gpsResidualXYMagnitude = sqrtf(sq(gpsResidualX) + sq(gpsResidualY)); + const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x; + const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y; + const float gpsVelXResidual = posEstimator.gps.vel.x - posEstimator.est.vel.x; + const float gpsVelYResidual = posEstimator.gps.vel.y - posEstimator.est.vel.y; + const float gpsPosResidualMag = sqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual)); - //const float gpsWeightScaler = scaleRangef(bellCurve(gpsResidualXYMagnitude, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f); + //const float gpsWeightScaler = scaleRangef(bellCurve(gpsPosResidualMag, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f); const float gpsWeightScaler = 1.0f; const float w_xy_gps_p = positionEstimationConfig()->w_xy_gps_p * gpsWeightScaler; const float w_xy_gps_v = positionEstimationConfig()->w_xy_gps_v * sq(gpsWeightScaler); - inavFilterCorrectPos(X, dt, gpsResidualX, w_xy_gps_p); - inavFilterCorrectPos(Y, dt, gpsResidualY, w_xy_gps_p); + // Coordinates + ctx->estPosCorr.x += gpsPosXResidual * w_xy_gps_p * ctx->dt; + ctx->estPosCorr.y += gpsPosYResidual * w_xy_gps_p * ctx->dt; - inavFilterCorrectVel(X, dt, gpsResidualXVel, w_xy_gps_v); - inavFilterCorrectVel(Y, dt, gpsResidualYVel, w_xy_gps_v); + // Velocity from coordinates + ctx->estVelCorr.x += gpsPosXResidual * sq(w_xy_gps_p) * ctx->dt; + ctx->estVelCorr.y += gpsPosYResidual * sq(w_xy_gps_p) * ctx->dt; + + // Velocity from direct measurement + ctx->estVelCorr.x += gpsVelXResidual * w_xy_gps_v * ctx->dt; + ctx->estVelCorr.y += gpsVelYResidual * w_xy_gps_v * ctx->dt; + + // Accelerometer bias + ctx->accBiasCorr.x -= gpsPosXResidual * sq(w_xy_gps_p); + ctx->accBiasCorr.y -= gpsPosYResidual * sq(w_xy_gps_p); /* Adjust EPH */ - newEPH = updateEPE(posEstimator.est.eph, dt, MAX(posEstimator.gps.eph, gpsResidualXYMagnitude), positionEstimationConfig()->w_xy_gps_p); + ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p); } + + return true; } - else { - inavFilterCorrectVel(X, dt, 0.0f - posEstimator.est.vel.x, positionEstimationConfig()->w_xy_res_v); - inavFilterCorrectVel(Y, dt, 0.0f - posEstimator.est.vel.y, positionEstimationConfig()->w_xy_res_v); + + return false; +} + +/** + * Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc) + * Function is called at main loop rate + */ +static void updateEstimatedTopic(timeUs_t currentTimeUs) +{ + estimationContext_t ctx; + + /* Calculate dT */ + ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime); + posEstimator.est.lastUpdateTime = currentTimeUs; + + /* If IMU is not ready we can't estimate anything */ + if (!isImuReady()) { + posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f; + posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f; + posEstimator.flags = 0; + return; } + /* Calculate new EPH and EPV for the case we didn't update postion */ + ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f); + ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f); + ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs); + vectorZero(&ctx.estPosCorr); + vectorZero(&ctx.estVelCorr); + vectorZero(&ctx.accBiasCorr); + + /* AGL estimation - separate process, decouples from Z coordinate */ + estimationCalculateAGL(&ctx); + + /* Prediction stage: X,Y,Z */ + estimationPredict(&ctx); + + /* Correction stage: Z */ + const bool estZCorrectOk = + estimationCalculateCorrection_Z(&ctx); + + /* Correction stage: XY: GPS, FLOW */ + // FIXME: Handle transition from FLOW to GPS and back - seamlessly fly indoor/outdoor + const bool estXYCorrectOk = + estimationCalculateCorrection_XY_GPS(&ctx) || + estimationCalculateCorrection_XY_FLOW(&ctx); + + // If we can't apply correction or accuracy is off the charts - decay velocity to zero + if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) { + ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt; + ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt; + } + + if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) { + ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt; + } + + // Apply corrections + vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr); + vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); + /* Correct accelerometer bias */ - if (updateAccBias) { - const float accelBiasCorrMagnitudeSq = sq(accelBiasCorr.x) + sq(accelBiasCorr.y) + sq(accelBiasCorr.z); + if (positionEstimationConfig()->w_acc_bias > 0.0f) { + const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z); if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) { /* transform error vector from NEU frame to body frame */ - imuTransformVectorEarthToBody(&accelBiasCorr); + imuTransformVectorEarthToBody(&ctx.accBiasCorr); /* Correct accel bias */ - posEstimator.imu.accelBias.x += accelBiasCorr.x * positionEstimationConfig()->w_acc_bias * dt; - posEstimator.imu.accelBias.y += accelBiasCorr.y * positionEstimationConfig()->w_acc_bias * dt; - posEstimator.imu.accelBias.z += accelBiasCorr.z * positionEstimationConfig()->w_acc_bias * dt; + posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * positionEstimationConfig()->w_acc_bias * ctx.dt; + posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * positionEstimationConfig()->w_acc_bias * ctx.dt; + posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * positionEstimationConfig()->w_acc_bias * ctx.dt; } } /* Update uncertainty */ - posEstimator.est.eph = newEPH; - posEstimator.est.epv = newEPV; + posEstimator.est.eph = ctx.newEPH; + posEstimator.est.epv = ctx.newEPV; - /* AGL estimation */ -#if defined(USE_RANGEFINDER) && defined(USE_BARO) - if (isSurfaceValid && isBaroValid) { - navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual; - bool resetSurfaceEstimate = false; - switch (posEstimator.est.aglQual) { - case SURFACE_QUAL_LOW: - if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) { - newAglQuality = SURFACE_QUAL_HIGH; - resetSurfaceEstimate = true; - } - else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) { - newAglQuality = SURFACE_QUAL_LOW; - } - else { - newAglQuality = SURFACE_QUAL_LOW; - } - break; - - case SURFACE_QUAL_MID: - if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) { - newAglQuality = SURFACE_QUAL_HIGH; - } - else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) { - newAglQuality = SURFACE_QUAL_MID; - } - else { - newAglQuality = SURFACE_QUAL_LOW; - } - break; - - case SURFACE_QUAL_HIGH: - if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) { - newAglQuality = SURFACE_QUAL_HIGH; - } - else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) { - newAglQuality = SURFACE_QUAL_MID; - } - else { - newAglQuality = SURFACE_QUAL_LOW; - } - break; - } - - posEstimator.est.aglQual = newAglQuality; - - if (resetSurfaceEstimate) { - posEstimator.est.aglAlt = posEstimator.surface.alt; - if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) { - posEstimator.est.aglVel = posEstimator.est.vel.z; - posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt; - } - else { - posEstimator.est.aglVel = 0; - posEstimator.est.aglOffset = 0; - } - } - - // Update estimate - posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * dt + posEstimator.imu.accelNEU.z * dt * dt * 0.5f; - posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.z * dt; - - // Apply correction - if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) { - // Correct estimate from rangefinder - const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt; - const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 75.0f), 0.0f, 1.0f, 0.1f, 1.0f); - - posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * dt; - posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * dt; - - // Update estimate offset - if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) { - posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt; - } - } - else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) { - // Correct estimate from altitude fused from rangefinder and global altitude - const float estAltResidual = (posEstimator.est.pos.z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt; - const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt; - const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability; - const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler); - - posEstimator.est.aglAlt += mixedResidual * positionEstimationConfig()->w_z_surface_p * dt; - posEstimator.est.aglVel += mixedResidual * positionEstimationConfig()->w_z_surface_v * dt; - } - else { // SURFACE_QUAL_LOW - // In this case rangefinder can't be trusted - simply use global altitude - posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset; - posEstimator.est.aglVel = posEstimator.est.vel.z; - } - } - else { - posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset; - posEstimator.est.aglVel = posEstimator.est.vel.z; - posEstimator.est.aglQual = SURFACE_QUAL_LOW; - } - -#if defined(NAV_BLACKBOX) - DEBUG_SET(DEBUG_AGL, 0, posEstimator.surface.reliability * 1000); - DEBUG_SET(DEBUG_AGL, 1, posEstimator.est.aglQual); - DEBUG_SET(DEBUG_AGL, 2, posEstimator.est.aglAlt); - DEBUG_SET(DEBUG_AGL, 3, posEstimator.est.aglVel); -#endif - -#else - posEstimator.est.aglAlt = posEstimator.est.pos.z; - posEstimator.est.aglVel = posEstimator.est.vel.z; - posEstimator.est.aglQual = SURFACE_QUAL_LOW; -#endif + // Keep flags for further usage + posEstimator.flags = ctx.newFlags; } /** @@ -954,10 +720,11 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs) if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) { /* Publish position update */ if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) { - updateActualHorizontalPositionAndVelocity(true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y); + // FIXME!!!!! + updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y); } else { - updateActualHorizontalPositionAndVelocity(false, posEstimator.est.pos.x, posEstimator.est.pos.y, 0, 0); + updateActualHorizontalPositionAndVelocity(false, false, posEstimator.est.pos.x, posEstimator.est.pos.y, 0, 0); } /* Publish altitude update and set altitude validity */ @@ -1001,6 +768,9 @@ void initializePositionEstimator(void) posEstimator.est.aglAlt = 0; posEstimator.est.aglVel = 0; + posEstimator.est.flowCoordinates[X] = 0; + posEstimator.est.flowCoordinates[Y] = 0; + posEstimator.imu.gravityCalibrationComplete = false; for (axis = 0; axis < 3; axis++) { diff --git a/src/main/navigation/navigation_pos_estimator_agl.c b/src/main/navigation/navigation_pos_estimator_agl.c new file mode 100644 index 0000000000..cba35d4069 --- /dev/null +++ b/src/main/navigation/navigation_pos_estimator_agl.c @@ -0,0 +1,196 @@ +/* + * 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" + +#if defined(USE_NAV) + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" +#include "navigation/navigation_pos_estimator_private.h" + +#include "sensors/rangefinder.h" +#include "sensors/barometer.h" + +extern navigationPosEstimator_t posEstimator; + +#ifdef USE_RANGEFINDER +/** + * Read surface and update alt/vel topic + * Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available + */ +void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt) +{ + const float dt = US2S(currentTimeUs - posEstimator.surface.lastUpdateTime); + float newReliabilityMeasurement = 0; + + posEstimator.surface.lastUpdateTime = currentTimeUs; + + if (newSurfaceAlt >= 0) { + if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) { + newReliabilityMeasurement = 1.0f; + posEstimator.surface.alt = newSurfaceAlt; + } + else { + newReliabilityMeasurement = 0.0f; + } + } + else { + // Negative values - out of range or failed hardware + newReliabilityMeasurement = 0.0f; + } + + /* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */ + if (dt > 0.5f) { + posEstimator.surface.reliability = 0.0f; + } + else { + const float relAlpha = dt / (dt + RANGEFINDER_RELIABILITY_RC_CONSTANT); + posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha; + } +} +#endif + +void estimationCalculateAGL(estimationContext_t * ctx) +{ +#if defined(USE_RANGEFINDER) && defined(USE_BARO) + if ((ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_BARO_VALID)) { + navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual; + bool resetSurfaceEstimate = false; + switch (posEstimator.est.aglQual) { + case SURFACE_QUAL_LOW: + if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) { + newAglQuality = SURFACE_QUAL_HIGH; + resetSurfaceEstimate = true; + } + else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) { + newAglQuality = SURFACE_QUAL_LOW; + } + else { + newAglQuality = SURFACE_QUAL_LOW; + } + break; + + case SURFACE_QUAL_MID: + if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) { + newAglQuality = SURFACE_QUAL_HIGH; + } + else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) { + newAglQuality = SURFACE_QUAL_MID; + } + else { + newAglQuality = SURFACE_QUAL_LOW; + } + break; + + case SURFACE_QUAL_HIGH: + if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) { + newAglQuality = SURFACE_QUAL_HIGH; + } + else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) { + newAglQuality = SURFACE_QUAL_MID; + } + else { + newAglQuality = SURFACE_QUAL_LOW; + } + break; + } + + posEstimator.est.aglQual = newAglQuality; + + if (resetSurfaceEstimate) { + posEstimator.est.aglAlt = posEstimator.surface.alt; + if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) { + posEstimator.est.aglVel = posEstimator.est.vel.z; + posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt; + } + else { + posEstimator.est.aglVel = 0; + posEstimator.est.aglOffset = 0; + } + } + + // Update estimate + posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * ctx->dt; + posEstimator.est.aglAlt = posEstimator.imu.accelNEU.z * sq(ctx->dt) * 0.5f; + posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.z * ctx->dt; + + // Apply correction + if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) { + // Correct estimate from rangefinder + const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt; + const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 75.0f), 0.0f, 1.0f, 0.1f, 1.0f); + + posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * ctx->dt; + posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * ctx->dt; + + // Update estimate offset + if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) { + posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt; + } + } + else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) { + // Correct estimate from altitude fused from rangefinder and global altitude + const float estAltResidual = (posEstimator.est.pos.z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt; + const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt; + const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability; + const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler); + + posEstimator.est.aglAlt += mixedResidual * positionEstimationConfig()->w_z_surface_p * ctx->dt; + posEstimator.est.aglVel += mixedResidual * positionEstimationConfig()->w_z_surface_v * ctx->dt; + } + else { // SURFACE_QUAL_LOW + // In this case rangefinder can't be trusted - simply use global altitude + posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset; + posEstimator.est.aglVel = posEstimator.est.vel.z; + } + } + else { + posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset; + posEstimator.est.aglVel = posEstimator.est.vel.z; + posEstimator.est.aglQual = SURFACE_QUAL_LOW; + } + + DEBUG_SET(DEBUG_AGL, 0, posEstimator.surface.reliability * 1000); + DEBUG_SET(DEBUG_AGL, 1, posEstimator.est.aglQual); + DEBUG_SET(DEBUG_AGL, 2, posEstimator.est.aglAlt); + DEBUG_SET(DEBUG_AGL, 3, posEstimator.est.aglVel); + +#else + UNUSED(ctx); + posEstimator.est.aglAlt = posEstimator.est.pos.z; + posEstimator.est.aglVel = posEstimator.est.vel.z; + posEstimator.est.aglQual = SURFACE_QUAL_LOW; +#endif +} + +#endif // NAV diff --git a/src/main/navigation/navigation_pos_estimator_flow.c b/src/main/navigation/navigation_pos_estimator_flow.c new file mode 100644 index 0000000000..c0763d42eb --- /dev/null +++ b/src/main/navigation/navigation_pos_estimator_flow.c @@ -0,0 +1,128 @@ +/* + * 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" + +#if defined(USE_NAV) + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" +#include "navigation/navigation_pos_estimator_private.h" + +#include "sensors/rangefinder.h" +#include "sensors/opflow.h" + +#include "flight/imu.h" + +extern navigationPosEstimator_t posEstimator; + +#ifdef USE_OPTICAL_FLOW +/** + * Read optical flow topic + * Function is called by OPFLOW task as soon as new update is available + */ +void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs) +{ + posEstimator.flow.lastUpdateTime = currentTimeUs; + posEstimator.flow.isValid = opflow.isHwHealty && (opflow.flowQuality == OPFLOW_QUALITY_VALID); + posEstimator.flow.flowRate[X] = opflow.flowRate[X]; + posEstimator.flow.flowRate[Y] = opflow.flowRate[Y]; + posEstimator.flow.bodyRate[X] = opflow.bodyRate[X]; + posEstimator.flow.bodyRate[Y] = opflow.bodyRate[Y]; +} +#endif + +bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx) +{ +#if defined(USE_RANGEFINDER) && defined(USE_OPTICAL_FLOW) + if (!((ctx->newFlags & EST_FLOW_VALID) && (ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_Z_VALID))) { + return false; + } + + // FIXME: flow may use AGL estimate if available + const bool canUseFlow = (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD); + + if (!canUseFlow) { + return false; + } + + // Calculate linear velocity based on angular velocity and altitude + // Technically we should calculate arc length here, but for fast sampling this is accurate enough + fpVector3_t flowVel = { + .x = - (posEstimator.flow.flowRate[Y] - posEstimator.flow.bodyRate[Y]) * posEstimator.surface.alt, + .y = (posEstimator.flow.flowRate[X] - posEstimator.flow.bodyRate[X]) * posEstimator.surface.alt, + .z = posEstimator.est.vel.z + }; + + // At this point flowVel will hold linear velocities in earth frame + imuTransformVectorBodyToEarth(&flowVel); + + // Calculate velocity correction + const float flowVelXInnov = flowVel.x - posEstimator.est.vel.x; + const float flowVelYInnov = flowVel.y - posEstimator.est.vel.y; + + ctx->estVelCorr.x = flowVelXInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt; + ctx->estVelCorr.y = flowVelYInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt; + + // Calculate position correction if possible/allowed + if ((ctx->newFlags & EST_GPS_XY_VALID)) { + // If GPS is valid - reset flow estimated coordinates to GPS + posEstimator.est.flowCoordinates[X] = posEstimator.gps.pos.x; + posEstimator.est.flowCoordinates[Y] = posEstimator.gps.pos.y; + } + else if (positionEstimationConfig()->allow_dead_reckoning) { + posEstimator.est.flowCoordinates[X] += flowVel.x * ctx->dt; + posEstimator.est.flowCoordinates[Y] += flowVel.y * ctx->dt; + + const float flowResidualX = posEstimator.est.flowCoordinates[X] - posEstimator.est.pos.x; + const float flowResidualY = posEstimator.est.flowCoordinates[Y] - posEstimator.est.pos.y; + + ctx->estPosCorr.x = flowResidualX * positionEstimationConfig()->w_xy_flow_p * ctx->dt; + ctx->estPosCorr.y = flowResidualY * positionEstimationConfig()->w_xy_flow_p * ctx->dt; + + 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, 2, posEstimator.est.flowCoordinates[X]); + DEBUG_SET(DEBUG_FLOW, 3, posEstimator.est.flowCoordinates[Y]); + + return true; +#else + UNUSED(ctx); + return false; +#endif +} + + +#endif // NAV diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h new file mode 100644 index 0000000000..6b2b6c5d68 --- /dev/null +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -0,0 +1,175 @@ +/* + * 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 + +#include "common/axis.h" +#include "common/maths.h" + +#include "sensors/sensors.h" + +#define INAV_GPS_DEFAULT_EPH 200.0f // 2m GPS HDOP (gives about 1.6s of dead-reckoning if GPS is temporary lost) +#define INAV_GPS_DEFAULT_EPV 500.0f // 5m GPS VDOP + +#define INAV_GPS_ACCEPTANCE_EPE 500.0f // 5m acceptance radius + +#define INAV_ACC_BIAS_ACCEPTANCE_VALUE (GRAVITY_CMSS * 0.25f) // Max accepted bias correction of 0.25G - unlikely we are going to be that much off anyway + +#define INAV_GPS_GLITCH_RADIUS 250.0f // 2.5m GPS glitch radius +#define INAV_GPS_GLITCH_ACCEL 1000.0f // 10m/s/s max possible acceleration for GPS glitch detection + +#define INAV_POSITION_PUBLISH_RATE_HZ 50 // Publish position updates at this rate +#define INAV_PITOT_UPDATE_RATE 10 + +#define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout +#define INAV_BARO_TIMEOUT_MS 200 // Baro timeout +#define INAV_SURFACE_TIMEOUT_MS 300 // Surface timeout (missed 3 readings in a row) +#define INAV_FLOW_TIMEOUT_MS 200 + +#define RANGEFINDER_RELIABILITY_RC_CONSTANT (0.47802f) +#define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f) +#define RANGEFINDER_RELIABILITY_LOW_THRESHOLD (0.33f) +#define RANGEFINDER_RELIABILITY_HIGH_THRESHOLD (0.75f) + +typedef struct { + timeUs_t lastTriggeredTime; + timeUs_t deltaTime; +} navigationTimer_t; + +typedef struct { + timeUs_t lastUpdateTime; // Last update time (us) +#if defined(NAV_GPS_GLITCH_DETECTION) + bool glitchDetected; + bool glitchRecovery; +#endif + fpVector3_t pos; // GPS position in NEU coordinate system (cm) + fpVector3_t vel; // GPS velocity (cms) + float eph; + float epv; +} navPositionEstimatorGPS_t; + +typedef struct { + timeUs_t lastUpdateTime; // Last update time (us) + float alt; // Raw barometric altitude (cm) + float epv; +} navPositionEstimatorBARO_t; + +typedef struct { + timeUs_t lastUpdateTime; // Last update time (us) + float airspeed; // airspeed (cm/s) +} navPositionEstimatorPITOT_t; + +typedef enum { + SURFACE_QUAL_LOW, // Surface sensor signal lost long ago - most likely surface distance is incorrect + SURFACE_QUAL_MID, // Surface sensor is not available but we can somewhat trust the estimate + SURFACE_QUAL_HIGH // All good +} navAGLEstimateQuality_e; + +typedef struct { + timeUs_t lastUpdateTime; // Last update time (us) + float alt; // Raw altitude measurement (cm) + float reliability; +} navPositionEstimatorSURFACE_t; + +typedef struct { + timeUs_t lastUpdateTime; // Last update time (us) + bool isValid; + float quality; + float flowRate[2]; + float bodyRate[2]; +} navPositionEstimatorFLOW_t; + +typedef struct { + timeUs_t lastUpdateTime; // Last update time (us) + + // 3D position, velocity and confidence + fpVector3_t pos; + fpVector3_t vel; + float eph; + float epv; + + // AGL + navAGLEstimateQuality_e aglQual; + float aglOffset; // Offset between surface and pos.Z + float aglAlt; + float aglVel; + + // FLOW + float flowCoordinates[2]; +} navPositionEstimatorESTIMATE_t; + +typedef struct { + fpVector3_t accelNEU; + fpVector3_t accelBias; + bool gravityCalibrationComplete; +} navPosisitonEstimatorIMU_t; + +typedef enum { + EST_GPS_XY_VALID = (1 << 0), + EST_GPS_Z_VALID = (1 << 1), + EST_BARO_VALID = (1 << 2), + EST_SURFACE_VALID = (1 << 3), + EST_FLOW_VALID = (1 << 4), + EST_XY_VALID = (1 << 5), + EST_Z_VALID = (1 << 6), +} navPositionEstimationFlags_e; + +typedef struct { + timeUs_t baroGroundTimeout; + float baroGroundAlt; + bool isBaroGroundValid; +} navPositionEstimatorSTATE_t; + + +typedef struct { + uint32_t flags; + + // Data sources + navPositionEstimatorGPS_t gps; + navPositionEstimatorBARO_t baro; + navPositionEstimatorSURFACE_t surface; + navPositionEstimatorPITOT_t pitot; + navPositionEstimatorFLOW_t flow; + + // IMU data + navPosisitonEstimatorIMU_t imu; + + // Estimate + navPositionEstimatorESTIMATE_t est; + + // Extra state variables + navPositionEstimatorSTATE_t state; +} navigationPosEstimator_t; + +typedef struct { + float dt; + uint32_t newFlags; + float newEPV; + float newEPH; + fpVector3_t estPosCorr; + fpVector3_t estVelCorr; + fpVector3_t accBiasCorr; +} estimationContext_t; + +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); + + diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 2500641878..34f853784d 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -21,7 +21,10 @@ #if defined(USE_NAV) +#include "common/axis.h" +#include "common/maths.h" #include "common/filter.h" +#include "common/time.h" #include "fc/runtime_config.h" #define MIN_POSITION_UPDATE_RATE_HZ 5 // Minimum position update rate at which XYZ controllers would be applied @@ -35,13 +38,6 @@ #define INAV_SURFACE_MAX_DISTANCE 40 -#define HZ2US(hz) (1000000 / (hz)) -#define US2S(us) ((us) * 1e-6f) -#define US2MS(us) ((us) * 1e-3f) -#define MS2US(ms) ((ms) * 1000) -#define MS2S(ms) ((ms) * 1e-3f) -#define HZ2S(hz) US2S(HZ2US(hz)) - typedef enum { NAV_POS_UPDATE_NONE = 0, NAV_POS_UPDATE_Z = 1 << 1, @@ -80,6 +76,7 @@ typedef struct navigationFlags_s { navigationEstimateStatus_e estAltStatus; // Indicates that we have a working altitude sensor (got at least one valid reading from it) navigationEstimateStatus_e estPosStatus; // Indicates that GPS is working (or not) + navigationEstimateStatus_e estVelStatus; // Indicates that GPS is working (or not) navigationEstimateStatus_e estAglStatus; navigationEstimateStatus_e estHeadingStatus; // Indicate valid heading - wither mag or GPS at certain speed on airplane @@ -95,46 +92,12 @@ typedef struct navigationFlags_s { bool forcedRTHActivated; } navigationFlags_t; -typedef struct { - float kP; - float kI; - float kD; - float kT; // Tracking gain (anti-windup) -} pidControllerParam_t; - -typedef struct { - float kP; -} pControllerParam_t; - typedef enum { PID_DTERM_FROM_ERROR = 1 << 0, PID_ZERO_INTEGRATOR = 1 << 1, PID_SHRINK_INTEGRATOR = 1 << 2, } pidControllerFlags_e; -typedef struct { - bool reset; - pidControllerParam_t param; - pt1Filter_t dterm_filter_state; // last derivative for low-pass filter - float integrator; // integrator value - float last_input; // last input for derivative -} pidController_t; - -typedef struct { - pControllerParam_t param; -} pController_t; - -typedef struct navigationPIDControllers_s { - /* Multicopter PIDs */ - pController_t pos[XYZ_AXIS_COUNT]; - pidController_t vel[XYZ_AXIS_COUNT]; - pidController_t surface; - - /* Fixed-wing PIDs */ - pidController_t fw_alt; - pidController_t fw_nav; -} navigationPIDControllers_t; - typedef struct { fpVector3_t pos; fpVector3_t vel; @@ -181,6 +144,9 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2, + NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D, + NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D, + NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ, NAV_FSM_EVENT_COUNT, } navigationFSMEvent_t; @@ -225,6 +191,14 @@ typedef enum { NAV_PERSISTENT_ID_LAUNCH_WAIT = 26, NAV_PERSISTENT_ID_UNUSED_3 = 27, // was NAV_STATE_LAUNCH_MOTOR_DELAY NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS = 28, + + NAV_PERSISTENT_ID_CRUISE_2D_INITIALIZE = 29, + NAV_PERSISTENT_ID_CRUISE_2D_IN_PROGRESS = 30, + NAV_PERSISTENT_ID_CRUISE_2D_ADJUSTING = 31, + + NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE = 32, + NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS = 33, + NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING = 34, } navigationPersistentId_e; typedef enum { @@ -263,6 +237,13 @@ typedef enum { NAV_STATE_LAUNCH_WAIT, NAV_STATE_LAUNCH_IN_PROGRESS, + NAV_STATE_CRUISE_2D_INITIALIZE, + NAV_STATE_CRUISE_2D_IN_PROGRESS, + NAV_STATE_CRUISE_2D_ADJUSTING, + NAV_STATE_CRUISE_3D_INITIALIZE, + NAV_STATE_CRUISE_3D_IN_PROGRESS, + NAV_STATE_CRUISE_3D_ADJUSTING, + NAV_STATE_COUNT, } navigationFSMState_t; @@ -310,6 +291,13 @@ typedef struct { float minimalDistanceToHome; } rthSanityChecker_t; +typedef struct { + fpVector3_t targetPos; + int32_t yaw; + int32_t previousYaw; + timeMs_t lastYawAdjustmentTime; +} navCruise_t; + typedef struct { /* Flags and navigation system state */ navigationFSMState_t navState; @@ -341,6 +329,9 @@ typedef struct { uint32_t homeDistance; // cm int32_t homeDirection; // deg*100 + /* Cruise */ + navCruise_t cruise; + /* Waypoint list */ navWaypoint_t waypointList[NAV_MAX_WAYPOINTS]; bool waypointListValid; @@ -385,7 +376,7 @@ bool isApproachingLastWaypoint(void); float getActiveWaypointSpeed(void); void updateActualHeading(bool headingValid, int32_t newHeading); -void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY); +void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY); void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus); bool checkForPositionSensorTimeout(void); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 7f0c85ebc4..d6abb4aeef 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -465,7 +465,7 @@ static uint16_t applyChannelFiltering(uint8_t chan, uint16_t sample) return quickMedianFilter5_16(rcSamples[chan]); } -void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) +bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); @@ -535,6 +535,7 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } rcSampleIndex++; + return true; } void parseRcChannels(const char *input) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 6ee9da5b55..d577dd6362 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -165,7 +165,7 @@ void rxUpdateRSSISource(void); bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime); bool rxIsReceivingSignal(void); bool rxAreFlightChannelsValid(void); -void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs); +bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs); void parseRcChannels(const char *input); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index e76f7f96e3..f90af8fb8f 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -114,7 +114,7 @@ typedef enum { #ifdef USE_RCDEVICE TASK_RCDEVICE, #endif -#ifdef VTX_CONTROL +#ifdef USE_VTX_CONTROL TASK_VTXCTRL, #endif diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 2aeddc5103..495ebb62ef 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -75,6 +75,9 @@ STATIC_FASTRAM int32_t accADC[XYZ_AXIS_COUNT]; STATIC_FASTRAM biquadFilter_t accFilter[XYZ_AXIS_COUNT]; +STATIC_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT]; +STATIC_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT]; + #ifdef USE_ACC_NOTCH STATIC_FASTRAM filterApplyFnPtr accNotchFilterApplyFn; STATIC_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT]; @@ -311,6 +314,7 @@ bool accInit(uint32_t targetLooptime) acc.dev.acc_1G = 256; // set default acc.dev.initFn(&acc.dev); acc.accTargetLooptime = targetLooptime; + acc.accClipCount = 0; accInitFilters(); if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) { @@ -531,10 +535,27 @@ void accUpdate(void) applySensorAlignment(accADC, accADC, acc.dev.accAlign); applyBoardAlignment(accADC); + // Calculate acceleration readings in G's for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { acc.accADCf[axis] = (float)accADC[axis] / acc.dev.acc_1G; } + // Before filtering check for clipping and vibration levels + if (ABS(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || ABS(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || ABS(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) { + acc.accClipCount++; + } + + // Calculate vibration levels + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + // filter accel at 5hz + const float accFloorFilt = pt1FilterApply(&accVibeFloorFilter[axis], acc.accADCf[axis]); + + // calc difference from this sample and 5hz filtered value, square and filter at 2hz + const float accDiff = acc.accADCf[axis] - accFloorFilt; + acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff); + } + + // 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]); @@ -554,6 +575,23 @@ void accUpdate(void) #endif } +void accGetVibrationLevels(fpVector3_t *accVibeLevels) +{ + accVibeLevels->x = sqrtf(acc.accVibeSq[X]); + accVibeLevels->y = sqrtf(acc.accVibeSq[Y]); + accVibeLevels->z = sqrtf(acc.accVibeSq[Z]); +} + +float accGetVibrationLevel(void) +{ + return sqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]); +} + +uint32_t accGetClipCount(void) +{ + return acc.accClipCount; +} + void accSetCalibrationValues(void) { if ((accelerometerConfig()->accZero.raw[X] == 0) && (accelerometerConfig()->accZero.raw[Y] == 0) && (accelerometerConfig()->accZero.raw[Z] == 0) && @@ -573,6 +611,12 @@ void accInitFilters(void) } } + const float accDt = acc.accTargetLooptime * 1e-6f; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + pt1FilterInit(&accVibeFloorFilter[axis], ACC_VIBE_FLOOR_FILT_HZ, accDt); + pt1FilterInit(&accVibeFilter[axis], ACC_VIBE_FILT_HZ, accDt); + } + #ifdef USE_ACC_NOTCH STATIC_FASTRAM biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT]; accNotchFilterApplyFn = nullFilterApply; diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 45f6ef728b..e02e654ba6 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -27,6 +27,10 @@ #define GRAVITY_CMSS 980.665f #define GRAVITY_MSS 9.80665f +#define ACC_CLIPPING_THRESHOLD_G 7.9f +#define ACC_VIBE_FLOOR_FILT_HZ 5.0f +#define ACC_VIBE_FILT_HZ 2.0f + // Type of accelerometer used/detected typedef enum { ACC_NONE = 0, @@ -48,6 +52,8 @@ 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; } acc_t; extern acc_t acc; @@ -68,6 +74,9 @@ bool accInit(uint32_t accTargetLooptime); bool accIsCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void accGetMeasuredAcceleration(fpVector3_t *measuredAcc); +void accGetVibrationLevels(fpVector3_t *accVibeLevels); +float accGetVibrationLevel(void); +uint32_t accGetClipCount(void); void accUpdate(void); void accSetCalibrationValues(void); void accInitFilters(void); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 82d78bba27..f1272a027f 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -15,14 +15,20 @@ * along with Cleanflight. If not, see . */ +#include + #include "stdbool.h" #include "stdint.h" +#include "stdlib.h" #include "platform.h" +#include "build/debug.h" + #include "common/maths.h" #include "common/filter.h" +#include "config/config_reset.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -30,7 +36,14 @@ #include "drivers/time.h" #include "fc/config.h" +#include "fc/fc_core.h" #include "fc/runtime_config.h" +#include "fc/stats.h" + +#include "flight/imu.h" +#include "flight/mixer.h" + +#include "navigation/navigation.h" #include "config/feature.h" @@ -42,7 +55,6 @@ #include "io/beeper.h" -#include "build/debug.h" #define ADCVREF 3300 // in mV (3300 = 3.3V) @@ -52,6 +64,7 @@ #define VBATT_HYSTERESIS 10 // Batt Hysteresis of +/-100mV for changing battery state #define VBATT_LPF_FREQ 1 // Battery voltage filtering cutoff #define AMPERAGE_LPF_FREQ 1 // Battery current filtering cutoff +#define IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH 10 // Minimum sample count to consider calculated power supply impedance as stable // Battery monitoring stuff @@ -62,44 +75,65 @@ static uint16_t batteryCriticalVoltage; static uint32_t batteryRemainingCapacity = 0; static bool batteryUseCapacityThresholds = false; static bool batteryFullWhenPluggedIn = false; +static bool profileAutoswitchDisable = false; static uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) static uint16_t vbatLatestADC = 0; // most recent unsmoothed raw reading from vbat ADC static uint16_t amperageLatestADC = 0; // most recent raw reading from current ADC static uint16_t powerSupplyImpedance = 0; // calculated impedance in milliohm static uint16_t sagCompensatedVBat = 0; // calculated no load vbat +static bool powerSupplyImpedanceIsValid = false; -static int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) +static int16_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) static int32_t power = 0; // power draw in cW (0.01W resolution) static int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start static int32_t mWhDrawn = 0; // energy (milliWatt hours) drawn from the battery since start batteryState_e batteryState; +const batteryProfile_t *currentBatteryProfile; -PG_REGISTER_WITH_RESET_TEMPLATE(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 2); +PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 0); -PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig, +void pgResetFn_batteryProfiles(batteryProfile_t *instance) +{ + for (int i = 0; i < MAX_BATTERY_PROFILE_COUNT; i++) { + RESET_CONFIG(batteryProfile_t, &instance[i], + .cells = 0, - .voltage = { - .scale = VBAT_SCALE_DEFAULT, - .cellDetect = 430, - .cellMax = 420, - .cellMin = 330, - .cellWarning = 350 - }, + .voltage = { + .cellDetect = 430, + .cellMax = 420, + .cellMin = 330, + .cellWarning = 350 + }, + + .capacity = { + .value = 0, + .warning = 0, + .critical = 0, + .unit = BAT_CAPACITY_UNIT_MAH, + } + ); + } +} + +PG_REGISTER_WITH_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, PG_BATTERY_METERS_CONFIG, 0); + +PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, + + .voltage_scale = VBAT_SCALE_DEFAULT, .current = { - .offset = CURRENT_METER_OFFSET, + .type = CURRENT_SENSOR_ADC, .scale = CURRENT_METER_SCALE, - .type = CURRENT_SENSOR_ADC + .offset = CURRENT_METER_OFFSET }, - .capacity = { - .value = 0, - .warning = 0, - .critical = 0, - .unit = BAT_CAPACITY_UNIT_MAH, - } + .voltageSource = BAT_VOLTAGE_RAW, + + .cruise_power = 0, + .idle_power = 0, + .rth_energy_margin = 5 ); @@ -107,13 +141,13 @@ uint16_t batteryAdcToVoltage(uint16_t src) { // calculate battery voltage based on ADC reading // result is Vbatt in 0.01V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 1100 = 11:1 voltage divider (10k:1k) - return((uint64_t)src * batteryConfig()->voltage.scale * ADCVREF / (0xFFF * 1000)); + return((uint64_t)src * batteryMetersConfig()->voltage_scale * ADCVREF / (0xFFF * 1000)); } -int32_t currentSensorToCentiamps(uint16_t src) +int16_t currentSensorToCentiamps(uint16_t src) { - int32_t microvolts = ((uint32_t)src * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryConfig()->current.offset * 1000; - return microvolts / batteryConfig()->current.scale; // current in 0.01A steps + int32_t microvolts = ((uint32_t)src * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryMetersConfig()->current.offset * 100; + return microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps } void batteryInit(void) @@ -125,6 +159,53 @@ void batteryInit(void) batteryCriticalVoltage = 0; } +// profileDetect() profile sorting compare function +static int profile_compare(profile_comp_t *a, profile_comp_t *b) { + if (a->max_voltage < b->max_voltage) + return -1; + else if (a->max_voltage > b->max_voltage) + return 1; + else + return 0; +} + +// Find profile matching plugged battery for profile_autoselect +static int8_t profileDetect() { + profile_comp_t profile_comp_array[MAX_BATTERY_PROFILE_COUNT]; + + // Prepare profile sort + for (uint8_t i = 0; i < MAX_BATTERY_PROFILE_COUNT; ++i) { + const batteryProfile_t *profile = batteryProfiles(i); + profile_comp_array[i].profile_index = i; + profile_comp_array[i].max_voltage = profile->cells * profile->voltage.cellDetect; + } + + // Sort profiles by max voltage + qsort(profile_comp_array, MAX_BATTERY_PROFILE_COUNT, sizeof(*profile_comp_array), (int (*)(const void *, const void *))profile_compare); + + // Return index of the first profile where vbat <= profile_max_voltage + uint16_t vbatLatest = batteryAdcToVoltage(vbatLatestADC); + for (uint8_t i = 0; i < MAX_BATTERY_PROFILE_COUNT; ++i) + if ((profile_comp_array[i].max_voltage > 0) && (vbatLatest <= profile_comp_array[i].max_voltage)) + return profile_comp_array[i].profile_index; + + // No matching profile found + return -1; +} + +void setBatteryProfile(uint8_t profileIndex) +{ + if (profileIndex >= MAX_BATTERY_PROFILE_COUNT) { + profileIndex = 0; + } + currentBatteryProfile = batteryProfiles(profileIndex); +} + +void activateBatteryProfile(void) +{ + batteryInit(); +} + static void updateBatteryVoltage(timeUs_t timeDelta) { uint16_t vbatSample; @@ -152,17 +233,28 @@ void batteryUpdate(timeUs_t timeDelta) delay(VBATT_STABLE_DELAY); updateBatteryVoltage(timeDelta); - unsigned cells = (batteryAdcToVoltage(vbatLatestADC) / batteryConfig()->voltage.cellDetect) + 1; - if (cells > 8) cells = 8; // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) + int8_t detectedProfileIndex = -1; + if (feature(FEATURE_BAT_PROFILE_AUTOSWITCH) && (!profileAutoswitchDisable)) + detectedProfileIndex = profileDetect(); - batteryCellCount = cells; - batteryFullVoltage = batteryCellCount * batteryConfig()->voltage.cellMax; - batteryWarningVoltage = batteryCellCount * batteryConfig()->voltage.cellWarning; - batteryCriticalVoltage = batteryCellCount * batteryConfig()->voltage.cellMin; + if (detectedProfileIndex != -1) { + systemConfigMutable()->current_battery_profile_index = detectedProfileIndex; + setBatteryProfile(detectedProfileIndex); + batteryCellCount = currentBatteryProfile->cells; + } else if (currentBatteryProfile->cells > 0) + batteryCellCount = currentBatteryProfile->cells; + else { + batteryCellCount = (batteryAdcToVoltage(vbatLatestADC) / currentBatteryProfile->voltage.cellDetect) + 1; + if (batteryCellCount > 8) batteryCellCount = 8; // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) + } - batteryFullWhenPluggedIn = batteryAdcToVoltage(vbatLatestADC) >= (batteryFullVoltage - cells * VBATT_CELL_FULL_MAX_DIFF); - batteryUseCapacityThresholds = feature(FEATURE_CURRENT_METER) && batteryFullWhenPluggedIn && (batteryConfig()->capacity.value > 0) && - (batteryConfig()->capacity.warning > 0) && (batteryConfig()->capacity.critical > 0); + batteryFullVoltage = batteryCellCount * currentBatteryProfile->voltage.cellMax; + batteryWarningVoltage = batteryCellCount * currentBatteryProfile->voltage.cellWarning; + batteryCriticalVoltage = batteryCellCount * currentBatteryProfile->voltage.cellMin; + + batteryFullWhenPluggedIn = batteryAdcToVoltage(vbatLatestADC) >= (batteryFullVoltage - batteryCellCount * VBATT_CELL_FULL_MAX_DIFF); + batteryUseCapacityThresholds = feature(FEATURE_CURRENT_METER) && batteryFullWhenPluggedIn && (currentBatteryProfile->capacity.value > 0) && + (currentBatteryProfile->capacity.warning > 0) && (currentBatteryProfile->capacity.critical > 0); } /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD */ @@ -175,33 +267,34 @@ void batteryUpdate(timeUs_t timeDelta) if (batteryState != BATTERY_NOT_PRESENT) { - if ((batteryConfig()->capacity.value > 0) && batteryFullWhenPluggedIn) { - uint32_t capacityDiffBetweenFullAndEmpty = batteryConfig()->capacity.value - batteryConfig()->capacity.critical; - int32_t drawn = (batteryConfig()->capacity.unit == BAT_CAPACITY_UNIT_MWH ? mWhDrawn : mAhDrawn); + if ((currentBatteryProfile->capacity.value > 0) && batteryFullWhenPluggedIn) { + uint32_t capacityDiffBetweenFullAndEmpty = currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical; + int32_t drawn = (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH ? mWhDrawn : mAhDrawn); batteryRemainingCapacity = (drawn > (int32_t)capacityDiffBetweenFullAndEmpty ? 0 : capacityDiffBetweenFullAndEmpty - drawn); } if (batteryUseCapacityThresholds) { if (batteryRemainingCapacity == 0) batteryState = BATTERY_CRITICAL; - else if (batteryRemainingCapacity <= batteryConfig()->capacity.warning - batteryConfig()->capacity.critical) + else if (batteryRemainingCapacity <= currentBatteryProfile->capacity.warning - currentBatteryProfile->capacity.critical) batteryState = BATTERY_WARNING; } else { + uint16_t stateVoltage = getBatteryVoltage(); switch (batteryState) { case BATTERY_OK: - if (vbat <= (batteryWarningVoltage - VBATT_HYSTERESIS)) + if (stateVoltage <= (batteryWarningVoltage - VBATT_HYSTERESIS)) batteryState = BATTERY_WARNING; break; case BATTERY_WARNING: - if (vbat <= (batteryCriticalVoltage - VBATT_HYSTERESIS)) { + if (stateVoltage <= (batteryCriticalVoltage - VBATT_HYSTERESIS)) { batteryState = BATTERY_CRITICAL; - } else if (vbat > (batteryWarningVoltage + VBATT_HYSTERESIS)){ + } else if (stateVoltage > (batteryWarningVoltage + VBATT_HYSTERESIS)){ batteryState = BATTERY_OK; } break; case BATTERY_CRITICAL: - if (vbat > (batteryCriticalVoltage + VBATT_HYSTERESIS)) + if (stateVoltage > (batteryCriticalVoltage + VBATT_HYSTERESIS)) batteryState = BATTERY_WARNING; break; default: @@ -210,16 +303,17 @@ void batteryUpdate(timeUs_t timeDelta) } // handle beeper - switch (batteryState) { - case BATTERY_WARNING: - beeper(BEEPER_BAT_LOW); - break; - case BATTERY_CRITICAL: - beeper(BEEPER_BAT_CRIT_LOW); - break; - default: - break; - } + if (ARMING_FLAG(ARMED) || !ARMING_FLAG(WAS_EVER_ARMED) || failsafeIsActive()) + switch (batteryState) { + case BATTERY_WARNING: + beeper(BEEPER_BAT_LOW); + break; + case BATTERY_CRITICAL: + beeper(BEEPER_BAT_CRIT_LOW); + break; + default: + break; + } } } @@ -245,10 +339,19 @@ bool isBatteryVoltageConfigured(void) uint16_t getBatteryVoltage(void) { + if (batteryMetersConfig()->voltageSource == BAT_VOLTAGE_SAG_COMP) { + return sagCompensatedVBat; + } + return vbat; } -uint16_t getSagCompensatedBatteryVoltage(void) +uint16_t getBatteryRawVoltage(void) +{ + return vbat; +} + +uint16_t getBatterySagCompensatedVoltage(void) { return sagCompensatedVBat; } @@ -274,6 +377,14 @@ uint8_t getBatteryCellCount(void) } uint16_t getBatteryAverageCellVoltage(void) +{ + if (batteryCellCount > 0) { + return getBatteryVoltage() / batteryCellCount; + } + return 0; +} + +uint16_t getBatteryRawAverageCellVoltage(void) { if (batteryCellCount > 0) { return vbat / batteryCellCount; @@ -281,6 +392,14 @@ uint16_t getBatteryAverageCellVoltage(void) return 0; } +uint16_t getBatterySagCompensatedAverageCellVoltage(void) +{ + if (batteryCellCount > 0) { + return sagCompensatedVBat / batteryCellCount; + } + return 0; +} + uint32_t getBatteryRemainingCapacity(void) { return batteryRemainingCapacity; @@ -288,15 +407,15 @@ uint32_t getBatteryRemainingCapacity(void) bool isAmperageConfigured(void) { - return feature(FEATURE_CURRENT_METER) && batteryConfig()->current.type != CURRENT_SENSOR_NONE; + return feature(FEATURE_CURRENT_METER) && batteryMetersConfig()->current.type != CURRENT_SENSOR_NONE; } -int32_t getAmperage(void) +int16_t getAmperage(void) { return amperage; } -int32_t getAmperageLatestADC(void) +int16_t getAmperageLatestADC(void) { return amperageLatestADC; } @@ -322,19 +441,19 @@ void currentMeterUpdate(timeUs_t timeDelta) static pt1Filter_t amperageFilterState; static int64_t mAhdrawnRaw = 0; - switch (batteryConfig()->current.type) { + switch (batteryMetersConfig()->current.type) { case CURRENT_SENSOR_ADC: amperageLatestADC = adcGetChannel(ADC_CURRENT); amperageLatestADC = pt1FilterApply4(&erageFilterState, amperageLatestADC, AMPERAGE_LPF_FREQ, timeDelta * 1e-6f); amperage = currentSensorToCentiamps(amperageLatestADC); break; case CURRENT_SENSOR_VIRTUAL: - amperage = batteryConfig()->current.offset; + amperage = batteryMetersConfig()->current.offset; if (ARMING_FLAG(ARMED)) { throttleStatus_e throttleStatus = calculateThrottleStatus(); int32_t throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); - amperage += throttleFactor * batteryConfig()->current.scale / 1000; + amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000; } break; case CURRENT_SENSOR_NONE: @@ -342,53 +461,97 @@ void currentMeterUpdate(timeUs_t timeDelta) break; } - mAhdrawnRaw += (amperage * timeDelta) / 1000; + // Work around int64 math compiler bug, don't change it unless the bug has been fixed ! + // should be: mAhdrawnRaw += (int64_t)amperage * timeDelta / 1000; + mAhdrawnRaw += (int64_t)((int32_t)amperage * timeDelta) / 1000; + mAhDrawn = mAhdrawnRaw / (3600 * 100); } void powerMeterUpdate(timeUs_t timeDelta) { static int64_t mWhDrawnRaw = 0; - power = amperage * vbat / 100; // power unit is cW (0.01W resolution) - int32_t heatLossesCompensatedPower_mW = amperage * vbat / 10 + sq((int64_t)amperage) * powerSupplyImpedance / 10000; - mWhDrawnRaw += (int64_t)heatLossesCompensatedPower_mW * timeDelta / 10000; + power = (int32_t)amperage * vbat / 100; // power unit is cW (0.01W resolution) + int32_t heatLossesCompensatedPower_mW = (int32_t)amperage * vbat / 10 + sq((int64_t)amperage) * powerSupplyImpedance / 10000; + + // Work around int64 math compiler bug, don't change it unless the bug has been fixed ! + // should be: mWhDrawnRaw += (int64_t)heatLossesCompensatedPower_mW * timeDelta / 10000; + mWhDrawnRaw += (int64_t)((int64_t)heatLossesCompensatedPower_mW * timeDelta) / 10000; + mWhDrawn = mWhDrawnRaw / (3600 * 100); } -void sagCompensatedVBatUpdate(timeUs_t currentTime) +// calculate true power including heat losses in power supply (battery + wires) +// power is in cW (0.01W) +// batteryWarningVoltage is in cV (0.01V) +int32_t heatLossesCompensatedPower(int32_t power) +{ + return power + sq(power * 100 / batteryWarningVoltage) * powerSupplyImpedance / 100000; +} + +void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) { static timeUs_t recordTimestamp = 0; - static int32_t amperageRecord; + static int16_t amperageRecord; static uint16_t vbatRecord; + static uint8_t impedanceSampleCount = 0; + static pt1Filter_t impedanceFilterState; + static pt1Filter_t sagCompVBatFilterState; + static batteryState_e last_battery_state = BATTERY_NOT_PRESENT; + + if ((batteryState != BATTERY_NOT_PRESENT) && (last_battery_state == BATTERY_NOT_PRESENT)) { + pt1FilterReset(&sagCompVBatFilterState, vbat); + pt1FilterReset(&impedanceFilterState, 0); + } if (batteryState == BATTERY_NOT_PRESENT) { recordTimestamp = 0; + impedanceSampleCount = 0; powerSupplyImpedance = 0; + powerSupplyImpedanceIsValid = false; sagCompensatedVBat = vbat; } else { - if (cmpTimeUs(currentTime, recordTimestamp) > 20000000) + if (cmpTimeUs(currentTime, recordTimestamp) > 500000) recordTimestamp = 0; if (!recordTimestamp) { amperageRecord = amperage; vbatRecord = vbat; recordTimestamp = currentTime; - } else if ((amperage - amperageRecord >= 400) && ((int16_t)vbatRecord - vbat >= 10)) { - powerSupplyImpedance = (int32_t)(vbatRecord - vbat) * 1000 / (amperage - amperageRecord); - amperageRecord = amperage; - vbatRecord = vbat; - recordTimestamp = currentTime; + } else if ((amperage - amperageRecord >= 200) && ((int16_t)vbatRecord - vbat >= 4)) { + + uint16_t impedanceSample = (int32_t)(vbatRecord - vbat) * 1000 / (amperage - amperageRecord); + + if (impedanceSampleCount <= IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH) { + impedanceSampleCount += 1; + } + + if (impedanceFilterState.state) { + pt1FilterSetTimeConstant(&impedanceFilterState, impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH ? 1.2 : 0.5); + pt1FilterApply3(&impedanceFilterState, impedanceSample, timeDelta * 1e-6f); + } else { + pt1FilterReset(&impedanceFilterState, impedanceSample); + } + + if (impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH) { + powerSupplyImpedance = lrintf(pt1FilterGetLastOutput(&impedanceFilterState)); + powerSupplyImpedanceIsValid = true; + } + } - sagCompensatedVBat = MIN(batteryFullVoltage, vbat + powerSupplyImpedance * amperage / 1000); - + uint16_t sagCompensatedVBatSample = MIN(batteryFullVoltage, vbat + (int32_t)powerSupplyImpedance * amperage / 1000); + pt1FilterSetTimeConstant(&sagCompVBatFilterState, sagCompensatedVBatSample < pt1FilterGetLastOutput(&sagCompVBatFilterState) ? 40 : 500); + sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, timeDelta * 1e-6f)); } DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 0, powerSupplyImpedance); DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 1, sagCompensatedVBat); + + last_battery_state = batteryState; } uint8_t calculateBatteryPercentage(void) @@ -396,9 +559,31 @@ uint8_t calculateBatteryPercentage(void) if (batteryState == BATTERY_NOT_PRESENT) return 0; - if (batteryFullWhenPluggedIn && feature(FEATURE_CURRENT_METER) && (batteryConfig()->capacity.value > 0) && (batteryConfig()->capacity.critical > 0)) { - uint32_t capacityDiffBetweenFullAndEmpty = batteryConfig()->capacity.value - batteryConfig()->capacity.critical; + if (batteryFullWhenPluggedIn && feature(FEATURE_CURRENT_METER) && (currentBatteryProfile->capacity.value > 0) && (currentBatteryProfile->capacity.critical > 0)) { + uint32_t capacityDiffBetweenFullAndEmpty = currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical; return constrain(batteryRemainingCapacity * 100 / capacityDiffBetweenFullAndEmpty, 0, 100); } else return constrain((vbat - batteryCriticalVoltage) * 100L / (batteryFullVoltage - batteryCriticalVoltage), 0, 100); } + +void batteryDisableProfileAutoswitch(void) { + profileAutoswitchDisable = true; +} + +bool isPowerSupplyImpedanceValid(void) { + return powerSupplyImpedanceIsValid; +} + +uint16_t getPowerSupplyImpedance(void) { + return powerSupplyImpedance; +} + +// returns cW (0.01W) +int32_t calculateAveragePower() { + return (int64_t)mWhDrawn * 360 / getFlightTime(); +} + +// returns mWh / meter +int32_t calculateAverageEfficiency() { + return getFlyingEnergy() * 100 / getTotalTravelDistance(); +} diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index ac6f08c6db..3fb063264f 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -34,6 +34,10 @@ #define CURRENT_METER_OFFSET 0 #endif +#ifndef MAX_BATTERY_PROFILE_COUNT +#define MAX_BATTERY_PROFILE_COUNT 3 +#endif + typedef enum { CURRENT_SENSOR_NONE = 0, CURRENT_SENSOR_ADC, @@ -46,15 +50,19 @@ typedef enum { BAT_CAPACITY_UNIT_MWH, } batCapacityUnit_e; -typedef struct batteryConfig_s { +typedef struct { + uint8_t profile_index; + uint16_t max_voltage; +} profile_comp_t; - struct { - uint16_t scale; // adjust this to match battery voltage to reported value - uint16_t cellDetect; // maximum voltage per cell, used for auto-detecting battery cell count in 0.01V units, default is 430 (4.3V) - uint16_t cellMax; // maximum voltage per cell, used for full battery detection and battery gauge voltage in 0.01V units, default is 424 (4.24V) - uint16_t cellMin; // minimum voltage per cell, this triggers battery critical alarm, in 0.01V units, default is 330 (3.3V) - uint16_t cellWarning; // warning voltage per cell, this triggers battery warning alarm, in 0.01V units, default is 350 (3.5V) - } voltage; +typedef enum { + BAT_VOLTAGE_RAW, + BAT_VOLTAGE_SAG_COMP +} batVoltageSource_e; + +typedef struct batteryMetersConfig_s { + + uint16_t voltage_scale; struct { int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A @@ -62,6 +70,25 @@ typedef struct batteryConfig_s { currentSensor_e type; // type of current meter used, either ADC or virtual } current; + batVoltageSource_e voltageSource; + + uint32_t cruise_power; // power drawn by the motor(s) at cruise throttle/speed (cW) + uint16_t idle_power; // power drawn by the system when the motor(s) are stopped (cW) + uint8_t rth_energy_margin; // Energy that should be left after RTH (%), used for remaining time/distance before RTH + +} batteryMetersConfig_t; + +typedef struct batteryProfile_s { + + uint8_t cells; + + struct { + uint16_t cellDetect; // maximum voltage per cell, used for auto-detecting battery cell count in 0.01V units, default is 430 (4.3V) + uint16_t cellMax; // maximum voltage per cell, used for auto-detecting battery voltage in 0.01V units, default is 421 (4.21V) + uint16_t cellMin; // minimum voltage per cell, this triggers battery critical alarm, in 0.01V units, default is 330 (3.3V) + uint16_t cellWarning; // warning voltage per cell, this triggers battery warning alarm, in 0.01V units, default is 350 (3.5V) + } voltage; + struct { uint32_t value; // mAh or mWh (see capacity.unit) uint32_t warning; // mAh or mWh (see capacity.unit) @@ -69,9 +96,12 @@ typedef struct batteryConfig_s { batCapacityUnit_e unit; // Describes unit of capacity.value, capacity.warning and capacity.critical } capacity; -} batteryConfig_t; +} batteryProfile_t; -PG_DECLARE(batteryConfig_t, batteryConfig); +PG_DECLARE(batteryMetersConfig_t, batteryMetersConfig); +PG_DECLARE_ARRAY(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles); + +extern const batteryProfile_t *currentBatteryProfile; typedef enum { BATTERY_OK = 0, @@ -80,32 +110,44 @@ typedef enum { BATTERY_NOT_PRESENT } batteryState_e; + uint16_t batteryAdcToVoltage(uint16_t src); batteryState_e getBatteryState(void); bool batteryWasFullWhenPluggedIn(void); bool batteryUsesCapacityThresholds(void); void batteryInit(void); +void setBatteryProfile(uint8_t profileIndex); +void activateBatteryProfile(void); +void batteryDisableProfileAutoswitch(void); bool isBatteryVoltageConfigured(void); +bool isPowerSupplyImpedanceValid(void); uint16_t getBatteryVoltage(void); -uint16_t getSagCompensatedBatteryVoltage(void); +uint16_t getBatteryRawVoltage(void); +uint16_t getBatterySagCompensatedVoltage(void); uint16_t getBatteryVoltageLatestADC(void); uint16_t getBatteryWarningVoltage(void); uint8_t getBatteryCellCount(void); +uint16_t getBatteryRawAverageCellVoltage(void); uint16_t getBatteryAverageCellVoltage(void); +uint16_t getBatterySagCompensatedAverageCellVoltage(void); uint32_t getBatteryRemainingCapacity(void); +uint16_t getPowerSupplyImpedance(void); bool isAmperageConfigured(void); -int32_t getAmperage(void); -int32_t getAmperageLatestADC(void); +int16_t getAmperage(void); +int16_t getAmperageLatestADC(void); int32_t getPower(void); int32_t getMAhDrawn(void); int32_t getMWhDrawn(void); void batteryUpdate(timeUs_t timeDelta); void currentMeterUpdate(timeUs_t timeDelta); -void sagCompensatedVBatUpdate(timeUs_t currentTime); +void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta); void powerMeterUpdate(timeUs_t timeDelta); uint8_t calculateBatteryPercentage(void); float calculateThrottleCompensationFactor(void); +int32_t calculateAveragePower(); +int32_t calculateAverageEfficiency(); +int32_t heatLossesCompensatedPower(int32_t power); diff --git a/src/main/sensors/opflow.c b/src/main/sensors/opflow.c index f39a023691..f76937caa1 100755 --- a/src/main/sensors/opflow.c +++ b/src/main/sensors/opflow.c @@ -73,8 +73,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLO PG_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, .opflow_hardware = OPFLOW_NONE, - .opflow_scale = 1.0f, .opflow_align = CW0_DEG_FLIP, + .opflow_scale = 1.0f, ); static bool opflowDetect(opflowDev_t * dev, uint8_t opflowHardwareToUse) @@ -178,19 +178,19 @@ void opflowUpdate(timeUs_t currentTimeUs) if ((opflow.flowQuality == OPFLOW_QUALITY_VALID) && (opflow.gyroBodyRateTimeUs > 0)) { const float integralToRateScaler = (1.0e6 / opflow.dev.rawData.deltaTime) / (float)opticalFlowConfig()->opflow_scale; - int32_t flowRateRaw[3] = { opflow.dev.rawData.flowRateRaw[X], opflow.dev.rawData.flowRateRaw[Y], 0 }; - applySensorAlignment(flowRateRaw, flowRateRaw, opticalFlowConfig()->opflow_align); - //applyBoardAlignment(flowRateRaw); + // 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(flowRateRaw[X] * integralToRateScaler); - opflow.flowRate[Y] = DEGREES_TO_RADIANS(flowRateRaw[Y] * integralToRateScaler); + 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); - DEBUG_SET(DEBUG_FLOW_RAW, 0, RADIANS_TO_DEGREES(opflow.flowRate[X])); - DEBUG_SET(DEBUG_FLOW_RAW, 1, RADIANS_TO_DEGREES(opflow.flowRate[Y])); + 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])); } diff --git a/src/main/sensors/opflow.h b/src/main/sensors/opflow.h index 4bd1ec8fcb..bbfced6470 100755 --- a/src/main/sensors/opflow.h +++ b/src/main/sensors/opflow.h @@ -43,8 +43,8 @@ typedef enum { typedef struct opticalFlowConfig_s { uint8_t opflow_hardware; - float opflow_scale; // Scaler value to convert between raw sensor units to [deg/s] uint8_t opflow_align; + float opflow_scale; // Scaler value to convert between raw sensor units to [deg/s] } opticalFlowConfig_t; PG_DECLARE(opticalFlowConfig_t, opticalFlowConfig); diff --git a/src/main/target/AIRBOTF4/target.h b/src/main/target/AIRBOTF4/target.h index e8c9d3bcbd..dd97eac146 100644 --- a/src/main/target/AIRBOTF4/target.h +++ b/src/main/target/AIRBOTF4/target.h @@ -58,6 +58,8 @@ #define MAG_I2C_BUS BUS_I2C2 #define MAG_HMC5883_ALIGN CW90_DEG #define USE_MAG_HMC5883 +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 #define USE_MAG_MAG3110 #define USE_MAG_QMC5883 #define USE_MAG_IST8310 diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index c86d677d0c..21518419ab 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -49,7 +49,7 @@ void targetConfiguration(void) { // alternative defaults settings for ALIENFLIGHTF1 and ALIENFLIGHTF3 targets serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL; - batteryConfigMutable()->voltage.scale = 200; + batteryMetersConfigMutable()->voltage_scale = 200; rxConfigMutable()->spektrum_sat_bind = 5; rxConfigMutable()->spektrum_sat_bind_autoreset = 1; diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 0a1077434c..b237b21b3f 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -70,6 +70,8 @@ #define MAG_I2C_BUS BUS_I2C2 #define USE_MAG_MPU9250 #define MAG_MPU9250_ALIGN CW180_DEG_FLIP +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 #define USE_MAG_HMC5883 #define USE_MAG_MAG3110 #define USE_MAG_QMC5883 diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index b7209d1c76..11be0fcc7f 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -56,8 +56,8 @@ void targetConfiguration(void) { serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL; - batteryConfigMutable()->current.offset = CURRENTOFFSET; - batteryConfigMutable()->current.scale = CURRENTSCALE; + batteryMetersConfigMutable()->current.offset = CURRENTOFFSET; + batteryMetersConfigMutable()->current.scale = CURRENTSCALE; if (hardwareMotorType == MOTOR_BRUSHED) { motorConfigMutable()->motorPwmProtocol = PWM_TYPE_BRUSHED; diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index 905616fd32..e15c1b7f22 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -58,8 +58,8 @@ void targetConfiguration(void) { serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL; - batteryConfigMutable()->current.offset = CURRENTOFFSET; - batteryConfigMutable()->current.scale = CURRENTSCALE; + batteryMetersConfigMutable()->current.offset = CURRENTOFFSET; + batteryMetersConfigMutable()->current.scale = CURRENTSCALE; if (hardwareMotorType == MOTOR_BRUSHED) { motorConfigMutable()->motorPwmRate = BRUSHED_MOTORS_PWM_RATE; diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index 5c42a5ec08..1d5d7bddc6 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -52,6 +52,8 @@ #define MAG_I2C_BUS BUS_I2C1 #define MAG_HMC5883_ALIGN CW90_DEG #define USE_MAG_HMC5883 +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 #define USE_MAG_QMC5883 #define USE_MAG_IST8310 #define USE_MAG_IST8308 diff --git a/src/main/target/BETAFLIGHTF3/config.c b/src/main/target/BETAFLIGHTF3/config.c index 08194ec9a7..479d4259d9 100755 --- a/src/main/target/BETAFLIGHTF3/config.c +++ b/src/main/target/BETAFLIGHTF3/config.c @@ -25,6 +25,6 @@ void targetConfiguration(void) { - batteryConfigMutable()->current.scale = 20; + batteryMetersConfigMutable()->current.scale = 20; } #endif diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 6296a8ac97..e27a2468c0 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -116,7 +116,7 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 -//#define LED_STRIP +//#define USE_LED_STRIP #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index 75d5a68e9e..4687719145 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -127,6 +127,8 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 //#define MAG_HMC5883_ALIGN CW90_DEG +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 #define USE_MAG_IST8310 diff --git a/src/main/target/CLRACINGF4AIR/CLRACINGF4AIRV3.mk b/src/main/target/CLRACINGF4AIR/CLRACINGF4AIRV3.mk new file mode 100644 index 0000000000..c604c0b302 --- /dev/null +++ b/src/main/target/CLRACINGF4AIR/CLRACINGF4AIRV3.mk @@ -0,0 +1 @@ +#CLRACINGF4AIRV3 \ No newline at end of file diff --git a/src/main/target/CLRACINGF4AIR/target.c b/src/main/target/CLRACINGF4AIR/target.c index a5e26d1159..4bb2314dec 100644 --- a/src/main/target/CLRACINGF4AIR/target.c +++ b/src/main/target/CLRACINGF4AIR/target.c @@ -23,20 +23,21 @@ const timerHardware_t timerHardware[] = { { TIM11, IO_TAG(PB9), TIM_Channel_1, 0, IOCFG_AF_PP, GPIO_AF_TIM11, TIM_USE_PPM | TIM_USE_PWM }, // PPM { TIM4, IO_TAG(PB8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_LED }, // LED_STRIP -#if defined(CLRACINGF4AIRV2) - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO 3 ELE - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO 4 ROLL 1 - { TIM12, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO 5 ROLL 1 - { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR 1 ESC - { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR 2 ESC - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO 6 YAW + +#if defined(CLRACINGF4AIRV2) || defined(CLRACINGF4AIRV3) + { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR 3 ELE + { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR 4 ROLL 1 + { TIM12, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR 5 ROLL 2 + { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR 1 ESC + { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR 2 ESC + { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // MOTOR 6 YAW #else - { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR_1 - { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR_2 - { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO_1 - { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO_2 - { TIM12, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO_3 - { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO_4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR_1 + { TIM3, IO_TAG(PB1), TIM_Channel_4, 1, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // MOTOR_2 + { TIM9, IO_TAG(PA3), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO_1 + { TIM2, IO_TAG(PA2), TIM_Channel_3, 1, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO_2 + { TIM12, IO_TAG(PB15), TIM_Channel_2, 1, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO_3 + { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // SERVO_4 #endif }; diff --git a/src/main/target/CLRACINGF4AIR/target.h b/src/main/target/CLRACINGF4AIR/target.h index 9da827678b..343a9e46ab 100644 --- a/src/main/target/CLRACINGF4AIR/target.h +++ b/src/main/target/CLRACINGF4AIR/target.h @@ -17,26 +17,33 @@ #if defined(CLRACINGF4AIRV2) #define TARGET_BOARD_IDENTIFIER "CLA2" #define USBD_PRODUCT_STRING "CLRACINGF4AIRV2" +#elif defined(CLRACINGF4AIRV3) +#define TARGET_BOARD_IDENTIFIER "CLA3" +#define USBD_PRODUCT_STRING "CLRACINGF4AIRV3" #else #define TARGET_BOARD_IDENTIFIER "CLRA" #define USBD_PRODUCT_STRING "CLRACINGF4AIR" #endif - +#if defined(CLRACINGF4AIRV3) +#define LED0 PC14 +#else #define LED0 PB5 +#endif #define BEEPER PB4 #define BEEPER_INVERTED #define INVERTER_PIN_UART1 PC0 // PC0 used as inverter select GPIO -// MPU-6000 GRYO -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_BUS BUS_SPI1 -#define USE_GYRO -#define USE_GYRO_MPU6000 -#define GYRO_MPU6000_ALIGN CW0_DEG -#define USE_ACC -#define USE_ACC_MPU6000 -#define ACC_MPU6000_ALIGN CW0_DEG - +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#if defined(CLRACINGF4AIRV3) +#define SPI3_MOSI_PIN PB5 +#else +#define SPI3_MOSI_PIN PC12 +#endif //MPU-9250 #define MPU9250_CS_PIN PA4 #define MPU9250_SPI_BUS BUS_SPI1 @@ -48,7 +55,6 @@ #define ACC_MPU9250_ALIGN CW0_DEG #define USE_MAG #define USE_MAG_MPU9250 - // MPU6 interrupts #define USE_EXTI #define GYRO_INT_EXTI PC4 @@ -68,7 +74,7 @@ #define USE_LED_STRIP #define WS2811_PIN PB8 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST7_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream7 +#define WS2811_DMA_STREAM DMA1_Stream7 #define WS2811_DMA_CHANNEL DMA_Channel_2 #define USE_VCP @@ -77,8 +83,7 @@ #define UART1_TX_PIN PA9 #define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 -//V2 add another uart 2 -#if defined( CLRACINGF4AIRV2 ) +#if defined( CLRACINGF4AIRV2) || defined(CLRACINGF4AIRV3) #define USE_UART2 #define UART2_RX_PIN PA2 #define UART2_TX_PIN PA3 @@ -86,65 +91,67 @@ #define USE_UART3 #define UART3_RX_PIN PB11 #define UART3_TX_PIN PB10 - #define USE_UART4 #define UART4_RX_PIN PA1 #define UART4_TX_PIN PA0 - #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 #if defined( CLRACINGF4AIRV2 ) #define SERIAL_PORT_COUNT 6 //VCP, USART1, UART2, USART3,USART4, USART6, +#elif defined(CLRACINGF4AIRV3) +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 +#define SERIAL_PORT_COUNT 7 //VCP, USART1, UART2, USART3,USART4, USART5,USART6, #else #define SERIAL_PORT_COUNT 5 //VCP, USART1, USART3,USART4, USART6, #endif - #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 - -#if defined( CLRACINGF4AIRV2 ) +#if defined( CLRACINGF4AIRV2 ) || defined(CLRACINGF4AIRV3) #define USE_I2C #define I2C_DEVICE (I2CDEV_2) #define I2C2_SCL PB10 #define I2C2_SDA PB11 #endif -#define USE_SPI -#define USE_SPI_DEVICE_1 - -#define USE_SPI_DEVICE_3 -#define SPI3_NSS_PIN PA15 -#define SPI3_SCK_PIN PC10 -#define SPI3_MISO_PIN PC11 -#define SPI3_MOSI_PIN PC12 - #define USE_ADC +#define ADC_INSTANCE ADC1 #define ADC_CHANNEL_1_PIN PC1 #define ADC_CHANNEL_2_PIN PC2 #define ADC_CHANNEL_3_PIN PC3 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + // V2 has airspeed input -#if defined( CLRACINGF4AIRV2 ) +#if defined( CLRACINGF4AIRV2 ) || defined(CLRACINGF4AIRV3) #define ADC_CHANNEL_4_PIN PC5 #define ADC_AIRSPEED_CHANNEL ADC_CHN_4 #define CURRENT_METER_SCALE 250 #endif -#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 -#define VBAT_ADC_CHANNEL ADC_CHN_2 -#define RSSI_ADC_CHANNEL ADC_CHN_3 #define USE_ESC_SENSOR #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY| FEATURE_VBAT | FEATURE_OSD ) #define SPEKTRUM_BIND_PIN UART1_RX_PIN #define USE_SERIAL_4WAY_BLHELI_INTERFACE - // Number of available PWM outputs #define MAX_PWM_OUTPUT_PORTS 8 #define TARGET_MOTOR_COUNT 8 -#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) -#define TARGET_IO_PORTB (0xffff & ~(BIT(2))) -#define TARGET_IO_PORTC (0xffff & ~(BIT(15)|BIT(14)|BIT(13))) +#define TARGET_IO_PORTA (0xffff) +#define TARGET_IO_PORTB (0xffff) +#define TARGET_IO_PORTC (0xffff) #define TARGET_IO_PORTD BIT(2) + +// V2 has airspeed input +#if defined( CLRACINGF4AIRV2 ) || defined(CLRACINGF4AIRV3) +#define USABLE_TIMER_CHANNEL_COUNT 8 +#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(8)| TIM_N(11) | TIM_N(12) ) +#else +#define USABLE_TIMER_CHANNEL_COUNT 8 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(9) | TIM_N(11) | TIM_N(12) ) +#endif diff --git a/src/main/target/CLRACINGF4AIR/target.mk b/src/main/target/CLRACINGF4AIR/target.mk index f18713beaf..f88b689f7e 100644 --- a/src/main/target/CLRACINGF4AIR/target.mk +++ b/src/main/target/CLRACINGF4AIR/target.mk @@ -7,15 +7,8 @@ TARGET_SRC = \ drivers/accgyro/accgyro_fake.c \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_mpu9250.c \ - drivers/accgyro/accgyro_mpu6000.c \ - drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ - drivers/barometer/barometer_ms56xx.c \ drivers/compass/compass_mpu9250.c \ - drivers/compass/compass_hmc5883l.c \ - drivers/compass/compass_qmc5883l.c \ - drivers/compass/compass_ist8310.c \ - drivers/compass/compass_mag3110.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stdperiph.c \ drivers/max7456.c diff --git a/src/main/target/DALRCF405/target.h b/src/main/target/DALRCF405/target.h index fe898641be..52b8241015 100644 --- a/src/main/target/DALRCF405/target.h +++ b/src/main/target/DALRCF405/target.h @@ -94,6 +94,8 @@ #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 diff --git a/src/main/target/FF_FORTINIF4/target.h b/src/main/target/FF_FORTINIF4/target.h index 48ecb4a1b7..084662d030 100644 --- a/src/main/target/FF_FORTINIF4/target.h +++ b/src/main/target/FF_FORTINIF4/target.h @@ -101,7 +101,7 @@ #define SERIAL_PORT_COUNT 6 -#define CMS +#define USE_CMS #define USE_MSP_DISPLAYPORT /*---------------------------------*/ @@ -130,7 +130,7 @@ /*---------------------------------*/ /*-----------LED Strip-------------*/ -#define LED_STRIP +#define USE_LED_STRIP #define WS2811_PIN PB7 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER #define WS2811_DMA_STREAM DMA1_Stream2 @@ -155,7 +155,7 @@ /*---------------------------------*/ /*--------SPEKTRUM BIND---------.--*/ -#define SPEKTRUM_BIND +#define USE_SPEKTRUM_BIND #define BIND_PIN UART3_RX_PIN /*---------------------------------*/ diff --git a/src/main/target/FF_PIKOF4/config.c b/src/main/target/FF_PIKOF4/config.c index 4f822e5800..513db9a597 100644 --- a/src/main/target/FF_PIKOF4/config.c +++ b/src/main/target/FF_PIKOF4/config.c @@ -33,6 +33,6 @@ void targetConfiguration(void) { rxConfigMutable()->halfDuplex = false; telemetryConfigMutable()->smartportUartUnidirectional = true; - batteryConfigMutable()->current.scale = CURRENTSCALE; + batteryMetersConfigMutable()->current.scale = CURRENTSCALE; } #endif diff --git a/src/main/target/FF_PIKOF4/target.c b/src/main/target/FF_PIKOF4/target.c index e267ee11b6..34083d8dc6 100644 --- a/src/main/target/FF_PIKOF4/target.c +++ b/src/main/target/FF_PIKOF4/target.c @@ -26,12 +26,12 @@ const timerHardware_t timerHardware[] = { #if defined(FF_PIKOF4OSD) - { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S2_OUT - { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // RC4 - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // RC5 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S1_OUT // TIM2_CH4 | TIM5_CH4 | TIM9_CH2 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S2_OUT // TIM1_CH3N | TIM3_CH4 | TIM8_CH3N + { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT // TIM2_CH3 | TIM5_CH3 | TIM9_CH1 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT // TIM1_CH2N | TIM3_CH3 | TIM8_CH2N + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // RC4 // TIM1_CH2N | TIM8_CH2N | TIM12_CH1 + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // RC5 // TIM1_CH3N | TIM8_CH3N | TIM12_CH2 // DEF_TIM(TIM5, CH4, PA3, TIM_USE_MOTOR, 0, 0 ), // S1_OUT - DMA1_ST7 // DEF_TIM(TIM3, CH3, PB1, TIM_USE_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST1 // DEF_TIM(TIM5, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA1_ST6 @@ -39,16 +39,16 @@ const timerHardware_t timerHardware[] = { // DEF_TIM(TIM8, CH2N, PB14, TIM_USE_MOTOR, 0, 0 ), // PA14 RC4 - DMA2_ST6, *DMA2_ST2 // DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MOTOR, 0, 0 ), // PA15 RC5 - DMA2_ST6, DMA2_ST6 #else - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S1_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S2_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, // S1_OUT // TIM2_CH4 | TIM5_CH4 | TIM9_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S2_OUT // TIM1_CH2N | TIM3_CH3 | TIM8_CH2N + { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S3_OUT // TIM2_CH3 | TIM5_CH3 | TIM9_CH1 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_EN, IOCFG_AF_PP, GPIO_AF_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, // S4_OUT // TIM1_CH3N | TIM3_CH4 | TIM8_CH3N // DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1 ), // S1_OUT - DMA1_ST6 // DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // S2_OUT - DMA1_ST7 // DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0 ), // S3_OUT - DMA1_ST1 // DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0 ), // S4_OUT - DMA1_ST2 #endif - { TIM4, IO_TAG(PB7), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_LED | TIM_USE_MC_SERVO }, // LED + { TIM4, IO_TAG(PB7), TIM_Channel_2, 0, IOCFG_AF_PP, GPIO_AF_TIM4, TIM_USE_LED | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, // LED // DEF_TIM(TIM4, CH2, PB7, TIM_USE_LED, 0, 0 ), // LED - DMA1_ST3 }; diff --git a/src/main/target/FF_PIKOF4/target.h b/src/main/target/FF_PIKOF4/target.h index 5987222273..89061bbfda 100644 --- a/src/main/target/FF_PIKOF4/target.h +++ b/src/main/target/FF_PIKOF4/target.h @@ -163,7 +163,7 @@ /*---------------------------------*/ /*-----------LED Strip-------------*/ -#define LED_STRIP +#define USE_LED_STRIP #define WS2811_PIN PB7 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER #define WS2811_DMA_STREAM DMA1_Stream2 diff --git a/src/main/target/FIREWORKSV2/target.h b/src/main/target/FIREWORKSV2/target.h index defa808758..2e7d5c13c5 100644 --- a/src/main/target/FIREWORKSV2/target.h +++ b/src/main/target/FIREWORKSV2/target.h @@ -54,6 +54,14 @@ #define GYRO_MPU6500_ALIGN CW180_DEG #define ACC_MPU6500_ALIGN CW180_DEG +// OmnibusF4 Nano v6 has a MPU6000 +#define USE_GYRO_MPU6000 +#define USE_ACC_MPU6000 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 +#define GYRO_MPU6000_ALIGN CW180_DEG +#define ACC_MPU6000_ALIGN CW180_DEG + #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 #define USE_MAG_HMC5883 diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index 10dc01a6a5..2d445fe0b8 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -33,7 +33,7 @@ const timerHardware_t timerHardware[] = { { TIM8, IO_TAG(PC8), TIM_Channel_3, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S3 DMA2_ST4 SV4 { TIM8, IO_TAG(PC9), TIM_Channel_4, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S4 DMA3_ST7 SV5 - { TIM2, IO_TAG(PA15), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_LED | TIM_USE_FW_SERVO }, //S5 DMA1_ST5 2812LED + { TIM2, IO_TAG(PA15), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2, TIM_USE_MC_MOTOR | TIM_USE_LED }, //S5 DMA1_ST5 2812LED #ifdef MATEKF405_SERVOS6 { TIM1, IO_TAG(PA8), TIM_Channel_1, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, //S6 DMA2_ST6 SV6 diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index 7f4c6f04d8..6287ee0cc8 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -159,6 +159,8 @@ #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 diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h index 0965d3d29f..aa0e054549 100644 --- a/src/main/target/MATEKF405SE/target.h +++ b/src/main/target/MATEKF405SE/target.h @@ -38,7 +38,7 @@ #define MPU6000_SPI_BUS BUS_SPI1 #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define GYRO_INT_EXTI PC4 #define USE_MPU_DATA_READY_SIGNAL #define USE_GYRO @@ -67,6 +67,8 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 #define USE_MAG_IST8310 diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h index 61ce42ab05..17877ca0eb 100755 --- a/src/main/target/MATEKF411/target.h +++ b/src/main/target/MATEKF411/target.h @@ -39,7 +39,7 @@ #define MPU6000_SPI_BUS BUS_SPI1 #define USE_EXTI -#define MPU_INT_EXTI PA1 +#define GYRO_INT_EXTI PA1 #define USE_MPU_DATA_READY_SIGNAL #define USE_GYRO diff --git a/src/main/target/MATEKF722/target.c b/src/main/target/MATEKF722/target.c index 2021af24b5..2453c5b10f 100755 --- a/src/main/target/MATEKF722/target.c +++ b/src/main/target/MATEKF722/target.c @@ -16,11 +16,13 @@ */ #include -#include + +#include "platform.h" + +#include "drivers/bus.h" #include "drivers/io.h" #include "drivers/pwm_mapping.h" #include "drivers/timer.h" -#include "drivers/bus.h" #define TIM_EN TIMER_OUTPUT_ENABLED #define TIM_EN_N TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL @@ -28,13 +30,13 @@ const timerHardware_t timerHardware[] = { { TIM5, IO_TAG(PA3), TIM_CHANNEL_4, 0, IOCFG_AF_PP_PD, GPIO_AF2_TIM5, TIM_USE_PPM }, - { TIM3, IO_TAG(PC6), TIM_CHANNEL_1, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, //S1 DMA1_ST4 MT1 - { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S2 DMA2_ST3 SV3 - { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S3 DMA2_ST4 SV4 - { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S4 DMA2_ST7 SV5 - { TIM3, IO_TAG(PB1), TIM_CHANNEL_4, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, //S5 DMA1_ST2 MT2 - { TIM1, IO_TAG(PA8), TIM_CHANNEL_1, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF1_TIM1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S6 DMA2_ST6 SV6 - { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM4, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, //S7 DMA1_ST7 + { TIM3, IO_TAG(PC6), TIM_CHANNEL_1, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, //S1 DMA1_ST4 MT1 + { TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S2 DMA2_ST3 SV3 + { TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S3 DMA2_ST4 SV4 + { TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF3_TIM8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO }, //S4 DMA2_ST7 SV5 + { TIM3, IO_TAG(PB1), TIM_CHANNEL_4, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR }, //S5 DMA1_ST2 MT2 + { TIM1, IO_TAG(PA8), TIM_CHANNEL_1, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF1_TIM1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, //S6 DMA2_ST6 SV6 + { TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM4, TIM_USE_MC_CHNFW | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO }, //S7 DMA1_ST7 //{ TIM5, IO_TAG(PA2), TIM_CHANNEL_3, TIM_EN, IOCFG_AF_PP_PD, GPIO_AF2_TIM5, TIM_USE_ANY }, diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h index 99e2fcc4a2..a99d2ad055 100755 --- a/src/main/target/MATEKF722/target.h +++ b/src/main/target/MATEKF722/target.h @@ -70,6 +70,9 @@ #define USE_MAG_IST8308 #define USE_MAG_MAG3110 +#define USE_PITOT_MS4525 +#define PITOT_I2C_BUS BUS_I2C1 + // *************** SD Card ************************** #define USE_SDCARD #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/MATEKF722/target.mk b/src/main/target/MATEKF722/target.mk index 29562df66c..fe040f61fc 100755 --- a/src/main/target/MATEKF722/target.mk +++ b/src/main/target/MATEKF722/target.mk @@ -13,5 +13,7 @@ TARGET_SRC = \ drivers/compass/compass_mag3110.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_hal.c \ - drivers/max7456.c + drivers/max7456.c \ + drivers/pitotmeter_ms4525.c \ + drivers/pitotmeter_adc.c \ diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 2ac40b1890..dec6832476 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -51,11 +51,6 @@ #define BMP280_SPI_BUS BUS_SPI1 #define BMP280_CS_PIN PA13 -#define BARO_I2C_BUS BUS_I2C1 -#define USE_BARO_BMP085 // External -#define USE_BARO_BMP180 // External -#define USE_BARO_MS5611 // External - #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG_HMC5883 @@ -64,13 +59,6 @@ #define USE_MAG_IST8308 #define USE_MAG_MAG3110 -#define USE_RANGEFINDER -#define USE_RANGEFINDER_HCSR04 -#define RANGEFINDER_HCSR04_ECHO_PIN PB2 // Has 1K series resistor -#define RANGEFINDER_HCSR04_TRIGGER_PIN PB4 // FT -#define USE_RANGEFINDER_HCSR04_I2C -#define RANGEFINDER_I2C_BUS BUS_I2C1 - #define USB_CABLE_DETECTION #define USB_DETECT_PIN PB5 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 6060856e29..b9d750d978 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -105,6 +105,11 @@ #define USE_BARO_BMP280 #define BMP280_SPI_BUS BUS_SPI3 #define BMP280_CS_PIN PB3 // v1 + + // Support external barometers + #define BARO_I2C_BUS BUS_I2C2 + #define USE_BARO_BMP085 + #define USE_BARO_MS5611 #else #define BARO_I2C_BUS BUS_I2C2 #define USE_BARO_BMP085 @@ -118,6 +123,10 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER_HCSR04_I2C +#define USE_RANGEFINDER_VL53L0X + +#define USE_OPTICAL_FLOW +#define USE_OPFLOW_CXOF #define USE_VCP #define VBUS_SENSING_PIN PC5 @@ -137,7 +146,11 @@ #define UART6_TX_PIN PC6 #if defined(OMNIBUSF4V3) -#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 +#define USE_SOFTSERIAL1 +#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 USE_SOFTSERIAL1 #define SOFTSERIAL_1_RX_PIN PC8 diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 950904d433..a2cc4cbafe 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -116,7 +116,7 @@ #define VBAT_ADC_CHANNEL ADC_CHN_2 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 -#define LED_STRIP +#define USE_LED_STRIP #define LED_STRIP_TIMER TIM5 #define USE_RANGEFINDER diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h index a273f79497..2241f4e4c6 100755 --- a/src/main/target/SPRACINGF3NEO/target.h +++ b/src/main/target/SPRACINGF3NEO/target.h @@ -112,11 +112,11 @@ #define SPI3_MISO_PIN PB4 #define SPI3_MOSI_PIN PB5 -#define VTX_RTC6705 +#define USE_VTX_RTC6705 #define VTX_RTC6705_OPTIONAL // VTX/OSD board is OPTIONAL -#undef VTX_SMARTAUDIO // Disabled due to flash size -#undef VTX_TRAMP // Disabled due to flash size +#undef USE_VTX_SMARTAUDIO // Disabled due to flash size +#undef USE_VTX_TRAMP // Disabled due to flash size #define RTC6705_CS_PIN PF4 #define RTC6705_SPI_INSTANCE SPI3 @@ -153,15 +153,10 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 - -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define USE_LED_STRIP #define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_STREAM DMA1_Channel2 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -#define WS2811_TIMER_GPIO_AF GPIO_AF_6 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/SPRACINGF3NEO/target.mk b/src/main/target/SPRACINGF3NEO/target.mk index 363533403d..ff21e040f4 100755 --- a/src/main/target/SPRACINGF3NEO/target.mk +++ b/src/main/target/SPRACINGF3NEO/target.mk @@ -13,5 +13,7 @@ TARGET_SRC = \ drivers/compass/compass_ist8310.c \ drivers/compass/compass_ist8308.c \ drivers/compass/compass_mag3110.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stdperiph.c \ drivers/max7456.c \ drivers/vtx_rtc6705.c diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index 69a3b93972..b33946b76e 100755 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -122,7 +122,7 @@ #define SPI3_MISO_PIN PB4 // NC #define SPI3_MOSI_PIN PB5 // NC -#define VTX_RTC6705 +#define USE_VTX_RTC6705 #define VTX_RTC6705_OPTIONAL // SPI3 on an F4 EVO may be used for RTC6705 VTX control. #define RTC6705_CS_PIN SPI3_NSS_PIN diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index d563ebe6ac..b77a25577c 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -152,7 +152,7 @@ #define RSSI_ADC_CHANNEL ADC_CHN_2 // LED Strip can run off Pin 5 (PB1) of the motor outputs -#define LED_STRIP +#define USE_LED_STRIP #define WS2811_PIN PB1 #define WS2811_TIMER TIM8 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER diff --git a/src/main/target/common.h b/src/main/target/common.h index 97d92fcf5c..298fa00dd6 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -58,6 +58,7 @@ #endif #if (FLASH_SIZE > 256) +#define USE_EXTENDED_CMS_MENUS #define USE_UAV_INTERCONNECT #define USE_RX_UIB #endif @@ -111,10 +112,10 @@ #define USE_PITOT_ADC //Enable VTX controll -#define VTX_COMMON -#define VTX_CONTROL -#define VTX_SMARTAUDIO -#define VTX_TRAMP +#define USE_VTX_COMMON +#define USE_VTX_CONTROL +#define USE_VTX_SMARTAUDIO +#define USE_VTX_TRAMP //Enable DST calculations #define RTC_AUTOMATIC_DST diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index ecf2eaf240..5bbcd4fe52 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -144,6 +144,17 @@ #endif #endif +#if defined(USE_MAG_AK8975) + #if defined(AK8975_SPI_BUS) + BUSDEV_REGISTER_SPI(busdev_ak8975, DEVHW_AK8975, AK8975_SPI_BUS, AK8975_CS_PIN, NONE, DEVFLAGS_NONE); + #elif defined(AK8975_I2C_BUS) || defined(MAG_I2C_BUS) + #if !defined(AK8975_I2C_BUS) + #define AK8975_I2C_BUS MAG_I2C_BUS + #endif + BUSDEV_REGISTER_I2C(busdev_ak8975, DEVHW_AK8975, AK8975_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE); + #endif +#endif + #if defined(USE_MAG_MAG3110) #if !defined(MAG3110_I2C_BUS) #define MAG3110_I2C_BUS MAG_I2C_BUS diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index fe285d989c..58a1f4fc4f 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -21,7 +21,7 @@ // Targets with built-in vtx do not need external vtx #if defined(VTX) || defined(USE_RTC6705) -# undef VTX_CONTROL +# undef USE_VTX_CONTROL #endif // Backward compatibility for I2C OLED display diff --git a/src/main/target/link/stm32_flash_f303_256k.ld b/src/main/target/link/stm32_flash_f303_256k.ld index cc033b846a..1917843495 100644 --- a/src/main/target/link/stm32_flash_f303_256k.ld +++ b/src/main/target/link/stm32_flash_f303_256k.ld @@ -16,8 +16,8 @@ _Min_Stack_Size = 0x1800; /* Specify the memory areas. */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 252K - FLASH_CONFIG (r) : ORIGIN = 0x0803F000, LENGTH = 4K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 250K + FLASH_CONFIG (r) : ORIGIN = 0x0803E800, LENGTH = 6K RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 8K diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index a608f7cf5f..070fa3a702 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -1,548 +1,87 @@ /* - * 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 . + * frsky.c + * Common functions for FrSky D-series and SmartPort telemetry */ -/* - * Initial FrSky Telemetry implementation by silpstream @ rcgroups. - * Addition protocol work by airmamaf @ github. - */ #include #include -#include -#include #include "platform.h" -#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_FRSKY) +#if defined(USE_TELEMETRY) && (defined(USE_TELEMETRY_FRSKY) || defined(USE_TELEMETRY_SMARTPORT)) #include "common/maths.h" -#include "common/axis.h" - -#include "config/feature.h" - -#include "drivers/time.h" -#include "drivers/serial.h" - -#include "fc/rc_controls.h" #include "fc/runtime_config.h" - -#include "fc/config.h" - -#include "flight/mixer.h" -#include "flight/pid.h" -#include "flight/imu.h" - +#include "fc/rc_modes.h" #include "io/gps.h" -#include "io/serial.h" - -#include "navigation/navigation.h" - -#include "sensors/acceleration.h" -#include "sensors/barometer.h" -#include "sensors/battery.h" -#include "sensors/gyro.h" -#include "sensors/sensors.h" - -#include "telemetry/telemetry.h" #include "telemetry/frsky.h" -static serialPort_t *frskyPort = NULL; -static serialPortConfig_t *portConfig; - -#define FRSKY_BAUDRATE 9600 -#define FRSKY_INITIAL_PORT_MODE MODE_TX - -static bool frskyTelemetryEnabled = false; -static portSharing_e frskyPortSharing; - -#define CYCLETIME 125 - -#define PROTOCOL_HEADER 0x5E -#define PROTOCOL_TAIL 0x5E - -// Data Ids (bp = before decimal point; af = after decimal point) -// Official data IDs -#define ID_GPS_ALTIDUTE_BP 0x01 -#define ID_GPS_ALTIDUTE_AP 0x09 -#define ID_TEMPRATURE1 0x02 -#define ID_RPM 0x03 -#define ID_FUEL_LEVEL 0x04 -#define ID_TEMPRATURE2 0x05 -#define ID_VOLT 0x06 -#define ID_ALTITUDE_BP 0x10 -#define ID_ALTITUDE_AP 0x21 -#define ID_GPS_SPEED_BP 0x11 -#define ID_GPS_SPEED_AP 0x19 -#define ID_LONGITUDE_BP 0x12 -#define ID_LONGITUDE_AP 0x1A -#define ID_E_W 0x22 -#define ID_LATITUDE_BP 0x13 -#define ID_LATITUDE_AP 0x1B -#define ID_N_S 0x23 -#define ID_COURSE_BP 0x14 -#define ID_COURSE_AP 0x1C -#define ID_DATE_MONTH 0x15 -#define ID_YEAR 0x16 -#define ID_HOUR_MINUTE 0x17 -#define ID_SECOND 0x18 -#define ID_ACC_X 0x24 -#define ID_ACC_Y 0x25 -#define ID_ACC_Z 0x26 -#define ID_VOLTAGE_AMP 0x39 -#define ID_VOLTAGE_AMP_BP 0x3A -#define ID_VOLTAGE_AMP_AP 0x3B -#define ID_CURRENT 0x28 -// User defined data IDs -#define ID_GYRO_X 0x40 -#define ID_GYRO_Y 0x41 -#define ID_GYRO_Z 0x42 - -#define ID_VERT_SPEED 0x30 //opentx vario - -#define GPS_BAD_QUALITY 300 -#define GPS_MAX_HDOP_VAL 9999 -#define DELAY_FOR_BARO_INITIALISATION (5 * 1000) //5s -#define BLADE_NUMBER_DIVIDER 5 // should set 12 blades in Taranis - -static uint32_t lastCycleTime = 0; -static uint8_t cycleNum = 0; -static void sendDataHead(uint8_t id) +uint16_t frskyGetFlightMode(void) { - serialWrite(frskyPort, PROTOCOL_HEADER); - serialWrite(frskyPort, id); + uint16_t tmpi = 0; + + // ones column + if (!isArmingDisabled()) + tmpi += 1; + else + tmpi += 2; + if (ARMING_FLAG(ARMED)) + tmpi += 4; + + // tens column + if (FLIGHT_MODE(ANGLE_MODE)) + tmpi += 10; + if (FLIGHT_MODE(HORIZON_MODE)) + tmpi += 20; + if (FLIGHT_MODE(MANUAL_MODE)) + tmpi += 40; + + // hundreds column + if (FLIGHT_MODE(HEADING_MODE)) + tmpi += 100; + if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) + tmpi += 200; + if (FLIGHT_MODE(NAV_POSHOLD_MODE)) + tmpi += 400; + + // thousands column + if (FLIGHT_MODE(NAV_RTH_MODE)) + tmpi += 1000; + if (FLIGHT_MODE(NAV_WP_MODE)) + tmpi += 2000; + if (FLIGHT_MODE(HEADFREE_MODE)) + tmpi += 4000; + + // ten thousands column + if (FLIGHT_MODE(FLAPERON)) + tmpi += 10000; + if (FLIGHT_MODE(FAILSAFE_MODE)) + tmpi += 40000; + else if (FLIGHT_MODE(AUTO_TUNE)) // intentionally reverse order and 'else-if' to prevent 16-bit overflow + tmpi += 20000; + + return tmpi; } -static void sendTelemetryTail(void) +uint16_t frskyGetGPSState(void) { - serialWrite(frskyPort, PROTOCOL_TAIL); -} + uint16_t tmpi = 0; -static void serializeFrsky(uint8_t data) -{ - // take care of byte stuffing - if (data == 0x5e) { - serialWrite(frskyPort, 0x5d); - serialWrite(frskyPort, 0x3e); - } else if (data == 0x5d) { - serialWrite(frskyPort, 0x5d); - serialWrite(frskyPort, 0x3d); - } else - serialWrite(frskyPort, data); -} - -static void serialize16(int16_t a) -{ - uint8_t t; - t = a; - serializeFrsky(t); - t = a >> 8 & 0xff; - serializeFrsky(t); -} - -static void sendAccel(void) -{ - int i; - - for (i = 0; i < 3; i++) { - sendDataHead(ID_ACC_X + i); - serialize16(lrintf(acc.accADCf[i] * 1000)); - } -} - -static void sendBaro(void) -{ - const int32_t alt = getEstimatedActualPosition(Z); - sendDataHead(ID_ALTITUDE_BP); - serialize16(alt / 100); - sendDataHead(ID_ALTITUDE_AP); - serialize16(ABS(alt % 100)); -} - -#ifdef USE_GPS -static void sendGpsAltitude(void) -{ - uint16_t altitude = gpsSol.llh.alt / 100; // meters - //Send real GPS altitude only if it's reliable (there's a GPS fix) - if (!STATE(GPS_FIX)) { - altitude = 0; - } - sendDataHead(ID_GPS_ALTIDUTE_BP); - serialize16(altitude); - sendDataHead(ID_GPS_ALTIDUTE_AP); - serialize16(0); -} -#endif - -static void sendThrottleOrBatterySizeAsRpm(void) -{ - uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; - sendDataHead(ID_RPM); - if (ARMING_FLAG(ARMED)) { - const throttleStatus_e throttleStatus = calculateThrottleStatus(); - if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) - throttleForRPM = 0; - serialize16(throttleForRPM); - } else { - serialize16((batteryConfig()->capacity.value / BLADE_NUMBER_DIVIDER)); - } - -} - -static void sendTemperature1(void) -{ - sendDataHead(ID_TEMPRATURE1); -#ifdef USE_BARO - serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf -#else - /* - * There is no temperature information, so send 0 - */ - serialize16(0); -#endif -} - -#ifdef USE_GPS -static void sendSatalliteSignalQualityAsTemperature2(void) -{ - uint16_t satellite = gpsSol.numSat; - if (gpsSol.hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s - satellite = constrain(gpsSol.hdop, 0, GPS_MAX_HDOP_VAL); - } - sendDataHead(ID_TEMPRATURE2); - serialize16(satellite); -} - -static void sendSpeed(void) -{ - if (!STATE(GPS_FIX)) { - return; - } - //Speed should be sent in knots (GPS speed is in cm/s) - sendDataHead(ID_GPS_SPEED_BP); - //convert to knots: 1cm/s = 0.0194384449 knots - serialize16(gpsSol.groundSpeed * 1944 / 100000); - sendDataHead(ID_GPS_SPEED_AP); - serialize16((gpsSol.groundSpeed * 1944 / 100) % 100); -} -#endif - -static void sendTime(void) -{ - uint32_t seconds = millis() / 1000; - uint8_t minutes = (seconds / 60) % 60; - - // if we fly for more than an hour, something's wrong anyway - sendDataHead(ID_HOUR_MINUTE); - serialize16(minutes << 8); - sendDataHead(ID_SECOND); - serialize16(seconds % 60); -} - -// Frsky pdf: dddmm.mmmm -// .mmmm is returned in decimal fraction of minutes. -static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) -{ - int32_t absgps, deg, min; - absgps = ABS(mwiigps); - deg = absgps / GPS_DEGREES_DIVIDER; - absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7 - min = absgps / GPS_DEGREES_DIVIDER; // minutes left - - if (telemetryConfig()->frsky_coordinate_format == FRSKY_FORMAT_DMS) { - result->dddmm = deg * 100 + min; - } else { - result->dddmm = deg * 60 + min; - } - - result->mmmm = (absgps - min * GPS_DEGREES_DIVIDER) / 1000; -} - -static void sendLatLong(int32_t coord[2]) -{ - gpsCoordinateDDDMMmmmm_t coordinate; - GPStoDDDMM_MMMM(coord[LAT], &coordinate); - sendDataHead(ID_LATITUDE_BP); - serialize16(coordinate.dddmm); - sendDataHead(ID_LATITUDE_AP); - serialize16(coordinate.mmmm); - sendDataHead(ID_N_S); - serialize16(coord[LAT] < 0 ? 'S' : 'N'); - - GPStoDDDMM_MMMM(coord[LON], &coordinate); - sendDataHead(ID_LONGITUDE_BP); - serialize16(coordinate.dddmm); - sendDataHead(ID_LONGITUDE_AP); - serialize16(coordinate.mmmm); - sendDataHead(ID_E_W); - serialize16(coord[LON] < 0 ? 'W' : 'E'); -} - -#ifdef USE_GPS -static void sendFakeLatLong(void) -{ - // Heading is only displayed on OpenTX if non-zero lat/long is also sent - int32_t coord[2] = {0,0}; - - coord[LAT] = (telemetryConfig()->gpsNoFixLatitude * GPS_DEGREES_DIVIDER); - coord[LON] = (telemetryConfig()->gpsNoFixLongitude * GPS_DEGREES_DIVIDER); - - sendLatLong(coord); -} -#endif - -static void sendFakeLatLongThatAllowsHeadingDisplay(void) -{ - // Heading is only displayed on OpenTX if non-zero lat/long is also sent - int32_t coord[2] = { - 1 * GPS_DEGREES_DIVIDER, - 1 * GPS_DEGREES_DIVIDER - }; - - sendLatLong(coord); -} - -#ifdef USE_GPS -static void sendGPSLatLong(void) -{ - static uint8_t gpsFixOccured = 0; - int32_t coord[2] = {0,0}; - - if (STATE(GPS_FIX) || gpsFixOccured == 1) { - // If we have ever had a fix, send the last known lat/long - gpsFixOccured = 1; - coord[LAT] = gpsSol.llh.lat; - coord[LON] = gpsSol.llh.lon; - sendLatLong(coord); - } else { - // otherwise send fake lat/long in order to display compass value - sendFakeLatLong(); - } -} -#endif - -/* - * Send vertical speed for opentx. ID_VERT_SPEED - * Unit is cm/s - */ -static void sendVario(void) -{ - sendDataHead(ID_VERT_SPEED); - serialize16((int16_t)lrintf(getEstimatedActualVelocity(Z))); -} - -/* - * Send voltage via ID_VOLT - * - * NOTE: This sends voltage divided by batteryCellCount. To get the real - * battery voltage, you need to multiply the value by batteryCellCount. - */ -static void sendVoltage(void) -{ - uint32_t cellVoltage; - uint16_t payload; - - /* - * Format for Voltage Data for single cells is like this: - * - * llll llll cccc hhhh - * l: Low voltage bits - * h: High voltage bits - * c: Cell number (starting at 0) - * - * The actual value sent for cell voltage has resolution of 0.002 volts - * Since vbat has resolution of 0.01 volts it has to be multiplied by 5 - */ - cellVoltage = ((uint32_t)getBatteryVoltage() * 10) / (getBatteryCellCount() * 2); - - // Cell number is at bit 9-12 (only uses vbat, so it can't send individual cell voltages, set cell number to 0) - payload = 0; - - // Lower voltage bits are at bit 0-8 - payload |= ((cellVoltage & 0x0ff) << 8); - - // Higher voltage bits are at bits 13-15 - payload |= ((cellVoltage & 0xf00) >> 8); - - sendDataHead(ID_VOLT); - serialize16(payload); -} - -/* - * Send voltage with ID_VOLTAGE_AMP - */ -static void sendVoltageAmp(void) -{ - uint16_t vbat = getBatteryVoltage(); - if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) { - /* - * Use new ID 0x39 to send voltage directly in 0.1 volts resolution - */ - sendDataHead(ID_VOLTAGE_AMP); - serialize16(vbat / 10); - } else { - uint16_t voltage = (vbat * 11) / 21; - uint16_t vfasVoltage; - if (telemetryConfig()->report_cell_voltage) { - vfasVoltage = voltage / getBatteryCellCount(); - } else { - vfasVoltage = voltage; - } - sendDataHead(ID_VOLTAGE_AMP_BP); - serialize16(vfasVoltage / 100); - sendDataHead(ID_VOLTAGE_AMP_AP); - serialize16(((vfasVoltage % 100) + 5) / 10); - } -} - -static void sendAmperage(void) -{ - sendDataHead(ID_CURRENT); - serialize16((uint16_t)(getAmperage() / 10)); -} - -static void sendFuelLevel(void) -{ - sendDataHead(ID_FUEL_LEVEL); - - serialize16((uint16_t)calculateBatteryPercentage()); -} - -static void sendHeading(void) -{ - sendDataHead(ID_COURSE_BP); - serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); - sendDataHead(ID_COURSE_AP); - serialize16(0); -} - -void initFrSkyTelemetry(void) -{ - portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY); - frskyPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY); -} - -void freeFrSkyTelemetryPort(void) -{ - closeSerialPort(frskyPort); - frskyPort = NULL; - frskyTelemetryEnabled = false; -} - -void configureFrSkyTelemetryPort(void) -{ - if (!portConfig) { - return; - } - - frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED); - if (!frskyPort) { - return; - } - - frskyTelemetryEnabled = true; -} - -bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis) -{ - return currentMillis - lastCycleTime >= CYCLETIME; -} - -void checkFrSkyTelemetryState(void) -{ - if (portConfig && telemetryCheckRxPortShared(portConfig)) { - if (!frskyTelemetryEnabled && telemetrySharedPort != NULL) { - frskyPort = telemetrySharedPort; - frskyTelemetryEnabled = true; - } - } else { - bool newTelemetryEnabledValue = telemetryDetermineEnabledState(frskyPortSharing); - - if (newTelemetryEnabledValue == frskyTelemetryEnabled) { - return; - } - - if (newTelemetryEnabledValue) - configureFrSkyTelemetryPort(); - else - freeFrSkyTelemetryPort(); - } -} - -void handleFrSkyTelemetry(void) -{ - if (!frskyTelemetryEnabled) { - return; - } - - uint32_t now = millis(); - - if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) { - return; - } - - lastCycleTime = now; - - cycleNum++; - - // Sent every 125ms - sendAccel(); - sendVario(); - sendTelemetryTail(); - - if ((cycleNum % 4) == 0) { // Sent every 500ms - if (lastCycleTime > DELAY_FOR_BARO_INITIALISATION) { //Allow 5s to boot correctly - sendBaro(); - } - sendHeading(); - sendTelemetryTail(); - } - - if ((cycleNum % 8) == 0) { // Sent every 1s - sendTemperature1(); - sendThrottleOrBatterySizeAsRpm(); - - if (feature(FEATURE_VBAT)) { - sendVoltage(); - sendVoltageAmp(); - sendAmperage(); - sendFuelLevel(); - } - -#ifdef USE_GPS - if (sensors(SENSOR_GPS)) { - sendSpeed(); - sendGpsAltitude(); - sendSatalliteSignalQualityAsTemperature2(); - sendGPSLatLong(); - } - else { - sendFakeLatLongThatAllowsHeadingDisplay(); - } -#else - sendFakeLatLongThatAllowsHeadingDisplay(); -#endif - - sendTelemetryTail(); - } - - if (cycleNum == 40) { //Frame 3: Sent every 5s - cycleNum = 0; - sendTime(); - sendTelemetryTail(); - } + // ones and tens columns (# of satellites 0 - 99) + tmpi += constrain(gpsSol.numSat, 0, 99); + + // hundreds column (satellite accuracy HDOP: 0 = worst [HDOP 6.5], 9 = best [HDOP 2.0]) + tmpi += (9 - constrain((gpsSol.hdop - 151) / 50, 0, 9)) * 100; + + // thousands column (GPS fix status) + if (STATE(GPS_FIX)) + tmpi += 1000; + if (STATE(GPS_FIX_HOME)) + tmpi += 2000; + if (ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) + tmpi += 4000; + + return tmpi; } #endif diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h index fe4846d011..9d0e71b482 100644 --- a/src/main/telemetry/frsky.h +++ b/src/main/telemetry/frsky.h @@ -1,30 +1,26 @@ /* - * This file is part of Cleanflight. + * This file is part of INAV. * - * Cleanflight is free software: you can redistribute it and/or modify + * 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. * - * Cleanflight is distributed in the hope that it will be useful, + * 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 Cleanflight. If not, see . + * along with INAV. If not, see . + */ + +/* + * frsky.h + * Common functions for FrSky D-series and SmartPort telemetry */ #pragma once -typedef enum { - FRSKY_VFAS_PRECISION_LOW = 0, - FRSKY_VFAS_PRECISION_HIGH -} frskyVFasPrecision_e; - -void handleFrSkyTelemetry(void); -void checkFrSkyTelemetryState(void); - -void initFrSkyTelemetry(void); -void configureFrSkyTelemetryPort(void); -void freeFrSkyTelemetryPort(void); +uint16_t frskyGetFlightMode(void); +uint16_t frskyGetGPSState(void); diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c new file mode 100644 index 0000000000..ed6860b897 --- /dev/null +++ b/src/main/telemetry/frsky_d.c @@ -0,0 +1,551 @@ +/* + * 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 . + */ + +/* + * Initial FrSky Telemetry implementation by silpstream @ rcgroups. + * Addition protocol work by airmamaf @ github. + */ +#include +#include +#include +#include + +#include "platform.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_FRSKY) + +#include "common/maths.h" +#include "common/axis.h" + +#include "config/feature.h" + +#include "drivers/time.h" +#include "drivers/serial.h" + +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "fc/config.h" +#include "fc/rc_modes.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" + +#include "io/gps.h" +#include "io/serial.h" + +#include "navigation/navigation.h" + +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" +#include "sensors/gyro.h" +#include "sensors/sensors.h" + +#include "telemetry/telemetry.h" +#include "telemetry/frsky_d.h" +#include "telemetry/frsky.h" + +static serialPort_t *frskyPort = NULL; +static serialPortConfig_t *portConfig; + +#define FRSKY_BAUDRATE 9600 +#define FRSKY_INITIAL_PORT_MODE MODE_TX + +static bool frskyTelemetryEnabled = false; +static portSharing_e frskyPortSharing; + +#define CYCLETIME 125 + +#define PROTOCOL_HEADER 0x5E +#define PROTOCOL_TAIL 0x5E + +// Data Ids (bp = before decimal point; af = after decimal point) +// Official data IDs +#define ID_GPS_ALTIDUTE_BP 0x01 +#define ID_GPS_ALTIDUTE_AP 0x09 +#define ID_TEMPRATURE1 0x02 +#define ID_RPM 0x03 +#define ID_FUEL_LEVEL 0x04 +#define ID_TEMPRATURE2 0x05 +#define ID_VOLT 0x06 +#define ID_ALTITUDE_BP 0x10 +#define ID_ALTITUDE_AP 0x21 +#define ID_GPS_SPEED_BP 0x11 +#define ID_GPS_SPEED_AP 0x19 +#define ID_LONGITUDE_BP 0x12 +#define ID_LONGITUDE_AP 0x1A +#define ID_E_W 0x22 +#define ID_LATITUDE_BP 0x13 +#define ID_LATITUDE_AP 0x1B +#define ID_N_S 0x23 +#define ID_COURSE_BP 0x14 +#define ID_COURSE_AP 0x1C +#define ID_DATE_MONTH 0x15 +#define ID_YEAR 0x16 +#define ID_HOUR_MINUTE 0x17 +#define ID_SECOND 0x18 +#define ID_ACC_X 0x24 +#define ID_ACC_Y 0x25 +#define ID_ACC_Z 0x26 +#define ID_VOLTAGE_AMP 0x39 +#define ID_VOLTAGE_AMP_BP 0x3A +#define ID_VOLTAGE_AMP_AP 0x3B +#define ID_CURRENT 0x28 +// User defined data IDs +#define ID_GYRO_X 0x40 +#define ID_GYRO_Y 0x41 +#define ID_GYRO_Z 0x42 +#define ID_HOME_DIST 0x07 + +#define ID_VERT_SPEED 0x30 //opentx vario + +#define GPS_BAD_QUALITY 300 +#define GPS_MAX_HDOP_VAL 9999 +#define DELAY_FOR_BARO_INITIALISATION (5 * 1000) //5s +#define BLADE_NUMBER_DIVIDER 5 // should set 12 blades in Taranis + +static uint32_t lastCycleTime = 0; +static uint8_t cycleNum = 0; +static void sendDataHead(uint8_t id) +{ + serialWrite(frskyPort, PROTOCOL_HEADER); + serialWrite(frskyPort, id); +} + +static void sendTelemetryTail(void) +{ + serialWrite(frskyPort, PROTOCOL_TAIL); +} + +static void serializeFrsky(uint8_t data) +{ + // take care of byte stuffing + if (data == 0x5e) { + serialWrite(frskyPort, 0x5d); + serialWrite(frskyPort, 0x3e); + } else if (data == 0x5d) { + serialWrite(frskyPort, 0x5d); + serialWrite(frskyPort, 0x3d); + } else + serialWrite(frskyPort, data); +} + +static void serialize16(int16_t a) +{ + uint8_t t; + t = a; + serializeFrsky(t); + t = a >> 8 & 0xff; + serializeFrsky(t); +} + +static void sendAccel(void) +{ + int i; + + for (i = 0; i < 3; i++) { + sendDataHead(ID_ACC_X + i); + serialize16(lrintf(acc.accADCf[i] * 1000)); + } +} + +static void sendBaro(void) +{ + const int32_t alt = getEstimatedActualPosition(Z); + sendDataHead(ID_ALTITUDE_BP); + serialize16(alt / 100); + sendDataHead(ID_ALTITUDE_AP); + serialize16(ABS(alt % 100)); +} + +#ifdef USE_GPS +static void sendGpsAltitude(void) +{ + uint16_t altitude = gpsSol.llh.alt / 100; // meters + //Send real GPS altitude only if it's reliable (there's a GPS fix) + if (!STATE(GPS_FIX)) { + altitude = 0; + } + sendDataHead(ID_GPS_ALTIDUTE_BP); + serialize16(altitude); + sendDataHead(ID_GPS_ALTIDUTE_AP); + serialize16(0); +} +#endif + +static void sendThrottleOrBatterySizeAsRpm(void) +{ + uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; + sendDataHead(ID_RPM); + if (ARMING_FLAG(ARMED)) { + const throttleStatus_e throttleStatus = calculateThrottleStatus(); + if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) + throttleForRPM = 0; + serialize16(throttleForRPM); + } else { + serialize16((currentBatteryProfile->capacity.value / BLADE_NUMBER_DIVIDER)); + } + +} + +static void sendFlightModeAsTemperature1(void) +{ + sendDataHead(ID_TEMPRATURE1); + serialize16(frskyGetFlightMode()); +} + +#ifdef USE_GPS +static void sendSatalliteSignalQualityAsTemperature2(void) +{ + sendDataHead(ID_TEMPRATURE2); + serialize16(frskyGetGPSState()); +} + +static void sendSpeed(void) +{ + if (!STATE(GPS_FIX)) { + return; + } + //Speed should be sent in knots (GPS speed is in cm/s) + sendDataHead(ID_GPS_SPEED_BP); + //convert to knots: 1cm/s = 0.0194384449 knots + serialize16(gpsSol.groundSpeed * 1944 / 100000); + sendDataHead(ID_GPS_SPEED_AP); + serialize16((gpsSol.groundSpeed * 1944 / 100) % 100); +} + +static void sendHomeDistance(void) +{ + if (!STATE(GPS_FIX)) { + return; + } + sendDataHead(ID_HOME_DIST); + serialize16(GPS_distanceToHome); +} + +#endif + +static void sendTime(void) +{ + uint32_t seconds = millis() / 1000; + uint8_t minutes = (seconds / 60) % 60; + + // if we fly for more than an hour, something's wrong anyway + sendDataHead(ID_HOUR_MINUTE); + serialize16(minutes << 8); + sendDataHead(ID_SECOND); + serialize16(seconds % 60); +} + +// Frsky pdf: dddmm.mmmm +// .mmmm is returned in decimal fraction of minutes. +static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) +{ + int32_t absgps, deg, min; + absgps = ABS(mwiigps); + deg = absgps / GPS_DEGREES_DIVIDER; + absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7 + min = absgps / GPS_DEGREES_DIVIDER; // minutes left + + if (telemetryConfig()->frsky_coordinate_format == FRSKY_FORMAT_DMS) { + result->dddmm = deg * 100 + min; + } else { + result->dddmm = deg * 60 + min; + } + + result->mmmm = (absgps - min * GPS_DEGREES_DIVIDER) / 1000; +} + +static void sendLatLong(int32_t coord[2]) +{ + gpsCoordinateDDDMMmmmm_t coordinate; + GPStoDDDMM_MMMM(coord[LAT], &coordinate); + sendDataHead(ID_LATITUDE_BP); + serialize16(coordinate.dddmm); + sendDataHead(ID_LATITUDE_AP); + serialize16(coordinate.mmmm); + sendDataHead(ID_N_S); + serialize16(coord[LAT] < 0 ? 'S' : 'N'); + + GPStoDDDMM_MMMM(coord[LON], &coordinate); + sendDataHead(ID_LONGITUDE_BP); + serialize16(coordinate.dddmm); + sendDataHead(ID_LONGITUDE_AP); + serialize16(coordinate.mmmm); + sendDataHead(ID_E_W); + serialize16(coord[LON] < 0 ? 'W' : 'E'); +} + +#ifdef USE_GPS +static void sendFakeLatLong(void) +{ + // Heading is only displayed on OpenTX if non-zero lat/long is also sent + int32_t coord[2] = {0,0}; + + coord[LAT] = (telemetryConfig()->gpsNoFixLatitude * GPS_DEGREES_DIVIDER); + coord[LON] = (telemetryConfig()->gpsNoFixLongitude * GPS_DEGREES_DIVIDER); + + sendLatLong(coord); +} +#endif + +static void sendFakeLatLongThatAllowsHeadingDisplay(void) +{ + // Heading is only displayed on OpenTX if non-zero lat/long is also sent + int32_t coord[2] = { + 1 * GPS_DEGREES_DIVIDER, + 1 * GPS_DEGREES_DIVIDER + }; + + sendLatLong(coord); +} + +#ifdef USE_GPS +static void sendGPSLatLong(void) +{ + static uint8_t gpsFixOccured = 0; + int32_t coord[2] = {0,0}; + + if (STATE(GPS_FIX) || gpsFixOccured == 1) { + // If we have ever had a fix, send the last known lat/long + gpsFixOccured = 1; + coord[LAT] = gpsSol.llh.lat; + coord[LON] = gpsSol.llh.lon; + sendLatLong(coord); + } else { + // otherwise send fake lat/long in order to display compass value + sendFakeLatLong(); + } +} +#endif + +/* + * Send vertical speed for opentx. ID_VERT_SPEED + * Unit is cm/s + */ +static void sendVario(void) +{ + sendDataHead(ID_VERT_SPEED); + serialize16((int16_t)lrintf(getEstimatedActualVelocity(Z))); +} + +/* + * Send voltage via ID_VOLT + * + * NOTE: This sends voltage divided by batteryCellCount. To get the real + * battery voltage, you need to multiply the value by batteryCellCount. + */ +static void sendVoltage(void) +{ + uint32_t cellVoltage; + uint16_t payload; + + /* + * Format for Voltage Data for single cells is like this: + * + * llll llll cccc hhhh + * l: Low voltage bits + * h: High voltage bits + * c: Cell number (starting at 0) + * + * The actual value sent for cell voltage has resolution of 0.002 volts + * Since vbat has resolution of 0.01 volts it has to be multiplied by 5 + */ + cellVoltage = ((uint32_t)getBatteryVoltage() * 10) / (getBatteryCellCount() * 2); + + // Cell number is at bit 9-12 (only uses vbat, so it can't send individual cell voltages, set cell number to 0) + payload = 0; + + // Lower voltage bits are at bit 0-8 + payload |= ((cellVoltage & 0x0ff) << 8); + + // Higher voltage bits are at bits 13-15 + payload |= ((cellVoltage & 0xf00) >> 8); + + sendDataHead(ID_VOLT); + serialize16(payload); +} + +/* + * Send voltage with ID_VOLTAGE_AMP + */ +static void sendVoltageAmp(void) +{ + uint16_t vbat = getBatteryVoltage(); + if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) { + /* + * Use new ID 0x39 to send voltage directly in 0.1 volts resolution + */ + sendDataHead(ID_VOLTAGE_AMP); + serialize16(vbat / 10); + } else { + uint16_t voltage = (vbat * 11) / 21; + uint16_t vfasVoltage; + if (telemetryConfig()->report_cell_voltage) { + vfasVoltage = voltage / getBatteryCellCount(); + } else { + vfasVoltage = voltage; + } + sendDataHead(ID_VOLTAGE_AMP_BP); + serialize16(vfasVoltage / 100); + sendDataHead(ID_VOLTAGE_AMP_AP); + serialize16(((vfasVoltage % 100) + 5) / 10); + } +} + +static void sendAmperage(void) +{ + sendDataHead(ID_CURRENT); + serialize16((uint16_t)(getAmperage() / 10)); +} + +static void sendFuelLevel(void) +{ + sendDataHead(ID_FUEL_LEVEL); + + serialize16((uint16_t)calculateBatteryPercentage()); +} + +static void sendHeading(void) +{ + sendDataHead(ID_COURSE_BP); + serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); + sendDataHead(ID_COURSE_AP); + serialize16(0); +} + +void initFrSkyTelemetry(void) +{ + portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY); + frskyPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY); +} + +void freeFrSkyTelemetryPort(void) +{ + closeSerialPort(frskyPort); + frskyPort = NULL; + frskyTelemetryEnabled = false; +} + +void configureFrSkyTelemetryPort(void) +{ + if (!portConfig) { + return; + } + + frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED); + if (!frskyPort) { + return; + } + + frskyTelemetryEnabled = true; +} + +bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis) +{ + return currentMillis - lastCycleTime >= CYCLETIME; +} + +void checkFrSkyTelemetryState(void) +{ + if (portConfig && telemetryCheckRxPortShared(portConfig)) { + if (!frskyTelemetryEnabled && telemetrySharedPort != NULL) { + frskyPort = telemetrySharedPort; + frskyTelemetryEnabled = true; + } + } else { + bool newTelemetryEnabledValue = telemetryDetermineEnabledState(frskyPortSharing); + + if (newTelemetryEnabledValue == frskyTelemetryEnabled) { + return; + } + + if (newTelemetryEnabledValue) + configureFrSkyTelemetryPort(); + else + freeFrSkyTelemetryPort(); + } +} + +void handleFrSkyTelemetry(void) +{ + if (!frskyTelemetryEnabled) { + return; + } + + uint32_t now = millis(); + + if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) { + return; + } + + lastCycleTime = now; + + cycleNum++; + + // Sent every 125ms + sendAccel(); + sendVario(); + sendTelemetryTail(); + + if ((cycleNum % 4) == 0) { // Sent every 500ms + if (lastCycleTime > DELAY_FOR_BARO_INITIALISATION) { //Allow 5s to boot correctly + sendBaro(); + } + sendHeading(); + sendTelemetryTail(); + } + + if ((cycleNum % 8) == 0) { // Sent every 1s + sendFlightModeAsTemperature1(); + sendThrottleOrBatterySizeAsRpm(); + + if (feature(FEATURE_VBAT)) { + sendVoltage(); + sendVoltageAmp(); + sendAmperage(); + sendFuelLevel(); + } + +#ifdef USE_GPS + if (sensors(SENSOR_GPS)) { + sendSpeed(); + sendHomeDistance(); + sendGpsAltitude(); + sendSatalliteSignalQualityAsTemperature2(); + sendGPSLatLong(); + } + else { + sendFakeLatLongThatAllowsHeadingDisplay(); + } +#else + sendFakeLatLongThatAllowsHeadingDisplay(); +#endif + + sendTelemetryTail(); + } + + if (cycleNum == 40) { //Frame 3: Sent every 5s + cycleNum = 0; + sendTime(); + sendTelemetryTail(); + } +} + +#endif diff --git a/src/main/telemetry/frsky_d.h b/src/main/telemetry/frsky_d.h new file mode 100644 index 0000000000..fe4846d011 --- /dev/null +++ b/src/main/telemetry/frsky_d.h @@ -0,0 +1,30 @@ +/* + * 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 + +typedef enum { + FRSKY_VFAS_PRECISION_LOW = 0, + FRSKY_VFAS_PRECISION_HIGH +} frskyVFasPrecision_e; + +void handleFrSkyTelemetry(void); +void checkFrSkyTelemetryState(void); + +void initFrSkyTelemetry(void); +void configureFrSkyTelemetryPort(void); +void freeFrSkyTelemetryPort(void); diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c index 66b0d949aa..6bfab4501f 100644 --- a/src/main/telemetry/ibus.c +++ b/src/main/telemetry/ibus.c @@ -243,7 +243,7 @@ void initIbusTelemetry(void) { changeTypeIbusTelemetry(15,IBUS_MEAS_TYPE1_FLIGHT_MODE, IBUS_MEAS_VALUE_MODE); } if (type == 6 || type == 7 || type == 8) { - if (batteryConfig()->current.type == CURRENT_SENSOR_VIRTUAL) + if (batteryMetersConfig()->current.type == CURRENT_SENSOR_VIRTUAL) changeTypeIbusTelemetry(3, IBUS_MEAS_TYPE1_FUEL, IBUS_MEAS_VALUE_FUEL); else changeTypeIbusTelemetry(3, IBUS_MEAS_TYPE1_BAT_CURR, IBUS_MEAS_VALUE_CURRENT); changeTypeIbusTelemetry(4, IBUS_MEAS_TYPE1_CMP_HEAD, IBUS_MEAS_VALUE_HEADING); diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 44e832ce29..bdd5b54a97 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -168,6 +168,8 @@ void ltm_sframe(sbuf_t *dst) lt_flightmode = 13; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) lt_flightmode = 9; + else if (FLIGHT_MODE(NAV_CRUISE_MODE)) + lt_flightmode = 18; else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) lt_flightmode = 8; else if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(HEADING_MODE)) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 998b22963c..2069110374 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -56,6 +56,7 @@ #include "telemetry/telemetry.h" #include "telemetry/smartport.h" +#include "telemetry/frsky.h" #include "telemetry/msp_shared.h" #define SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500 @@ -441,77 +442,17 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear break; case FSSP_DATAID_T1 : { - uint32_t tmpi = 0; - - // ones column - if (!isArmingDisabled()) - tmpi += 1; - else - tmpi += 2; - - if (ARMING_FLAG(ARMED)) - tmpi += 4; - - // tens column - if (FLIGHT_MODE(ANGLE_MODE)) - tmpi += 10; - if (FLIGHT_MODE(HORIZON_MODE)) - tmpi += 20; - if (FLIGHT_MODE(MANUAL_MODE)) - tmpi += 40; - - // hundreds column - if (FLIGHT_MODE(HEADING_MODE)) - tmpi += 100; - if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) - tmpi += 200; - if (FLIGHT_MODE(NAV_POSHOLD_MODE)) - tmpi += 400; - - // thousands column - if (FLIGHT_MODE(NAV_RTH_MODE)) - tmpi += 1000; - if (FLIGHT_MODE(NAV_WP_MODE)) - tmpi += 2000; - if (FLIGHT_MODE(HEADFREE_MODE)) - tmpi += 4000; - - // ten thousands column - if (FLIGHT_MODE(FLAPERON)) - tmpi += 10000; - if (FLIGHT_MODE(AUTO_TUNE)) - tmpi += 20000; - if (FLIGHT_MODE(FAILSAFE_MODE)) - tmpi += 40000; - - smartPortSendPackage(id, (uint32_t)tmpi); + smartPortSendPackage(id, frskyGetFlightMode()); *clearToSend = false; break; } +#ifdef USE_GPS case FSSP_DATAID_T2 : if (smartPortShouldSendGPSData()) { - uint32_t tmpi = 0; -#ifdef USE_GPS - // ones and tens columns (# of satellites 0 - 99) - tmpi += constrain(gpsSol.numSat, 0, 99); - - // hundreds column (satellite accuracy HDOP: 0 = worst, 9 = best) - tmpi += (9 - constrain(gpsSol.hdop / 1000, 0, 9)) * 100; - - // thousands column (GPS fix status) - if (STATE(GPS_FIX)) - tmpi += 1000; - if (STATE(GPS_FIX_HOME)) - tmpi += 2000; - if (ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) - tmpi += 4000; - - smartPortSendPackage(id, tmpi); + smartPortSendPackage(id, frskyGetGPSState()); *clearToSend = false; -#endif } break; -#ifdef USE_GPS case FSSP_DATAID_SPEED : if (smartPortShouldSendGPSData()) { //convert to knots: 1cm/s = 0.0194384449 knots diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 4a2053a438..3beb31704e 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -40,7 +40,7 @@ #include "rx/rx.h" #include "telemetry/telemetry.h" -#include "telemetry/frsky.h" +#include "telemetry/frsky_d.h" #include "telemetry/hott.h" #include "telemetry/smartport.h" #include "telemetry/ltm.h" diff --git a/src/test/Makefile b/src/test/Makefile index ca1aa3294d..405cb814c9 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -612,14 +612,6 @@ $(OBJECT_DIR)/build/debug.o : \ @mkdir -p $(dir $@) $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/build/debug.c -o $@ -$(OBJECT_DIR)/drivers/gyro_sync.o : \ - $(USER_DIR)/drivers/gyro_sync.c \ - $(USER_DIR)/drivers/gyro_sync.h \ - $(GTEST_HEADERS) - - @mkdir -p $(dir $@) - $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/gyro_sync.c -o $@ - $(OBJECT_DIR)/drivers/accgyro/accgyro_fake.o : \ $(USER_DIR)/drivers/accgyro/accgyro_fake.c \ $(USER_DIR)/drivers/accgyro/accgyro_fake.h \ @@ -647,8 +639,7 @@ $(OBJECT_DIR)/sensor_gyro_unittest.o : \ $(OBJECT_DIR)/sensor_gyro_unittest : \ $(OBJECT_DIR)/build/debug.o \ $(OBJECT_DIR)/common/maths.o \ - $(OBJECT_DIR)/common/filter.o \ - $(OBJECT_DIR)/drivers/gyro_sync.o \ + $(OBJECT_DIR)/common/filter.o \ $(OBJECT_DIR)/drivers/accgyro/accgyro_fake.o \ $(OBJECT_DIR)/sensors/gyro.o \ $(OBJECT_DIR)/sensors/boardalignment.o \ diff --git a/src/test/unit/alignsensor_unittest.cc b/src/test/unit/alignsensor_unittest.cc index 4fcaaecef0..d060315112 100644 --- a/src/test/unit/alignsensor_unittest.cc +++ b/src/test/unit/alignsensor_unittest.cc @@ -30,7 +30,7 @@ extern "C" { /* * This test file contains an independent method of rotating a vector. - * The output of alignSensor() is compared to the output of the test + * The output of applySensorAlignment() is compared to the output of the test * rotation method. * * For each alignment condition (CW0, CW90, etc) the source vector under @@ -110,7 +110,7 @@ static void testCW(sensor_align_e rotation, int32_t angle) initZAxisRotation(matrix, angle); rotateVector(matrix, src, test); - alignSensors(src, rotation); + applySensorAlignment(src, src, rotation); EXPECT_EQ(test[X], src[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_EQ(test[Y], src[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; EXPECT_EQ(test[Z], src[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z]; @@ -121,7 +121,7 @@ static void testCW(sensor_align_e rotation, int32_t angle) src[Z] = 0; rotateVector(matrix, src, test); - alignSensors(src, rotation); + applySensorAlignment(src, src, rotation); EXPECT_EQ(test[X], src[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_EQ(test[Y], src[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; EXPECT_EQ(test[Z], src[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z]; @@ -132,7 +132,7 @@ static void testCW(sensor_align_e rotation, int32_t angle) src[Z] = 1; rotateVector(matrix, src, test); - alignSensors(src, rotation); + applySensorAlignment(src, src, rotation); EXPECT_EQ(test[X], src[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_EQ(test[Y], src[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; EXPECT_EQ(test[Z], src[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << src[Z]; @@ -143,7 +143,7 @@ static void testCW(sensor_align_e rotation, int32_t angle) src[Z] = rand() % 5; rotateVector(matrix, src, test); - alignSensors(src, rotation); + applySensorAlignment(src, src, rotation); EXPECT_EQ(test[X], src[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_EQ(test[Y], src[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; EXPECT_EQ(test[Z], src[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << src[Z]; @@ -169,7 +169,7 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle) initZAxisRotation(matrix, angle); rotateVector(matrix, test, test); - alignSensors(src, rotation); + applySensorAlignment(src, src, rotation); EXPECT_EQ(test[X], src[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_EQ(test[Y], src[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; @@ -185,7 +185,7 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle) initZAxisRotation(matrix, angle); rotateVector(matrix, test, test); - alignSensors(src, rotation); + applySensorAlignment(src, src, rotation); EXPECT_EQ(test[X], src[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_EQ(test[Y], src[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; @@ -201,7 +201,7 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle) initZAxisRotation(matrix, angle); rotateVector(matrix, test, test); - alignSensors(src, rotation); + applySensorAlignment(src, src, rotation); EXPECT_EQ(test[X], src[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_EQ(test[Y], src[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; @@ -217,7 +217,7 @@ static void testCWFlip(sensor_align_e rotation, int32_t angle) initZAxisRotation(matrix, angle); rotateVector(matrix, test, test); - alignSensors(src, rotation); + applySensorAlignment(src, src, rotation); EXPECT_EQ(test[X], src[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << src[X]; EXPECT_EQ(test[Y], src[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << src[Y]; diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc index 2d273e9ece..77779d366e 100644 --- a/src/test/unit/maths_unittest.cc +++ b/src/test/unit/maths_unittest.cc @@ -26,6 +26,7 @@ extern "C" { #include "common/maths.h" + #include "common/vector.h" } #include "unittest_macros.h" @@ -118,33 +119,37 @@ TEST(MathsUnittest, TestApplyDeadband) EXPECT_EQ(applyDeadband(-11, 10), -1); } -void expectVectorsAreEqual(struct fp_vector *a, struct fp_vector *b) +void expectVectorsAreEqual(fpVector3_t *a, fpVector3_t *b) { - EXPECT_FLOAT_EQ(a->X, b->X); - EXPECT_FLOAT_EQ(a->Y, b->Y); - EXPECT_FLOAT_EQ(a->Z, b->Z); + EXPECT_FLOAT_EQ(a->x, b->x); + EXPECT_FLOAT_EQ(a->y, b->y); + EXPECT_FLOAT_EQ(a->z, b->z); } TEST(MathsUnittest, TestRotateVectorWithNoAngle) { - fp_vector vector = {1.0f, 0.0f, 0.0f}; + fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f}; fp_angles_t euler_angles = {.raw={0.0f, 0.0f, 0.0f}}; - rotateV(&vector, &euler_angles); - fp_vector expected_result = {1.0f, 0.0f, 0.0f}; + fpMat3_t rmat; + rotationMatrixFromAngles(&rmat, &euler_angles); + rotationMatrixRotateVector(&vector, &vector, &rmat); + fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f}; expectVectorsAreEqual(&vector, &expected_result); } TEST(MathsUnittest, TestRotateVectorAroundAxis) { // Rotate a vector <1, 0, 0> around an each axis x y and z. - fp_vector vector = {1.0f, 0.0f, 0.0f}; + fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f}; fp_angles_t euler_angles = {.raw={90.0f, 0.0f, 0.0f}}; - rotateV(&vector, &euler_angles); - fp_vector expected_result = {1.0f, 0.0f, 0.0f}; + fpMat3_t rmat; + rotationMatrixFromAngles(&rmat, &euler_angles); + rotationMatrixRotateVector(&vector, &vector, &rmat); + fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f}; expectVectorsAreEqual(&vector, &expected_result); } diff --git a/src/test/unit/rcdevice_unittest.cc b/src/test/unit/rcdevice_unittest.cc.txt similarity index 96% rename from src/test/unit/rcdevice_unittest.cc rename to src/test/unit/rcdevice_unittest.cc.txt index 6bdc05e4cf..e63bd82a0a 100644 --- a/src/test/unit/rcdevice_unittest.cc +++ b/src/test/unit/rcdevice_unittest.cc.txt @@ -29,6 +29,7 @@ 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" @@ -60,6 +61,9 @@ extern "C" { 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; } } #define MAX_RESPONSES_COUNT 10 @@ -95,7 +99,7 @@ static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlus testData.responseBufCount++; } - + memcpy(testData.responesBufs[testData.responseBufCount], data, dataLen); testData.responseBufsLen[testData.responseBufCount] = dataLen; testData.responseBufCount++; @@ -133,7 +137,7 @@ TEST(RCDeviceTest, TestInitDevice) testData.isAllowBufferReadWrite = true; uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; addResponseData(responseData, sizeof(responseData), true); - + bool result = runcamDeviceInit(&device); EXPECT_EQ(result, true); } @@ -382,13 +386,13 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol) EXPECT_EQ(true, result); clearResponseBuff(); - // open connection with correct response but wrong data length + // 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); clearResponseBuff(); - + // open connection with invalid crc uint8_t incorrectResponseDataOfOpenConnection2[] = { 0xCC, 0x10, 0x42 }; addResponseData(incorrectResponseDataOfOpenConnection2, sizeof(incorrectResponseDataOfOpenConnection2), true); @@ -408,7 +412,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol) EXPECT_EQ(true, result); clearResponseBuff(); - // close connection with correct response but wrong data length + // close connection with correct response but wrong data length uint8_t responseDataOfCloseConnection1[] = { 0xCC, 0x21, 0x11, 0xC1 }; addResponseData(responseDataOfCloseConnection1, sizeof(responseDataOfCloseConnection1), true); result = runcamDeviceClose5KeyOSDCableConnection(camDevice); @@ -434,7 +438,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol) EXPECT_EQ(true, result); clearResponseBuff(); - // simulate press button with correct response but wrong data length + // simulate press button with correct response but wrong data length uint8_t responseDataOfSimulation2[] = { 0xCC, 0xA5, 0x22 }; addResponseData(responseDataOfSimulation2, sizeof(responseDataOfSimulation2), true); result = runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET); @@ -448,7 +452,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol) EXPECT_EQ(false, result); clearResponseBuff(); - // simulate release button event + // simulate release button event result = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice); EXPECT_EQ(false, result); clearResponseBuff(); @@ -484,7 +488,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport) 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; @@ -534,7 +538,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWith5KeyFeatureSupport) EXPECT_EQ(true, rcdeviceInMenu); EXPECT_EQ(true, needRelease); clearResponseBuff(); - // relase button + // relase button rcData[ROLL] = 1500; // ROLL Mid rcData[PITCH] = 1500; // PITCH Mid rcData[YAW] = 1500; // Yaw High @@ -555,7 +559,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWith5KeyFeatureSupport) EXPECT_EQ(false, rcdeviceInMenu); EXPECT_EQ(true, needRelease); clearResponseBuff(); - // relase button + // relase button rcData[ROLL] = 1500; // ROLL Mid rcData[PITCH] = 1500; // PITCH Mid rcData[YAW] = 1500; // Yaw High @@ -574,7 +578,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWith5KeyFeatureSupport) EXPECT_EQ(true, rcdeviceInMenu); EXPECT_EQ(true, needRelease); clearResponseBuff(); - // relase button + // relase button rcData[ROLL] = 1500; // ROLL Mid rcData[PITCH] = 1500; // PITCH Mid rcData[YAW] = 1500; // Yaw High @@ -589,7 +593,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWith5KeyFeatureSupport) rcdeviceUpdate(0); EXPECT_EQ(true, needRelease); clearResponseBuff(); - // relase button + // relase button rcData[ROLL] = 1500; // ROLL Mid rcData[PITCH] = 1500; // PITCH Mid rcData[YAW] = 1500; // Yaw High @@ -656,7 +660,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWith5KeyFeatureSupport) EXPECT_EQ(true, needRelease); clearResponseBuff(); - // relase button + // relase button rcData[ROLL] = 1500; // ROLL Mid rcData[PITCH] = 1500; // PITCH Mid rcData[YAW] = 1500; // Yaw High @@ -671,7 +675,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWith5KeyFeatureSupport) rcdeviceUpdate(0); EXPECT_EQ(true, needRelease); clearResponseBuff(); - // send relase button + // send relase button rcData[ROLL] = 1500; // ROLL Mid rcData[PITCH] = 1500; // PITCH Mid rcData[YAW] = 1500; // Yaw High @@ -692,7 +696,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWith5KeyFeatureSupport) EXPECT_EQ(false, rcdeviceInMenu); EXPECT_EQ(true, needRelease); clearResponseBuff(); - // relase button + // relase button rcData[ROLL] = 1500; // ROLL Mid rcData[PITCH] = 1500; // PITCH Mid rcData[YAW] = 1500; // Yaw High @@ -703,12 +707,15 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWith5KeyFeatureSupport) } extern "C" { - serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options) + serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e function, + serialReceiveCallbackPtr rxCallback, void *rxCallbackData, + uint32_t baudRate, portMode_t mode, portOptions_t options) { UNUSED(identifier); - UNUSED(functionMask); + UNUSED(function); + UNUSED(rxCallback); + UNUSED(rxCallbackData); UNUSED(baudRate); - UNUSED(callback); UNUSED(mode); UNUSED(options); @@ -752,8 +759,8 @@ extern "C" { return NULL; } - uint32_t serialRxBytesWaiting(const serialPort_t *instance) - { + uint32_t serialRxBytesWaiting(const serialPort_t *instance) + { UNUSED(instance); uint8_t bufIndex = testData.indexOfCurrentRespBuf; @@ -771,8 +778,8 @@ extern "C" { return 0; } - uint8_t serialRead(serialPort_t *instance) - { + uint8_t serialRead(serialPort_t *instance) + { UNUSED(instance); uint8_t bufIndex = testData.indexOfCurrentRespBuf; @@ -789,41 +796,41 @@ extern "C" { return buffer[testData.responseDataReadPos++]; } - return 0; + return 0; } - void sbufWriteString(sbuf_t *dst, const char *string) - { - UNUSED(dst); UNUSED(string); + void sbufWriteString(sbuf_t *dst, const char *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); + + void sbufWriteU8(sbuf_t *dst, uint8_t val) + { + UNUSED(dst); UNUSED(val); if (testData.isAllowBufferReadWrite) { *dst->ptr++ = val; } } - + 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); dst->ptr += len; - } } // 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; @@ -889,9 +896,9 @@ extern "C" { bool feature(uint32_t) { return false; } - void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) - { - UNUSED(instance); UNUSED(data); UNUSED(count); + void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) + { + UNUSED(instance); UNUSED(data); UNUSED(count); // // reset the input buffer testData.responseDataReadPos = 0; @@ -925,7 +932,6 @@ extern "C" { ret |= sbufReadU8(src) << 24; return ret; } - uint32_t millis(void) { return testData.millis++; } diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index 74c86c60eb..d86448bde9 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -112,10 +112,10 @@ TEST(SensorGyro, Update) EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration)); gyroInit(); fakeGyroSet(5, 6, 7); - gyroUpdate(); + gyroUpdate(0); EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration)); while (!isCalibrationComplete(&gyroCalibration)) { - gyroUpdate(); + gyroUpdate(0); } EXPECT_EQ(true, isCalibrationComplete(&gyroCalibration)); EXPECT_EQ(5, gyroDev0.gyroZero[X]); @@ -124,13 +124,13 @@ TEST(SensorGyro, Update) EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]); EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]); EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]); - gyroUpdate(); + gyroUpdate(0); // expect zero values since gyro is calibrated EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]); EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]); EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]); fakeGyroSet(15, 26, 97); - gyroUpdate(); + gyroUpdate(0); EXPECT_FLOAT_EQ(10 * gyroDev0.scale, gyro.gyroADCf[X]); // gyroADCf values are scaled EXPECT_FLOAT_EQ(20 * gyroDev0.scale, gyro.gyroADCf[Y]); EXPECT_FLOAT_EQ(90 * gyroDev0.scale, gyro.gyroADCf[Z]); diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index 89777917ee..fadbee50c9 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -54,7 +54,7 @@ extern "C" { PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0); uint16_t testBatteryVoltage = 0; - int32_t testAmperage = 0; + int16_t testAmperage = 0; int32_t testMAhDrawn = 0; } @@ -216,11 +216,15 @@ void serialSetMode(serialPort_t *instance, portMode_t mode) { } -serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options) { +serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e function, + serialReceiveCallbackPtr rxCallback, void *rxCallbackData, + uint32_t baudRate, portMode_t mode, portOptions_t options) +{ UNUSED(identifier); - UNUSED(functionMask); + UNUSED(function); + UNUSED(rxCallback); + UNUSED(rxCallbackData); UNUSED(baudRate); - UNUSED(callback); UNUSED(mode); UNUSED(options); @@ -266,7 +270,7 @@ uint16_t getBatteryVoltage(void) { return testBatteryVoltage; } -int32_t getAmperage(void) { +int16_t getAmperage(void) { return testAmperage; } diff --git a/src/utils/rename-ifdefs.py b/src/utils/rename-ifdefs.py index 8cffac4c63..095d1d55e6 100644 --- a/src/utils/rename-ifdefs.py +++ b/src/utils/rename-ifdefs.py @@ -45,6 +45,11 @@ RENAMES = [ 'BOOTLOG', 'STATS', 'FIXED_WING_LANDING', + 'VTX_CONTROL', + 'VTX_SMARTAUDIO', + 'VTX_TRAMP', + 'VTX_RTC6705', + 'VTX_COMMON', ] NEW_NAMES = { diff --git a/src/utils/settings.rb b/src/utils/settings.rb index 792744f584..c42901c87f 100644 --- a/src/utils/settings.rb +++ b/src/utils/settings.rb @@ -532,6 +532,8 @@ class Generator return "VAR_UINT32" when "float" return "VAR_FLOAT" + when "string" + return "VAR_STRING" else raise "unknown variable type #{typ.inspect}" end @@ -764,6 +766,10 @@ class Generator typ = "uint32_t" when "float" typ = "float" + when /^char \[(\d+)\]/ + # Substract 1 to show the maximum string size without the null terminator + member["max"] = $1.to_i - 1; + typ = "string" else raise "Unknown type #{m[1]} when resolving type for setting #{member["name"]}" end