diff --git a/.travis.yml b/.travis.yml index 6b5339d12c7..55c0264c5c5 100755 --- a/.travis.yml +++ b/.travis.yml @@ -1,5 +1,6 @@ env: - TARGET=CC3D + - TARGET=CJMCU - TARGET=NAZE - TARGET=STM32F3DISCOVERY - TARGET=RMDO @@ -7,8 +8,8 @@ env: - TARGET=SPRACINGF3EVO - TARGET=SPARKY - TARGET=FURYF3 - - TARGET=FURYF3_SPIFLASH - TARGET=RCEXPLORERF3 + - TARGET=REVO # use new docker environment sudo: false @@ -30,10 +31,10 @@ language: cpp compiler: clang before_install: - - curl --retry 10 --retry-max-time 120 -L "https://launchpad.net/gcc-arm-embedded/4.9/4.9-2015-q2-update/+download/gcc-arm-none-eabi-4_9-2015q2-20150609-linux.tar.bz2" | tar xfj - + - curl --retry 10 --retry-max-time 120 -L "https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q2-update/+download/gcc-arm-none-eabi-5_4-2016q2-20160622-linux.tar.bz2" | tar xfj - install: - - export PATH=$PATH:$PWD/gcc-arm-none-eabi-4_9-2015q2/bin + - export PATH=$PATH:$PWD/gcc-arm-none-eabi-5_4-2016q2/bin before_script: arm-none-eabi-gcc --version script: ./.travis.sh diff --git a/CREDITS b/CREDITS new file mode 100644 index 00000000000..de85d956d3f --- /dev/null +++ b/CREDITS @@ -0,0 +1,84 @@ +This is at least a partial credits-file of people that have contributed to the INAV project. +If you believe you should be on this list - feel free to open a PR updating it. For faster +inclusion into this list please provide information about which part of code belongs to you. + +Albert Kravcov +Alex Gorbatchev +Alex Zaitsev +Alexander Fedorov +Andre Bernet +Andreas Tacke +Andrew Payne +Austin St. Aubin +Bas Huisman +Ben Hitchcock +Boris B +Brnadon Miller +ChiggerPepi +Chris Campbell +Chris Mavrakis +Chris Nisbet +Chris Penny +Damjan Adamic +Dave Pitman +David Bieber +Davide Bertola +Denis Kisselev +Dominic Clifton +Frank Zhao +Fredrik Steen +Gareth Wilkins +Gaël James +Gregor Ottmann +Hyon Lim +James Harrison +Jan Staal +Jeremy Waters +Joe Poser +Joel Fuster +Johannes Kasberger +Jonas Harnesk +Jonathan Hudson +Joshua Bardwell +Juan González +Kemal Hadimli +Kieran Moore +Konstantin Sharlaimov +Krzysztof Rosinski +Kyle Manna +Larry Davis +Marc Egli +Mark Williams +Martin Budden +Matthew Evans +Mauro Mombelli +Max Winterstein +Michael Corcoran +Michael Hope +Michael Jakob +Miha Valencic +Mikael Blomqvist +Moritz Ulrich +Moshen Chan +Nicholas Sherlock +Paul Fertser +Paul Rogalinski +Pawel Spychalski +Petr Ledvina +Phillip Jones +Pierre Hugo +Richard Birkby +Richard Lehey +Richard Marko +Rimas Avizienis +Sam Cook +Sami Korhonen +Samuel Brucksch +Scott Shawcroft +Sean Vig +Stefan Grufman +Steve Amor +Thomas Buck +Trey Marc +Tuomas Kuosmanen +Zap Andersson \ No newline at end of file diff --git a/Makefile b/Makefile index 2ae9aa1c22c..0d9148e8f4f 100644 --- a/Makefile +++ b/Makefile @@ -51,7 +51,7 @@ BIN_DIR = $(ROOT)/obj CMSIS_DIR = $(ROOT)/lib/main/CMSIS INCLUDE_DIRS = $(SRC_DIR) \ $(ROOT)/src/main/target -LINKER_DIR = $(ROOT)/src/main/target +LINKER_DIR = $(ROOT)/src/main/target/link # default xtal value for F4 targets HSE_VALUE = 8000000 @@ -332,6 +332,10 @@ ifneq ($(FLASH_SIZE),) DEVICE_FLAGS := $(DEVICE_FLAGS) -DFLASH_SIZE=$(FLASH_SIZE) endif +ifneq ($(HSE_VALUE),) +DEVICE_FLAGS := $(DEVICE_FLAGS) -DHSE_VALUE=$(HSE_VALUE) +endif + TARGET_DIR = $(ROOT)/src/main/target/$(BASE_TARGET) TARGET_DIR_SRC = $(notdir $(wildcard $(TARGET_DIR)/*.c)) @@ -371,6 +375,9 @@ COMMON_SRC = \ common/typeconversion.c \ common/streambuf.c \ config/config.c \ + config/feature.c \ + config/config_eeprom.c \ + config/parameter_group.c \ fc/runtime_config.c \ drivers/logging.c \ drivers/adc.c \ @@ -384,6 +391,7 @@ COMMON_SRC = \ drivers/io.c \ drivers/light_led.c \ drivers/rx_nrf24l01.c \ + drivers/rx_spi.c \ drivers/rx_xn297.c \ drivers/pwm_mapping.c \ drivers/pwm_output.c \ @@ -392,27 +400,32 @@ COMMON_SRC = \ drivers/serial.c \ drivers/serial_uart.c \ drivers/sound_beeper.c \ + drivers/stack_check.c \ drivers/system.c \ drivers/timer.c \ + drivers/io_pca9685.c \ flight/failsafe.c \ flight/imu.c \ flight/hil.c \ flight/mixer.c \ + flight/servos.c \ flight/pid.c \ io/beeper.c \ + fc/msp_fc.c \ fc/rc_controls.c \ fc/rc_curves.c \ + fc/fc_tasks.c \ io/serial.c \ io/serial_4way.c \ io/serial_4way_avrootloader.c \ io/serial_4way_stk500v2.c \ io/serial_cli.c \ - io/serial_msp.c \ io/statusindicator.c \ + io/pwmdriver_i2c.c \ + msp/msp_serial.c \ rx/ibus.c \ rx/jetiexbus.c \ rx/msp.c \ - rx/nrf24.c \ rx/nrf24_cx10.c \ rx/nrf24_inav.c \ rx/nrf24_h8_3d.c \ @@ -420,13 +433,13 @@ COMMON_SRC = \ rx/nrf24_v202.c \ rx/pwm.c \ rx/rx.c \ + rx/rx_spi.c \ rx/sbus.c \ rx/spektrum.c \ rx/sumd.c \ rx/sumh.c \ rx/xbus.c \ scheduler/scheduler.c \ - scheduler/scheduler_tasks.c \ sensors/acceleration.c \ sensors/battery.c \ sensors/boardalignment.c \ @@ -463,7 +476,8 @@ HIGHEND_SRC = \ telemetry/hott.c \ telemetry/smartport.c \ telemetry/mavlink.c \ - telemetry/ltm.c + telemetry/ltm.c \ + telemetry/ibus.c ifeq ($(TARGET),$(filter $(TARGET),$(F4_TARGETS))) VCP_SRC = \ @@ -561,7 +575,6 @@ TARGET_SRC += $(VCP_SRC) endif # end target specific make file checks - # Search path and source files for the ST stdperiph library VPATH := $(VPATH):$(STDPERIPH_DIR)/src @@ -688,7 +701,7 @@ all: $(VALID_TARGETS) $(VALID_TARGETS): echo "" && \ echo "Building $@" && \ - $(MAKE) -j binary hex TARGET=$@ && \ + $(MAKE) -j TARGET=$@ && \ echo "Building $@ succeeded." ## clean : clean up all temporary / machine-generated files diff --git a/docs/Board - OMNIBUS.md b/docs/Board - OMNIBUS.md new file mode 100644 index 00000000000..b5310592601 --- /dev/null +++ b/docs/Board - OMNIBUS.md @@ -0,0 +1,76 @@ +# Board - OMNIBUS F3 + +## Hardware Features + +Refer to the product web page: +[OMNIBUS AIO F3 Flight Control](http://shop.myairbot.com/index.php/flight-control/cleanflight-baseflight/omnibusv11.html) + +### Hardware Notes + +There are few things to note on how things are connected on the board. + +1. VBAT (J4) +This is a battery input to the board, and is also a input to voltage sensor. + +2. J11 Power distribution +The RAM is user defined power rail, and all RAM through holes (J6, J7 and J11) are connected together. By connecting 5V or VBAT to RAM at J11, the RAM becomes 5V or VBAT power rail respectively. The VBAT on J11 can also be used to power the Board if necessary. + +3. RSSI (J4) +The pin is labelled as RSSI, but it will not be used for RSSI input for a hardware configuration limitation. In this document, the "RSSI" is used to indicate the pin location, not the function. + +4. UART1 in boot-loader/DFU mode +The UART1 is scanned during boot-loader/DFU mode, together with USB for possible interaction with a host PC. It is observed that devices that autonomously transmits some data, such as GPS, will prevent the MCU to talk to the USB. It is advised not to connect or disconnect such devices to/from UART1. UART2 is safe from this catch. + +## iNav Specific Target Configuration + +The first support for the OMNIBUS F3 appeared in BetaFlight. +The OMNIBUS target in iNav has different configuration from the BetaFlight support, to maximize the hardware resource utilization for navigation oriented use cases. + + [PIN CONFIGURATION PIC HERE] + +### PWM Outputs + +Six PWM outputs (PWM1~PWM6) are supported, but PWM5 and PWM6 is not available when UART3 is in use. +PWM7 and PWM8 are dedicated for I2C; in this document, they are used to indicate the pin location, not the function. + +Note: Tested only for QUAD-X configuration. + +### Hardware UART Ports + +PPM/SBUS jumper for J8 is assumed to be configured for PPM (SBUS=R18 removed). + +| UART | Location | Note | +|-------|----------|-------------------| +| UART1 |J13 | | +| UART2 |J12 | | +| UART3 |J22 | PWM5=TX3,PWM6=RX3 | + +All UARTs are Serial RX capable. + +### I2C + +I2C is available on J22 PWM7 and PWM8 + +|signal | Location | Alt. Location | +|-------|------------|---------------| +|SCL | J22 (PWM7) | J3 (SCL) | +|SDA | J22 (PWM8) | J3 (SDA) | + +### SONAR + +SONAR is supported when NOT using PPM. + +|signal | Location | +|-------|------------| +|TRIG | J8 (PPM) | +|ECHO | J4 (RSSI) | + +5V sonar can be connected directly without inline resistors. + +### OSD + +Integrated OSD is not supported yet. + +### RSSI Sensor Input + +The RSSI sensor adc is not supported due to the hardware configuration limitation. diff --git a/docs/Board - Paris Air Hero 32 F3.md b/docs/Board - Paris Air Hero 32 F3.md new file mode 100755 index 00000000000..bf1e270424f --- /dev/null +++ b/docs/Board - Paris Air Hero 32 F3.md @@ -0,0 +1,46 @@ +# Board - Paris Air Hero 32 / Acro Naze 32 Mini + +This is the AIR3 PARIS Sirius AirHERO 32 F3 board from MultiWiiCopter +Source: http://www.multiwiicopter.com/products/air-hero-32-f3-spi-flight-controller + +## Sensors + +MPU6500 via SPI interface. +BMP280 via SPI interface + +## Ports + +6 x 3pin ESC / Servo outputs +1 x 8pin JST connector (PPM/PWM/UART2) +1 x 4pin JST connector (UART3) + +## I2C bus + +I2C bus is made available with a special target - AIRHEROF3_QUAD. This target limits motor outputs to 4 and adds I2C bus at M5/M6 connectors. + +## Pinouts + +The 10 pin RC I/O connector has the following pinouts when used in RX_PPM/RX_SERIAL mode. + +From right to left when looking at the socket from the edge of the board. + +| Pin | Function | Notes | +| --- | -------------- | -------------------------------- | +| 1 | Ground | | +| 2 | +5V | | +| 3 | RX_PPM | Enable `feature RX_PPM` | +| 4 | RSSI_ADC | Enable `feature RSSI_ADC`. Connect to the output of a PWM-RSSI conditioner, 0v-3.3v input | +| 5 | USART2 TX | | +| 6 | USART2 RX | | +| 7 | LED_STRIP | Enable `feature LED_STRIP` | +| 8 | unused | | + + +## Serial Ports + +| Value | Identifier | RX | TX | Notes | +| ----- | ------------ | ---------- | ------------------ | ------------------------------------------------------------------------------------------- | +| 1 | USART1 | RX / PA10 | TX / PA9 / TELEM | TELEM output is always inverted (for FrSky). Internally connected to USB port via CP2102 IC | +| 2 | USART2 | RC4 / PA3 | RC3 / PA2 | | +| 3 | USART3 | F3 / PB11 | F2 / PB10 | Flex port is configured as UART3 when port is configured | + diff --git a/docs/Cli.md b/docs/Cli.md index 1f280ea62af..bfd5e2ee948 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -116,15 +116,16 @@ Re-apply any new defaults as desired. | `rssi_ppm_invert` | | 0 | 1 | 0 | Master | UINT8 | | `input_filtering_mode` | | 0 | 1 | 0 | Master | INT8 | | `rc_smoothing ` | Interpolation of Rc data during looptimes when there are no new updates. This gives smoother RC input to PID controller and cleaner PIDsum | OFF | ON | ON | Master | INT8 | -| `min_throttle` | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. | 0 | 2000 | 1150 | Master | UINT16 | -| `max_throttle` | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. If you have brushed motors, the value should be set to 2000. | 0 | 2000 | 1850 | Master | UINT16 | +| `min_throttle` | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. | 0 | 2000 | 1150 | Master | UINT16 | +| `max_throttle` | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. If you have brushed motors, the value should be set to 2000. | 0 | 2000 | 1850 | Master | UINT16 | | `min_command` | This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once. | 0 | 2000 | 1000 | Master | UINT16 | | `servo_center_pulse` | | 0 | 2000 | 1500 | Master | UINT16 | | `3d_deadband_low` | | 0 | 2000 | 1406 | Master | UINT16 | | `3d_deadband_high` | | 0 | 2000 | 1514 | Master | UINT16 | | `3d_neutral` | | 0 | 2000 | 1460 | Master | UINT16 | | `3d_deadband_throttle` | | 0 | 2000 | 50 | Master | UINT16 | -| `motor_pwm_rate` | Output frequency (in Hz) for motor pins. Defaults are 400Hz for motor. If setting above 500Hz, will switch to brushed (direct drive) motors mode. For example, setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set ```max_throttle``` to 2000. | 50 | 32000 | 400 | Master | UINT16 | +| `motor_pwm_protocol` | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, BRUSHED | PWM | BRUSHED | PWM | Master | UINT8 | +| `motor_pwm_rate` | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with `motor_pwm_protocol` set to `STANDARD`. For `*SHOT` (e.g. `ONESHOT125`) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For `BRUSHED` values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set ```max_throttle``` to 2000. | 50 | 32000 | 400 | Master | UINT16 | | `servo_pwm_rate` | Output frequency (in Hz) servo pins. Default is 50Hz. When using tricopters or gimbal with digital servo, this rate can be increased. Max of 498Hz (for 500Hz pwm period), and min of 50Hz. Most digital servos will support for example 330Hz. | 50 | 498 | 50 | Master | UINT16 | | `servo_lowpass_freq` | Selects the servo PWM output cutoff frequency. Valid values range from 10 to 400. This is a fraction of the loop frequency in 1/1000ths. For example, `40` means `0.040`. The cutoff frequency can be determined by the following formula: `Frequency = 1000 * servo_lowpass_freq / looptime` | 10 | 400 | 400 | Master | INT16 | | `servo_lowpass_enable` | Disabled by default. | OFF | ON | OFF | Master | INT8 | @@ -250,15 +251,16 @@ Re-apply any new defaults as desired. | `yaw_jump_prevention_limit` | Prevent yaw jumps during yaw stops and rapid YAW input. To disable set to 500. Adjust this if your aircraft 'skids out'. Higher values increases YAW authority but can cause roll/pitch instability in case of underpowered UAVs. Lower values makes yaw adjustments more gentle but can cause UAV unable to keep heading | 80 | 500 | 200 | Master | UINT16 | | `yaw_p_limit` | Limiter for yaw P term. This parameter is only affecting PID controller MW23. To disable set to 500 (actual default). | 100 | 500 | 500 | Profile | UINT16 | | `blackbox_rate_num` | | 1 | 32 | 1 | Master | UINT8 | -| `blackbox_rate_denom` | | 1 | 32 | 1 | Master | UINT8 | -| `mag_hold_rate_limit` | This setting limits yaw rotation rate that MAG_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when MAG_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes.| 10 | 255 | 40 | Profile | UINT8 | -| `nav_max_climb_rate` | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. In cm/s | 10 | 2000 | 500 | Master | UINT16 | -| `flaperon_throw_offset` | Defines throw range in us for both ailerons that will be applied (before scaling) when FLAPERON mode is activated. | 100 | 400 | 250 | Profile | UINT16 | -| `flaperon_throw_inverted` | Inverts throw offset on both ailerons. Can be used to create SPOILERON or just to change throw direction | OFF | ON | OFF | Profile | UINT8 | -| `iterm_ignore_threshold` | Used to prevent Iterm accumulation on ROLL/PITCH axis during stick movements. Iterm is allowed to change fully when sticks are centered. Iterm will not change when requested rotation speed is above `iterm_ignore_threshold`. Iterm acumulation is scaled lineary between `0` and `iterm_ignore_threshold` | 15 | 1000 | 200 | Profile | UINT8 | -| `yaw_iterm_ignore_threshold` | Used to prevent Iterm accumulation on YAW axis during stick movements. Iterm is allowed to change fully when sticks are centered. Iterm will not change when requested rotation speed is above `yaw_iterm_ignore_threshold`. Iterm acumulation is scaled lineary between `0` and `yaw_iterm_ignore_threshold` | 15 | 1000 | 50 | Profile | UINT8 | -| `rate_accel_limit_roll_pitch` | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of `0` disables limiting. Disabled by default. | 0 | 500000 | 0 | Profile | UINT8 | -| `rate_accel_limit_yaw` | Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability durig yaw turns. Value of `0` disables limiting. Enabled by default. | 0 | 500000 | 10000 | Profile | UINT8 | +| `blackbox_rate_denom` | | 1 | 32 | 1 | Master | UINT8 | +| `mag_hold_rate_limit` | This setting limits yaw rotation rate that MAG_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when MAG_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes.| 10 | 255 | 40 | Profile | UINT8 | +| `nav_max_climb_rate` | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. In cm/s | 10 | 2000 | 500 | Master | UINT16 | +| `flaperon_throw_offset` | Defines throw range in us for both ailerons that will be applied (before scaling) when FLAPERON mode is activated. | 100 | 400 | 250 | Profile | UINT16 | +| `flaperon_throw_inverted` | Inverts throw offset on both ailerons. Can be used to create SPOILERON or just to change throw direction | OFF | ON | OFF | Profile | UINT8 | +| `fw_iterm_throw_limit` | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set `0` to disable completely. | 0 | 500 | 165 | Profile | UINT16 | +| `iterm_ignore_threshold` | Used to prevent Iterm accumulation on ROLL/PITCH axis during stick movements. Iterm is allowed to change fully when sticks are centered. Iterm will not change when requested rotation speed is above `iterm_ignore_threshold`. Iterm acumulation is scaled lineary between `0` and `iterm_ignore_threshold` | 15 | 1000 | 200 | Profile | UINT8 | +| `yaw_iterm_ignore_threshold` | Used to prevent Iterm accumulation on YAW axis during stick movements. Iterm is allowed to change fully when sticks are centered. Iterm will not change when requested rotation speed is above `yaw_iterm_ignore_threshold`. Iterm acumulation is scaled lineary between `0` and `yaw_iterm_ignore_threshold` | 15 | 1000 | 50 | Profile | UINT8 | +| `rate_accel_limit_roll_pitch` | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of `0` disables limiting. Disabled by default. | 0 | 500000 | 0 | Profile | UINT8 | +| `rate_accel_limit_yaw` | Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability durig yaw turns. Value of `0` disables limiting. Enabled by default. | 0 | 500000 | 10000 | Profile | UINT8 | ## New iNav specific | `Variable` | Description/Units | Min | Max | Default | Type | Datatype | @@ -343,3 +345,4 @@ Re-apply any new defaults as desired. | `nav_fw_pitch2thr` | Amount of throttle applied related to pitch attitude in GPS assisted modes | unknown| unknown| 10 | Master | UNKNOWN | | `nav_fw_roll2pitch` | Amount of positive pitch (nose up) when the plane turns by ailerons in GPS assisted modes. With high wind is better lowering this | unknown| unknown| 75 | Master | UNKNOWN | | `nav_fw_loiter_radius` | PosHold radius in cm. 3000 to 7500 is a good value (30-75m) | unknown| unknown| 5000 | Master | UNKNOWN | +| `fixed_wing_auto_arm` | Auto-arm fixed wing aircraft on throttle above min_throttle, and disarming with stick commands are disabled, so power cycle is requirred to disarm. Requirres enabled motorstop and no arm switch configured. | OFF | ON | OFF | Master | UNKNOWN | diff --git a/docs/Getting Started.md b/docs/Getting Started.md index 91be094806a..60561bee330 100644 --- a/docs/Getting Started.md +++ b/docs/Getting Started.md @@ -22,7 +22,7 @@ For an overview of the hardware INAV (hereby CF) can run on, see [Boards.md](Boa * Do you want your RC Receiver's RSSI to be sent to the board? [The RSSI chapter](Rssi.md) explains how. You may or may not need to make an additional connection from your Receiver to the FC. -* Would you like to try using a GPS unit to get your aircraft to Loiter or Return-To-Launch? Take a look at the [GPS](Gps.md) and [GPS Tested Hardware](Gps_-_Tested_Hardware.md) chapters. +* Would you like to try using a GPS unit to get your aircraft to Loiter or Return-To-Launch? Take a look at the [GPS](Gps.md) and [GPS Tested Hardware](Gps.md#hardware) chapters. * You may also want to read the [Serial](Serial.md) chapter to determine what extra devices (such as Blackbox, OSD, Telemetry) you may want to use, and how they should be connected. diff --git a/docs/Navigation.md b/docs/Navigation.md index 9d33f10645b..df1e96aaf76 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -8,8 +8,7 @@ Altitude hold requires a valid source of altitude - barometer or sonar. The best In this mode THROTTLE stick controls climb rate (vertical velocity). When pilot moves stick up - quad goes up, pilot moves stick down - quad descends, you keep stick at neutral position - quad hovers. ### CLI parameters affecting ALTHOLD mode: -* *nav_use_midrc_for_althold* - when set to "0", firmware will remember where your throttle stick was when ALTHOLD was activated - this will be considered neutral position. When set to "1" - 50% throttle will be considered neutral position. -* *nav_throttle_tilt_comp* - when set to "1" firmware will automatically increase throttle when copter is tilted and in ALTHOLD mode. +* *nav_use_midthr_for_althold* - when set to "0", firmware will remember where your throttle stick was when ALTHOLD was activated - this will be considered neutral position. When set to "1" - 50% throttle will be considered neutral position. ### Related PIDs PIDs affecting altitude hold: ALT & VEL @@ -19,7 +18,7 @@ PID meaning: ## Throttle tilt compensation -Throttle tilt compensation attempts to maintain constant vertical thrust when copter is tilted giving additional throttle if tilt angle (pitch/roll) is not zero. Controlled by *nav_throttle_tilt_comp* CLI variable. +Throttle tilt compensation attempts to maintain constant vertical thrust when copter is tilted giving additional throttle if tilt angle (pitch/roll) is not zero. Controlled by *throttle_tilt_comp_str* CLI variable. ## NAV POSHOLD mode - position hold diff --git a/docs/development/Building in Windows light.md b/docs/development/Building in Windows light.md new file mode 100644 index 00000000000..9c4767e9907 --- /dev/null +++ b/docs/development/Building in Windows light.md @@ -0,0 +1,100 @@ +# Building in windows light +no cygwin and no path changes + +##Install Git for windows +download https://github.com/git-for-windows/git/releases/download/v2.10.1.windows.1/Git-2.10.1-32-bit.exe + +Recommended install location is C:\Git (no spaces or special characters in path) + +Follow images as not all are at default settings. + +![Git Installation](assets/001.gitwin.png) + +![Git Installation](assets/002.gitwin.png) + +![Git Installation](assets/003.gitwin.png) + +![Git Installation](assets/004.gitwin.png) + +![Git Installation](assets/005.gitwin.png) + +![Git Installation](assets/006.gitwin.png) + +![Git Installation](assets/007.gitwin.png) + +![Git Installation](assets/008.gitwin.png) + +![Git Installation](assets/009.gitwin.png) + +![Git Installation](assets/010.gitwin.png) + +##Install toolset scripts +download https://www.dropbox.com/s/hhlr16h657y4l5u/devtools.zip?dl=0 + +extract it into C:\ it creates devtools folder + +##Install arm toolchain +download https://launchpad.net/gcc-arm-embedded/4.9/4.9-2015-q2-update/+download/gcc-arm-none-eabi-4_9-2015q2-20150609-win32.zip + +extract it into C:\devtools\gcc-arm-none-eabi-4_9-2015q2-20150609-win32 (folder already there) + +##Test +Run C:\devtools\shF4.cmd + +If everything went according the manual you should be in mingw console window. (if not we need to update this manual) + +Try command "arm-none-eabi-gcc --version" and output should be like in screenshot. (tab complete works here) + +![Test toolchain](assets/001.test.png) + +Note1: Advanced users can edit shF4.cmd for paths if they don't want to use defaults. You might want to change TOOLS_DIR and PATH_DIRS variables. + +Note2: You can copy shF4.cmd anywhere you want and run it from there. It will open console window in that folder. + +Note3: Included example batch-scripts (make_REVO.bat) that you can use to build target just by double clicking it. + +## Checkout and compile INAV + +Head over to the INAV Github page and grab the URL of the GIT Repository: "https://github.com/iNavFlight/inav" + +Run shF4.cmd and use the git commandline to checkout the repository: + +```bash +git clone https://github.com/iNavFlight/inav +``` +![GIT Checkout](assets/011.git_checkout.png) + +![GIT Checkout](assets/002.test.png) + +To compile your INAV binaries, enter the inav directory and build the project using the make command. You can append TARGET=[HARDWARE] if you want to build anything other than the default NAZE target: + +```bash +cd inav +make TARGET=NAZE +``` + +![GIT Checkout](assets/003.test.png) + +within few moments you should have your binary ready: + +```bash +(...) +arm-none-eabi-size ./obj/main/inav_NAZE.elf + text data bss dec hex filename + 127468 916 16932 145316 237a4 ./obj/main/inav_NAZE.elf +arm-none-eabi-objcopy -O ihex --set-start 0x8000000 obj/main/inav_NAZE.elf obj/inav_1.2.1_NAZE.hex +``` + +You can use the INAV-Configurator to flash the ```obj/inav_1.2.1_NAZE.hex``` file. + +## Updating and rebuilding + +Navigate to the local inavflight repository and use the following steps to pull the latest changes and rebuild your version of inavflight: + +```bash +cd inav +git reset --hard +git pull +make clean TARGET=NAZE +make +``` diff --git a/docs/development/Building in Windows.md b/docs/development/Building in Windows.md index a4c8926118d..49c3229351c 100755 --- a/docs/development/Building in Windows.md +++ b/docs/development/Building in Windows.md @@ -65,7 +65,7 @@ git clone https://github.com/iNavFlight/inav.git ![GIT Checkout](assets/012.git_checkout.png) -To compile your INAV binaries, enter the cleanflight directory and build the project using the make command. You can append TARGET=[HARDWARE] if you want to build anything other than the default NAZE target: +To compile your INAV binaries, enter the inav directory and build the project using the make command. You can append TARGET=[HARDWARE] if you want to build anything other than the default NAZE target: ```bash cd inav diff --git a/docs/development/assets/001.gitwin.png b/docs/development/assets/001.gitwin.png new file mode 100644 index 00000000000..49e59c122f0 Binary files /dev/null and b/docs/development/assets/001.gitwin.png differ diff --git a/docs/development/assets/001.test.png b/docs/development/assets/001.test.png new file mode 100644 index 00000000000..ac7b6baf749 Binary files /dev/null and b/docs/development/assets/001.test.png differ diff --git a/docs/development/assets/002.gitwin.png b/docs/development/assets/002.gitwin.png new file mode 100644 index 00000000000..6a461af0b15 Binary files /dev/null and b/docs/development/assets/002.gitwin.png differ diff --git a/docs/development/assets/002.test.png b/docs/development/assets/002.test.png new file mode 100644 index 00000000000..6d13cb8df0a Binary files /dev/null and b/docs/development/assets/002.test.png differ diff --git a/docs/development/assets/003.gitwin.png b/docs/development/assets/003.gitwin.png new file mode 100644 index 00000000000..a8df0fd948a Binary files /dev/null and b/docs/development/assets/003.gitwin.png differ diff --git a/docs/development/assets/003.test.png b/docs/development/assets/003.test.png new file mode 100644 index 00000000000..6b92fc45b8f Binary files /dev/null and b/docs/development/assets/003.test.png differ diff --git a/docs/development/assets/004.gitwin.png b/docs/development/assets/004.gitwin.png new file mode 100644 index 00000000000..ef22d489bb2 Binary files /dev/null and b/docs/development/assets/004.gitwin.png differ diff --git a/docs/development/assets/005.gitwin.png b/docs/development/assets/005.gitwin.png new file mode 100644 index 00000000000..e84e280ad3b Binary files /dev/null and b/docs/development/assets/005.gitwin.png differ diff --git a/docs/development/assets/006.gitwin.png b/docs/development/assets/006.gitwin.png new file mode 100644 index 00000000000..b1c3231521d Binary files /dev/null and b/docs/development/assets/006.gitwin.png differ diff --git a/docs/development/assets/007.gitwin.png b/docs/development/assets/007.gitwin.png new file mode 100644 index 00000000000..dd815b2e30a Binary files /dev/null and b/docs/development/assets/007.gitwin.png differ diff --git a/docs/development/assets/008.gitwin.png b/docs/development/assets/008.gitwin.png new file mode 100644 index 00000000000..48a9f685e7d Binary files /dev/null and b/docs/development/assets/008.gitwin.png differ diff --git a/docs/development/assets/009.gitwin.png b/docs/development/assets/009.gitwin.png new file mode 100644 index 00000000000..467c9e2a775 Binary files /dev/null and b/docs/development/assets/009.gitwin.png differ diff --git a/docs/development/assets/010.gitwin.png b/docs/development/assets/010.gitwin.png new file mode 100644 index 00000000000..dfbca185d44 Binary files /dev/null and b/docs/development/assets/010.gitwin.png differ diff --git a/lib/main/STM32_USB-FS-Device_Driver/inc/usb_mem.h b/lib/main/STM32_USB-FS-Device_Driver/inc/usb_mem.h index 0b2910060ae..a182432d7df 100644 --- a/lib/main/STM32_USB-FS-Device_Driver/inc/usb_mem.h +++ b/lib/main/STM32_USB-FS-Device_Driver/inc/usb_mem.h @@ -35,7 +35,7 @@ /* Exported constants --------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ /* Exported functions ------------------------------------------------------- */ -void UserToPMABufferCopy(uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes); +void UserToPMABufferCopy(const uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes); void PMAToUserBufferCopy(uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes); /* External variables --------------------------------------------------------*/ diff --git a/lib/main/STM32_USB-FS-Device_Driver/src/usb_mem.c b/lib/main/STM32_USB-FS-Device_Driver/src/usb_mem.c index 4e0748ac39d..46074cc6cd4 100644 --- a/lib/main/STM32_USB-FS-Device_Driver/src/usb_mem.c +++ b/lib/main/STM32_USB-FS-Device_Driver/src/usb_mem.c @@ -45,7 +45,7 @@ * Output : None. * Return : None . *******************************************************************************/ -void UserToPMABufferCopy(uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes) +void UserToPMABufferCopy(const uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes) { uint32_t n = (wNBytes + 1) >> 1; /* n = (wNBytes + 1) / 2 */ uint32_t i, temp1, temp2; diff --git a/lib/main/STM32_USB_Device_Library/Class/cdc/inc/usbd_cdc_core.h b/lib/main/STM32_USB_Device_Library/Class/cdc/inc/usbd_cdc_core.h index b9afa7423d1..d05ecf5f8e1 100644 --- a/lib/main/STM32_USB_Device_Library/Class/cdc/inc/usbd_cdc_core.h +++ b/lib/main/STM32_USB_Device_Library/Class/cdc/inc/usbd_cdc_core.h @@ -95,7 +95,7 @@ typedef struct _CDC_IF_PROP uint16_t (*pIf_Init) (void); uint16_t (*pIf_DeInit) (void); uint16_t (*pIf_Ctrl) (uint32_t Cmd, uint8_t* Buf, uint32_t Len); - uint16_t (*pIf_DataTx) (uint8_t* Buf, uint32_t Len); + uint16_t (*pIf_DataTx) (const uint8_t* Buf, uint32_t Len); uint16_t (*pIf_DataRx) (uint8_t* Buf, uint32_t Len); } CDC_IF_Prop_TypeDef; diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 319abe9d7c5..e70b6195da0 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -55,13 +55,13 @@ #include "io/beeper.h" #include "io/display.h" -#include "io/escservo.h" +#include "io/motors.h" +#include "io/servos.h" #include "io/gimbal.h" #include "io/gps.h" #include "io/ledstrip.h" #include "io/serial.h" #include "io/serial_cli.h" -#include "io/serial_msp.h" #include "io/statusindicator.h" #include "rx/rx.h" @@ -70,6 +70,7 @@ #include "telemetry/telemetry.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/navigation_rewrite.h" @@ -77,6 +78,7 @@ #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" #include "blackbox.h" #include "blackbox_io.h" @@ -580,7 +582,7 @@ static void writeIntraframe(void) * Write the throttle separately from the rest of the RC data so we can apply a predictor to it. * Throttle lies in range [minthrottle..maxthrottle]: */ - blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - masterConfig.escAndServoConfig.minthrottle); + blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - masterConfig.motorConfig.minthrottle); if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { /* @@ -624,7 +626,7 @@ static void writeIntraframe(void) blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT); //Motors can be below minthrottle when disarmed, but that doesn't happen much - blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle); + blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.motorConfig.minthrottle); //Motors tend to be similar to each other so use the first motor's value as a predictor of the others for (x = 1; x < motorCount; x++) { @@ -1275,8 +1277,8 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime); BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", 100); //For compatibility reasons write rc_rate 100 - BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle); - BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.escAndServoConfig.maxthrottle); + BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.motorConfig.minthrottle); + BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.motorConfig.maxthrottle); BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale)); BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.acc_1G); diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 321d9195020..f7bf106cdb2 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -28,6 +28,7 @@ #include "common/axis.h" #include "common/color.h" #include "common/encoding.h" +#include "common/streambuf.h" #include "drivers/gpio.h" #include "drivers/sensor.h" @@ -50,23 +51,27 @@ #include "io/beeper.h" #include "io/display.h" -#include "io/escservo.h" -#include "rx/rx.h" +#include "io/motors.h" +#include "io/servos.h" #include "fc/rc_controls.h" - #include "io/gimbal.h" #include "io/gps.h" #include "io/ledstrip.h" #include "io/serial.h" #include "io/serial_cli.h" -#include "io/serial_msp.h" #include "io/statusindicator.h" + +#include "msp/msp_serial.h" + #include "rx/msp.h" +#include "rx/rx.h" + #include "telemetry/telemetry.h" #include "common/printf.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/navigation_rewrite.h" @@ -666,7 +671,7 @@ void blackboxDeviceClose(void) * of time to shut down asynchronously, we're the only ones that know when to call it. */ if (blackboxPortSharing == PORTSHARING_SHARED) { - mspAllocateSerialPorts(); + mspSerialAllocatePorts(); } break; default: diff --git a/src/main/build/version.h b/src/main/build/version.h index 9b755e6129d..90b723b98bf 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -16,8 +16,8 @@ */ #define FC_VERSION_MAJOR 1 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 2 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed +#define FC_VERSION_MINOR 3 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 2ce69286e1c..2c5c6cd07b5 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -20,59 +20,18 @@ #include #include -#include "common/axis.h" #include "common/filter.h" #include "common/maths.h" -#include "drivers/gyro_sync.h" - -#define BIQUAD_Q (1.0f / 1.41421356f) /* quality factor - butterworth (1 / sqrt(2)) */ -#define M_PI_FLOAT 3.14159265358979323846f - -void biquadFilterInit(biquadFilter_t *filter, uint32_t filterCutFreq, uint32_t refreshRate) -{ - // setup variables - const float sampleRate = 1.0f / ((float)refreshRate * 0.000001f); - const float omega = 2 * M_PIf * ((float)filterCutFreq) / sampleRate; - const float sn = sin_approx(omega); - const float cs = cos_approx(omega); - const float alpha = sn / (2 * BIQUAD_Q); - - float a0, a1, a2, b0, b1, b2; - - b0 = (1 - cs) / 2; - b1 = 1 - cs; - b2 = (1 - cs) / 2; - a0 = 1 + alpha; - a1 = -2 * cs; - a2 = 1 - alpha; - - // precompute the coefficients - filter->b0 = b0 / a0; - filter->b1 = b1 / a0; - filter->b2 = b2 / a0; - filter->a1 = a1 / a0; - filter->a2 = a2 / a0; - - // zero initial samples - filter->d1 = filter->d2 = 1; -} - -/* Computes a biquad_t filter on a sample */ -float biquadFilterApply(biquadFilter_t *filter, float input) -{ - const float result = filter->b0 * input + filter->d1; - filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2; - filter->d2 = filter->b2 * input - filter->a2 * result; - return result; -} +#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */ +#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/ // PT1 Low Pass filter // f_cut = cutoff frequency void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT) { - filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut ); + filter->RC = 1.0f / ( 2.0f * M_PIf * f_cut ); filter->dT = dT; } @@ -82,13 +41,14 @@ float pt1FilterApply(pt1Filter_t *filter, float input) return filter->state; } -float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT) +float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dT) { // Pre calculate and store RC if (!filter->RC) { - filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut ); + filter->RC = 1.0f / ( 2.0f * M_PIf * f_cut ); } + filter->dT = dT; // cache latest dT for possible use in pt1FilterApply filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state); return filter->state; } @@ -117,7 +77,67 @@ float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_l return filter->state; } -// FIR filter +float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff) +{ + const float octaves = log2f((float)centerFreq / (float)cutoff) * 2; + return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1); +} + +// sets up a biquad Filter +void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate) +{ + biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); +} + +void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) +{ + // setup variables + const float sampleRate = 1.0f / ((float)refreshRate * 0.000001f); + const float omega = 2.0f * M_PIf * ((float)filterFreq) / sampleRate; + const float sn = sin_approx(omega); + const float cs = cos_approx(omega); + const float alpha = sn / (2 * Q); + + float b0, b1, b2; + switch (filterType) { + case FILTER_LPF: + b0 = (1 - cs) / 2; + b1 = 1 - cs; + b2 = (1 - cs) / 2; + break; + case FILTER_NOTCH: + b0 = 1; + b1 = -2 * cs; + b2 = 1; + break; + } + const float a0 = 1 + alpha; + const float a1 = -2 * cs; + const float a2 = 1 - alpha; + + // precompute the coefficients + filter->b0 = b0 / a0; + filter->b1 = b1 / a0; + filter->b2 = b2 / a0; + filter->a1 = a1 / a0; + filter->a2 = a2 / a0; + + // zero initial samples + filter->d1 = filter->d2 = 0; +} + +// Computes a biquad_t filter on a sample +float biquadFilterApply(biquadFilter_t *filter, float input) +{ + const float result = filter->b0 * input + filter->d1; + filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2; + filter->d2 = filter->b2 * input - filter->a2 * result; + return result; +} + +/* + * FIR filter + */ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength) { filter->buf = buf; @@ -127,6 +147,10 @@ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const fl memset(filter->buf, 0, sizeof(float) * filter->bufLength); } +/* + * FIR filter initialisation + * If FIR filter is just used for averaging, coeffs can be set to NULL + */ void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs) { firFilterInit2(filter, buf, bufLength, coeffs, bufLength); @@ -138,7 +162,7 @@ void firFilterUpdate(firFilter_t *filter, float input) filter->buf[0] = input; } -float firFilterApply(firFilter_t *filter) +float firFilterApply(const firFilter_t *filter) { float ret = 0.0f; for (int ii = 0; ii < filter->coeffsLength; ++ii) { diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 089dae5701f..66b4a5a317d 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -33,6 +33,17 @@ typedef struct biquadFilter_s { float d1, d2; } biquadFilter_t; +typedef enum { + FILTER_PT1 = 0, + FILTER_BIQUAD, + FILTER_FIR, +} filterType_e; + +typedef enum { + FILTER_LPF, + FILTER_NOTCH +} biquadFilterType_e; + typedef struct firFilter_s { float *buf; const float *coeffs; @@ -41,17 +52,19 @@ typedef struct firFilter_s { } firFilter_t; float pt1FilterApply(pt1Filter_t *filter, float input); -float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt); +float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dt); void pt1FilterReset(pt1Filter_t *filter, float input); void rateLimitFilterInit(rateLimitFilter_t *filter); float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT); -void biquadFilterInit(biquadFilter_t *filter, uint32_t filterCutFreq, uint32_t refreshRate); +void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate); +void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); float biquadFilterApply(biquadFilter_t *filter, float sample); +float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff); void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs); void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength); void firFilterUpdate(firFilter_t *filter, float input); -float firFilterApply(firFilter_t *filter); +float firFilterApply(const firFilter_t *filter); diff --git a/src/main/common/streambuf.c b/src/main/common/streambuf.c index 0f47511e037..118b205b60d 100644 --- a/src/main/common/streambuf.c +++ b/src/main/common/streambuf.c @@ -73,14 +73,14 @@ uint32_t sbufReadU32(sbuf_t *src) return ret; } -void sbufReadData(sbuf_t *src, void *data, int len) +void sbufReadData(const sbuf_t *src, void *data, int len) { memcpy(data, src->ptr, len); } // reader - return bytes remaining in buffer // writer - return available space -int sbufBytesRemaining(sbuf_t *buf) +int sbufBytesRemaining(const sbuf_t *buf) { return buf->end - buf->ptr; } @@ -90,6 +90,11 @@ uint8_t* sbufPtr(sbuf_t *buf) return buf->ptr; } +const uint8_t* sbufConstPtr(const sbuf_t *buf) +{ + return buf->ptr; +} + // advance buffer pointer // reader - skip data // writer - commit written data diff --git a/src/main/common/streambuf.h b/src/main/common/streambuf.h index 7de771ef02f..0ec169666e1 100644 --- a/src/main/common/streambuf.h +++ b/src/main/common/streambuf.h @@ -36,10 +36,11 @@ void sbufWriteString(sbuf_t *dst, const char *string); uint8_t sbufReadU8(sbuf_t *src); uint16_t sbufReadU16(sbuf_t *src); uint32_t sbufReadU32(sbuf_t *src); -void sbufReadData(sbuf_t *dst, void *data, int len); +void sbufReadData(const sbuf_t *dst, void *data, int len); -int sbufBytesRemaining(sbuf_t *buf); +int sbufBytesRemaining(const sbuf_t *buf); uint8_t* sbufPtr(sbuf_t *buf); +const uint8_t* sbufConstPtr(const sbuf_t *buf); void sbufAdvance(sbuf_t *buf, int size); void sbufSwitchToReader(sbuf_t *buf, uint8_t * base); diff --git a/src/main/config/config.c b/src/main/config/config.c index 21e2e4ab66b..2d9e3e06023 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -23,6 +23,7 @@ #include "build/build_config.h" +#include "blackbox/blackbox_io.h" #include "common/color.h" #include "common/axis.h" @@ -33,9 +34,10 @@ #include "drivers/accgyro.h" #include "drivers/compass.h" #include "drivers/system.h" -#include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" +#include "drivers/rx_spi.h" +#include "drivers/pwm_output.h" #include "drivers/rx_nrf24l01.h" #include "drivers/serial.h" @@ -44,26 +46,26 @@ #include "sensors/compass.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" -#include "sensors/boardalignment.h" #include "sensors/battery.h" +#include "sensors/boardalignment.h" #include "io/beeper.h" #include "io/serial.h" #include "io/gimbal.h" -#include "io/escservo.h" +#include "io/motors.h" +#include "io/servos.h" #include "fc/rc_controls.h" #include "fc/rc_curves.h" #include "io/ledstrip.h" #include "io/gps.h" #include "rx/rx.h" -#include "rx/nrf24.h" - -#include "blackbox/blackbox_io.h" +#include "rx/rx_spi.h" #include "telemetry/telemetry.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/failsafe.h" @@ -72,107 +74,28 @@ #include "fc/runtime_config.h" #include "config/config.h" - +#include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" #ifndef DEFAULT_RX_FEATURE #define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM #endif +#ifndef RX_SPI_DEFAULT_PROTOCOL +#define RX_SPI_DEFAULT_PROTOCOL 0 +#endif #define BRUSHED_MOTORS_PWM_RATE 16000 #define BRUSHLESS_MOTORS_PWM_RATE 400 -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); - -#ifndef DEFAULT_RX_FEATURE -#define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM -#endif -#ifndef NRF24_DEFAULT_PROTOCOL -#define NRF24_DEFAULT_PROTOCOL 0 -#endif - -#if !defined(FLASH_SIZE) -#error "Flash size not defined for target. (specify in KB)" -#endif - - -#ifndef FLASH_PAGE_SIZE - #ifdef STM32F303xC - #define FLASH_PAGE_SIZE ((uint16_t)0x800) - #endif - - #ifdef STM32F10X_MD - #define FLASH_PAGE_SIZE ((uint16_t)0x400) - #endif - - #ifdef STM32F10X_HD - #define FLASH_PAGE_SIZE ((uint16_t)0x800) - #endif - - #if defined(STM32F40_41xxx) - #define FLASH_PAGE_SIZE ((uint32_t)0x20000) - #endif - - #if defined (STM32F411xE) - #define FLASH_PAGE_SIZE ((uint32_t)0x20000) - #endif - -#endif - -#if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT) - #ifdef STM32F10X_MD - #define FLASH_PAGE_COUNT 128 - #endif - - #ifdef STM32F10X_HD - #define FLASH_PAGE_COUNT 128 - #endif -#endif - -#if defined(FLASH_SIZE) -#if defined(STM32F40_41xxx) -#define FLASH_PAGE_COUNT 4 // just to make calculations work -#elif defined (STM32F411xE) -#define FLASH_PAGE_COUNT 4 // just to make calculations work -#else -#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE) -#endif -#endif - -#if !defined(FLASH_PAGE_SIZE) -#error "Flash page size not defined for target." -#endif - -#if !defined(FLASH_PAGE_COUNT) -#error "Flash page count not defined for target." -#endif - -#if FLASH_SIZE <= 128 -#define FLASH_TO_RESERVE_FOR_CONFIG 0x800 -#else -#define FLASH_TO_RESERVE_FOR_CONFIG 0x1000 -#endif - -// use the last flash pages for storage -#ifdef CUSTOM_FLASH_MEMORY_ADDRESS -size_t custom_flash_memory_address = 0; -#define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address) -#else -// use the last flash pages for storage -#ifndef CONFIG_START_FLASH_ADDRESS -#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) -#endif -#endif master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; -static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 121; static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain) { @@ -232,6 +155,10 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->max_angle_inclination[FD_ROLL] = 300; // 30 degrees pidProfile->max_angle_inclination[FD_PITCH] = 300; // 30 degrees +#ifdef USE_SERVOS + pidProfile->fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT; +#endif + } #ifdef NAV @@ -320,13 +247,29 @@ void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) sensorAlignmentConfig->mag_align = ALIGN_DEFAULT; } -void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) +void resetMotorConfig(motorConfig_t *motorConfig) +{ +#ifdef BRUSHED_MOTORS + motorConfig->minthrottle = 1000; + motorConfig->motorPwmProtocol = PWM_TYPE_BRUSHED; + motorConfig->motorPwmRate = BRUSHED_MOTORS_PWM_RATE; +#else + motorConfig->minthrottle = 1150; + motorConfig->motorPwmProtocol = PWM_TYPE_STANDARD; + motorConfig->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; +#endif + motorConfig->maxthrottle = 1850; + motorConfig->mincommand = 1000; + +} + +#ifdef USE_SERVOS +void resetServoConfig(servoConfig_t *servoConfig) { - escAndServoConfig->minthrottle = 1150; - escAndServoConfig->maxthrottle = 1850; - escAndServoConfig->mincommand = 1000; - escAndServoConfig->servoCenterPulse = 1500; + servoConfig->servoCenterPulse = 1500; + servoConfig->servoPwmRate = 50; } +#endif void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) { @@ -420,22 +363,27 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { rcControlsConfig->alt_hold_deadband = 50; } -void resetMixerConfig(mixerConfig_t *mixerConfig) { +static void resetMixerConfig(mixerConfig_t *mixerConfig) +{ mixerConfig->yaw_motor_direction = 1; mixerConfig->yaw_jump_prevention_limit = 200; +} + #ifdef USE_SERVOS - mixerConfig->tri_unarmed_servo = 1; - mixerConfig->servo_lowpass_freq = 400; - mixerConfig->servo_lowpass_enable = 0; -#endif +static void resetServoMixerConfig(servoMixerConfig_t *servoMixerConfig) +{ + servoMixerConfig->tri_unarmed_servo = 1; + servoMixerConfig->servo_lowpass_freq = 400; + servoMixerConfig->servo_lowpass_enable = 0; } +#endif uint8_t getCurrentProfile(void) { return masterConfig.current_profile_index; } -static void setProfile(uint8_t profileIndex) +void setProfile(uint8_t profileIndex) { currentProfile = &masterConfig.profile[profileIndex]; } @@ -449,7 +397,7 @@ controlRateConfig_t *getControlRateConfig(uint8_t profileIndex) { return &masterConfig.controlRateProfiles[profileIndex]; } -static void setControlRateProfile(uint8_t profileIndex) +void setControlRateProfile(uint8_t profileIndex) { currentControlRateProfileIndex = profileIndex; currentControlRateProfile = &masterConfig.controlRateProfiles[profileIndex]; @@ -457,14 +405,12 @@ static void setControlRateProfile(uint8_t profileIndex) uint16_t getCurrentMinthrottle(void) { - return masterConfig.escAndServoConfig.minthrottle; + return masterConfig.motorConfig.minthrottle; } // Default settings static void resetConf(void) { - int i; - // Clear all configuration memset(&masterConfig, 0, sizeof(master_t)); setProfile(0); @@ -512,8 +458,12 @@ static void resetConf(void) resetTelemetryConfig(&masterConfig.telemetryConfig); #endif +#ifdef SERIALRX_PROVIDER + masterConfig.rxConfig.serialrx_provider = SERIALRX_PROVIDER; +#else masterConfig.rxConfig.serialrx_provider = 0; - masterConfig.rxConfig.nrf24rx_protocol = NRF24_DEFAULT_PROTOCOL; +#endif + masterConfig.rxConfig.rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL; masterConfig.rxConfig.spektrum_sat_bind = 0; masterConfig.rxConfig.midrc = 1500; masterConfig.rxConfig.mincheck = 1100; @@ -521,7 +471,7 @@ static void resetConf(void) masterConfig.rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection - for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { + for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[i]; channelFailsafeConfiguration->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD; channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc); @@ -541,18 +491,14 @@ static void resetConf(void) masterConfig.small_angle = 25; resetMixerConfig(&masterConfig.mixerConfig); +#ifdef USE_SERVOS + resetServoMixerConfig(&masterConfig.servoMixerConfig); + resetServoConfig(&masterConfig.servoConfig); +#endif - // Motor/ESC/Servo - resetEscAndServoConfig(&masterConfig.escAndServoConfig); + resetMotorConfig(&masterConfig.motorConfig); resetFlight3DConfig(&masterConfig.flight3DConfig); -#ifdef BRUSHED_MOTORS - masterConfig.motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE; -#else - masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; -#endif - masterConfig.servo_pwm_rate = 50; - #ifdef GPS // gps/nav stuff masterConfig.gpsConfig.provider = GPS_UBLOX; @@ -577,7 +523,7 @@ static void resetConf(void) resetControlRateConfig(&masterConfig.controlRateProfiles[0]); - // for (i = 0; i < CHECKBOXITEMS; i++) + // for (int i = 0; i < CHECKBOXITEMS; i++) // cfg.activate[i] = 0; currentProfile->mag_declination = 0; @@ -587,7 +533,11 @@ static void resetConf(void) resetBarometerConfig(&masterConfig.barometerConfig); // Radio +#ifdef RX_CHANNELS_TAER + parseRcChannels("TAER1234", &masterConfig.rxConfig); +#else parseRcChannels("AETR1234", &masterConfig.rxConfig); +#endif resetRcControlsConfig(¤tProfile->rcControlsConfig); @@ -603,7 +553,7 @@ static void resetConf(void) #ifdef USE_SERVOS // servos - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN; currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX; currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE; @@ -622,8 +572,9 @@ static void resetConf(void) #endif // custom mixer. clear by defaults. - for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { masterConfig.customMotorMixer[i].throttle = 0.0f; + } #ifdef LED_STRIP applyDefaultColors(masterConfig.colors); @@ -657,7 +608,6 @@ static void resetConf(void) masterConfig.rxConfig.rcmap[6] = 6; masterConfig.rxConfig.rcmap[7] = 7; - featureSet(FEATURE_ONESHOT125); featureSet(FEATURE_VBAT); featureSet(FEATURE_LED_STRIP); featureSet(FEATURE_FAILSAFE); @@ -671,11 +621,10 @@ static void resetConf(void) #else masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; #endif - masterConfig.rxConfig.serialrx_provider = 1; masterConfig.rxConfig.spektrum_sat_bind = 5; - masterConfig.escAndServoConfig.minthrottle = 1000; - masterConfig.escAndServoConfig.maxthrottle = 2000; - masterConfig.motor_pwm_rate = 32000; + masterConfig.motorConfig.minthrottle = 1000; + masterConfig.motorConfig.maxthrottle = 2000; + masterConfig.motorConfig.motorPwmRate = 32000; masterConfig.looptime = 2000; currentProfile->pidProfile.P8[ROLL] = 36; currentProfile->pidProfile.P8[PITCH] = 36; @@ -736,55 +685,23 @@ static void resetConf(void) #endif // copy first profile into remaining profile - for (i = 1; i < MAX_PROFILE_COUNT; i++) { + for (int i = 1; i < MAX_PROFILE_COUNT; i++) { memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); } // copy first control rate config into remaining profile - for (i = 1; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) { + for (int i = 1; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) { memcpy(&masterConfig.controlRateProfiles[i], currentControlRateProfile, sizeof(controlRateConfig_t)); } - for (i = 1; i < MAX_PROFILE_COUNT; i++) { + for (int i = 1; i < MAX_PROFILE_COUNT; i++) { masterConfig.profile[i].defaultRateProfileIndex = i % MAX_CONTROL_RATE_PROFILE_COUNT; } } -static uint8_t calculateChecksum(const uint8_t *data, uint32_t length) -{ - uint8_t checksum = 0; - const uint8_t *byteOffset; - - for (byteOffset = data; byteOffset < (data + length); byteOffset++) - checksum ^= *byteOffset; - return checksum; -} - -static bool isEEPROMContentValid(void) -{ - const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS; - uint8_t checksum = 0; - - // check version number - if (EEPROM_CONF_VERSION != temp->version) - return false; - - // check size and magic numbers - if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF) - return false; - - // verify integrity of temporary copy - checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t)); - if (checksum != 0) - return false; - - // looks good, let's roll! - return true; -} - void activateControlRateConfig(void) { - generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig); + generateThrottleCurve(currentControlRateProfile, &masterConfig.motorConfig); } void activateConfig(void) @@ -797,7 +714,7 @@ void activateConfig(void) useRcControlsConfig( currentProfile->modeActivationConditions, - &masterConfig.escAndServoConfig, + &masterConfig.motorConfig, ¤tProfile->pidProfile ); @@ -813,16 +730,10 @@ void activateConfig(void) setAccelerationGain(&masterConfig.accGain); setAccelerationFilter(currentProfile->pidProfile.acc_soft_lpf_hz); - mixerUseConfigs( + mixerUseConfigs(&masterConfig.flight3DConfig, &masterConfig.motorConfig, &masterConfig.mixerConfig, &masterConfig.rxConfig); #ifdef USE_SERVOS - currentProfile->servoConf, - ¤tProfile->gimbalConfig, + servosUseConfigs(&masterConfig.servoMixerConfig, currentProfile->servoConf, ¤tProfile->gimbalConfig, &masterConfig.rxConfig); #endif - &masterConfig.flight3DConfig, - &masterConfig.escAndServoConfig, - &masterConfig.mixerConfig, - &masterConfig.rxConfig - ); imuRuntimeConfig.dcm_kp_acc = masterConfig.dcm_kp_acc / 10000.0f; imuRuntimeConfig.dcm_ki_acc = masterConfig.dcm_ki_acc / 10000.0f; @@ -840,7 +751,7 @@ void activateConfig(void) navigationUseRcControlsConfig(¤tProfile->rcControlsConfig); navigationUseRxConfig(&masterConfig.rxConfig); navigationUseFlight3DConfig(&masterConfig.flight3DConfig); - navigationUseEscAndServoConfig(&masterConfig.escAndServoConfig); + navigationUsemotorConfig(&masterConfig.motorConfig); #endif #ifdef BARO @@ -848,48 +759,33 @@ void activateConfig(void) #endif } -static void validateAndFixConfig(void) +void validateAndFixConfig(void) { - if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_NRF24))) { - featureSet(DEFAULT_RX_FEATURE); - } - - if (featureConfigured(FEATURE_RX_PPM)) { - featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_NRF24); - } + // Disable unused features + featureClear(FEATURE_UNUSED_1 | FEATURE_UNUSED_2); - if (featureConfigured(FEATURE_RX_MSP)) { - featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_NRF24); - } + if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) { + featureSet(DEFAULT_RX_FEATURE); + } - if (featureConfigured(FEATURE_RX_SERIAL)) { - featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_NRF24); - } + if (featureConfigured(FEATURE_RX_PPM)) { + featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_SPI); + } - if (featureConfigured(FEATURE_RX_NRF24)) { - featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_MSP); - } + if (featureConfigured(FEATURE_RX_MSP)) { + featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_SPI); + } -#if defined(NAV) - // Ensure sane values of navConfig settings - validateNavConfig(&masterConfig.navConfig); -#endif + if (featureConfigured(FEATURE_RX_SERIAL)) { + featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI); + } - if (featureConfigured(FEATURE_SOFTSPI)) { - featureClear(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL | FEATURE_VBAT); -#if defined(STM32F10X) - featureClear(FEATURE_LED_STRIP); - // rssi adc needs the same ports - featureClear(FEATURE_RSSI_ADC); - // current meter needs the same ports - if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) { - featureClear(FEATURE_CURRENT_METER); - } -#endif + if (featureConfigured(FEATURE_RX_SPI)) { + featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_MSP); } if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) { - featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_NRF24); + featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI); #if defined(STM32F10X) // rssi adc needs the same ports featureClear(FEATURE_RSSI_ADC); @@ -897,18 +793,12 @@ static void validateAndFixConfig(void) if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) { featureClear(FEATURE_CURRENT_METER); } - #if defined(CC3D) // There is a timer clash between PWM RX pins and motor output pins - this forces us to have same timer tick rate for these timers // which is only possible when using brushless motors w/o oneshot (timer tick rate is PWM_TIMER_MHZ) - // On CC3D OneShot is incompatible with PWM RX - featureClear(FEATURE_ONESHOT125); - - // Brushed motors on CC3D are not possible when using PWM RX - if (masterConfig.motor_pwm_rate > BRUSHLESS_MOTORS_PWM_RATE) { - masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; - } + masterConfig.motorConfig.motorPwmProtocol = PWM_TYPE_STANDARD; + masterConfig.motorConfig.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE; #endif #endif @@ -921,6 +811,21 @@ static void validateAndFixConfig(void) featureClear(FEATURE_SOFTSERIAL); } +#ifdef USE_SOFTSPI + if (featureConfigured(FEATURE_SOFTSPI)) { + featureClear(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL | FEATURE_VBAT); +#if defined(STM32F10X) + featureClear(FEATURE_LED_STRIP); + // rssi adc needs the same ports + featureClear(FEATURE_RSSI_ADC); + // current meter needs the same ports + if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) { + featureClear(FEATURE_CURRENT_METER); + } +#endif + } +#endif + #ifdef STM32F10X // avoid overloading the CPU on F1 targets when using gyro sync and GPS. if (masterConfig.gyroSync && masterConfig.gyroSyncDenominator < 2 && featureConfigured(FEATURE_GPS)) { @@ -960,7 +865,7 @@ static void validateAndFixConfig(void) } #endif -#if defined(CC3D) && defined(DISPLAY) && defined(USE_USART3) +#if defined(CC3D) && defined(DISPLAY) && defined(USE_UART3) if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DISPLAY)) { featureClear(FEATURE_DISPLAY); } @@ -992,6 +897,10 @@ static void validateAndFixConfig(void) #endif // CC3D_PPM1 #endif // CC3D +#ifndef USE_PMW_SERVO_DRIVER + featureClear(FEATURE_PWM_SERVO_DRIVER); +#endif + #if defined(COLIBRI_RACE) masterConfig.serialConfig.portConfigs[0].functionMask = FUNCTION_MSP; if(featureConfigured(FEATURE_RX_SERIAL)) { @@ -1010,49 +919,47 @@ static void validateAndFixConfig(void) /* * If provided predefined mixer setup is disabled, fallback to default one */ - if (!isMixerEnabled(masterConfig.mixerMode)) { - masterConfig.mixerMode = DEFAULT_MIXER; - } -} - -void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch) -{ - updateBoardAlignment(&masterConfig.boardAlignment, roll, pitch); - - saveConfigAndNotify(); -} - -void initEEPROM(void) -{ -} - -void readEEPROM(void) -{ - // Sanity check - if (!isEEPROMContentValid()) - failureMode(FAILURE_INVALID_EEPROM_CONTENTS); + if (!isMixerEnabled(masterConfig.mixerMode)) { + masterConfig.mixerMode = DEFAULT_MIXER; + } - suspendRxSignal(); +#if defined(NAV) + // Ensure sane values of navConfig settings + validateNavConfig(&masterConfig.navConfig); +#endif - // Read flash - memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t)); + /* Limitations of different protocols */ + switch (masterConfig.motorConfig.motorPwmProtocol) { + case PWM_TYPE_STANDARD: // Limited to 490 Hz + masterConfig.motorConfig.motorPwmRate = MIN(masterConfig.motorConfig.motorPwmRate, 490); + break; - if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check - masterConfig.current_profile_index = 0; + case PWM_TYPE_ONESHOT125: // Limited to 3900 Hz + masterConfig.motorConfig.motorPwmRate = MIN(masterConfig.motorConfig.motorPwmRate, 3900); + break; - setProfile(masterConfig.current_profile_index); + case PWM_TYPE_ONESHOT42: // 2-8 kHz + masterConfig.motorConfig.motorPwmRate = constrain(masterConfig.motorConfig.motorPwmRate, 2000, 8000); + break; - if (currentProfile->defaultRateProfileIndex > MAX_CONTROL_RATE_PROFILE_COUNT - 1) // sanity check - currentProfile->defaultRateProfileIndex = 0; + case PWM_TYPE_MULTISHOT: // 2-16 kHz + masterConfig.motorConfig.motorPwmRate = constrain(masterConfig.motorConfig.motorPwmRate, 2000, 16000); + break; - setControlRateProfile(currentProfile->defaultRateProfileIndex); + case PWM_TYPE_BRUSHED: // 500Hz - 32kHz + masterConfig.motorConfig.motorPwmRate = constrain(masterConfig.motorConfig.motorPwmRate, 500, 32000); + break; + } +} - validateAndFixConfig(); - activateConfig(); +void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch) +{ + updateBoardAlignment(&masterConfig.boardAlignment, roll, pitch); - resumeRxSignal(); + saveConfigAndNotify(); } + void readEEPROMAndNotify(void) { // re-read written data @@ -1060,71 +967,6 @@ void readEEPROMAndNotify(void) beeperConfirmationBeeps(1); } -void writeEEPROM(void) -{ - // Generate compile time error if the config does not fit in the reserved area of flash. - BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG); - - FLASH_Status status = 0; - uint32_t wordOffset; - int8_t attemptsRemaining = 3; - - suspendRxSignal(); - - // prepare checksum/version constants - masterConfig.version = EEPROM_CONF_VERSION; - masterConfig.size = sizeof(master_t); - masterConfig.magic_be = 0xBE; - masterConfig.magic_ef = 0xEF; - masterConfig.chk = 0; // erase checksum before recalculating - masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t)); - - // write it - FLASH_Unlock(); - while (attemptsRemaining--) { -#ifdef STM32F40_41xxx - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); -#endif -#ifdef STM32F303 - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR); -#endif -#ifdef STM32F10X - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); -#endif - for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) { - if (wordOffset % FLASH_PAGE_SIZE == 0) { -#if defined(STM32F40_41xxx) - status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000 -#elif defined (STM32F411xE) - status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000 -#else - status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset); -#endif - if (status != FLASH_COMPLETE) { - break; - } - } - - status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset, - *(uint32_t *) ((char *) &masterConfig + wordOffset)); - if (status != FLASH_COMPLETE) { - break; - } - } - if (status == FLASH_COMPLETE) { - break; - } - } - FLASH_Lock(); - - // Flash write failed - just die now - if (status != FLASH_COMPLETE || !isEEPROMContentValid()) { - failureMode(FAILURE_FLASH_WRITE_FAILED); - } - - resumeRxSignal(); -} - void ensureEEPROMContainsValidData(void) { if (isEEPROMContentValid()) { @@ -1163,52 +1005,6 @@ void changeControlRateProfile(uint8_t profileIndex) activateControlRateConfig(); } -void handleOneshotFeatureChangeOnRestart(void) -{ - // Shutdown PWM on all motors prior to soft restart - StopPwmAllMotors(); - delay(50); - // Apply additional delay when OneShot125 feature changed from on to off state - if (feature(FEATURE_ONESHOT125) && !featureConfigured(FEATURE_ONESHOT125)) { - delay(ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS); - } -} - -void latchActiveFeatures() -{ - activeFeaturesLatch = masterConfig.enabledFeatures; -} - -bool featureConfigured(uint32_t mask) -{ - return masterConfig.enabledFeatures & mask; -} - -bool feature(uint32_t mask) -{ - return activeFeaturesLatch & mask; -} - -void featureSet(uint32_t mask) -{ - masterConfig.enabledFeatures |= mask; -} - -void featureClear(uint32_t mask) -{ - masterConfig.enabledFeatures &= ~(mask); -} - -void featureClearAll() -{ - masterConfig.enabledFeatures = 0; -} - -uint32_t featureMask(void) -{ - return masterConfig.enabledFeatures; -} - void persistentFlagClearAll() { masterConfig.persistentFlags = 0; diff --git a/src/main/config/config.h b/src/main/config/config.h index 03abdd6e2de..d78f29bb57c 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -17,7 +17,14 @@ #pragma once +#include +#include + +#if FLASH_SIZE <= 128 +#define MAX_PROFILE_COUNT 2 +#else #define MAX_PROFILE_COUNT 3 +#endif #define MAX_CONTROL_RATE_PROFILE_COUNT 3 #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 @@ -25,7 +32,7 @@ typedef enum { FEATURE_RX_PPM = 1 << 0, FEATURE_VBAT = 1 << 1, - //FEATURE_INFLIGHT_ACC_CAL = 1 << 2, + FEATURE_UNUSED_1 = 1 << 2, // Unused in INAV FEATURE_RX_SERIAL = 1 << 3, FEATURE_MOTOR_STOP = 1 << 4, FEATURE_SERVO_TILT = 1 << 5, @@ -41,29 +48,22 @@ typedef enum { FEATURE_RSSI_ADC = 1 << 15, FEATURE_LED_STRIP = 1 << 16, FEATURE_DISPLAY = 1 << 17, - FEATURE_ONESHOT125 = 1 << 18, + FEATURE_UNUSED_2 = 1 << 18, // Unused in INAV FEATURE_BLACKBOX = 1 << 19, FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_TRANSPONDER = 1 << 21, FEATURE_AIRMODE = 1 << 22, FEATURE_SUPEREXPO_RATES = 1 << 23, FEATURE_VTX = 1 << 24, - FEATURE_RX_NRF24 = 1 << 25, + FEATURE_RX_SPI = 1 << 25, FEATURE_SOFTSPI = 1 << 26, + FEATURE_PWM_SERVO_DRIVER = 1 << 27, } features_e; typedef enum { FLAG_MAG_CALIBRATION_DONE = 1 << 0, } persistent_flags_e; -void handleOneshotFeatureChangeOnRestart(void); -void latchActiveFeatures(void); -bool featureConfigured(uint32_t mask); -bool feature(uint32_t mask); -void featureSet(uint32_t mask); -void featureClear(uint32_t mask); -void featureClearAll(void); -uint32_t featureMask(void); void beeperOffSet(uint32_t mask); void beeperOffSetAll(uint8_t beeperCount); void beeperOffClear(uint32_t mask); @@ -80,20 +80,28 @@ void persistentFlagClearAll(); void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex); -void initEEPROM(void); void resetEEPROM(void); -void readEEPROM(void); void readEEPROMAndNotify(void); -void writeEEPROM(); void ensureEEPROMContainsValidData(void); + void saveConfigAndNotify(void); +void validateAndFixConfig(void); +void activateConfig(void); uint8_t getCurrentProfile(void); void changeProfile(uint8_t profileIndex); +void setProfile(uint8_t profileIndex); +void setControlRateProfile(uint8_t profileIndex); +struct pidProfile_s; +void resetPidProfile(struct pidProfile_s *pidProfile); + uint8_t getCurrentControlRateProfile(void); void changeControlRateProfile(uint8_t profileIndex); bool canSoftwareSerialBeUsed(void); void applyAndSaveBoardAlignmentDelta(int16_t roll, int16_t pitch); uint16_t getCurrentMinthrottle(void); +struct master_s; +void targetConfiguration(struct master_s *config); + diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c new file mode 100755 index 00000000000..b666f3c2526 --- /dev/null +++ b/src/main/config/config_eeprom.c @@ -0,0 +1,269 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/build_config.h" + +#include "common/color.h" +#include "common/axis.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/motors.h" +#include "io/servos.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/servos.h" +#include "flight/pid.h" +#include "flight/failsafe.h" +#include "flight/navigation_rewrite.h" + +#include "config/config.h" +#include "config/config_eeprom.h" +#include "config/config_profile.h" +#include "config/config_master.h" + +#if !defined(FLASH_SIZE) +#error "Flash size not defined for target. (specify in KB)" +#endif + + +#ifndef FLASH_PAGE_SIZE + #ifdef STM32F303xC + #define FLASH_PAGE_SIZE ((uint16_t)0x800) + #endif + + #ifdef STM32F10X_MD + #define FLASH_PAGE_SIZE ((uint16_t)0x400) + #endif + + #ifdef STM32F10X_HD + #define FLASH_PAGE_SIZE ((uint16_t)0x800) + #endif + + #if defined(STM32F40_41xxx) + #define FLASH_PAGE_SIZE ((uint32_t)0x20000) + #endif + + #if defined (STM32F411xE) + #define FLASH_PAGE_SIZE ((uint32_t)0x20000) + #endif + +#endif + +#if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT) + #ifdef STM32F10X_MD + #define FLASH_PAGE_COUNT 128 + #endif + + #ifdef STM32F10X_HD + #define FLASH_PAGE_COUNT 128 + #endif +#endif + +#if defined(FLASH_SIZE) +#if defined(STM32F40_41xxx) +#define FLASH_PAGE_COUNT 4 // just to make calculations work +#elif defined (STM32F411xE) +#define FLASH_PAGE_COUNT 4 // just to make calculations work +#else +#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE) +#endif +#endif + +#if !defined(FLASH_PAGE_SIZE) +#error "Flash page size not defined for target." +#endif + +#if !defined(FLASH_PAGE_COUNT) +#error "Flash page count not defined for target." +#endif + +#if FLASH_SIZE <= 128 +#define FLASH_TO_RESERVE_FOR_CONFIG 0x800 +#else +#define FLASH_TO_RESERVE_FOR_CONFIG 0x1000 +#endif + +// use the last flash pages for storage +#ifdef CUSTOM_FLASH_MEMORY_ADDRESS +size_t custom_flash_memory_address = 0; +#define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address) +#else +// use the last flash pages for storage +#ifndef CONFIG_START_FLASH_ADDRESS +#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) +#endif +#endif + +void initEEPROM(void) +{ +} + +static uint8_t calculateChecksum(const uint8_t *data, uint32_t length) +{ + uint8_t checksum = 0; + const uint8_t *byteOffset; + + for (byteOffset = data; byteOffset < (data + length); byteOffset++) + checksum ^= *byteOffset; + return checksum; +} + +bool isEEPROMContentValid(void) +{ + const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS; + uint8_t checksum = 0; + + // check version number + if (EEPROM_CONF_VERSION != temp->version) + return false; + + // check size and magic numbers + if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF) + return false; + + // verify integrity of temporary copy + checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t)); + if (checksum != 0) + return false; + + // looks good, let's roll! + return true; +} + +void writeEEPROM(void) +{ + // Generate compile time error if the config does not fit in the reserved area of flash. + BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG); + + FLASH_Status status = 0; + uint32_t wordOffset; + int8_t attemptsRemaining = 3; + + suspendRxSignal(); + + // prepare checksum/version constants + masterConfig.version = EEPROM_CONF_VERSION; + masterConfig.size = sizeof(master_t); + masterConfig.magic_be = 0xBE; + masterConfig.magic_ef = 0xEF; + masterConfig.chk = 0; // erase checksum before recalculating + masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t)); + + // write it + FLASH_Unlock(); + while (attemptsRemaining--) { +#ifdef STM32F40_41xxx + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); +#endif +#ifdef STM32F303 + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR); +#endif +#ifdef STM32F10X + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); +#endif + for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) { + if (wordOffset % FLASH_PAGE_SIZE == 0) { +#if defined(STM32F40_41xxx) + status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000 +#elif defined (STM32F411xE) + status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000 +#else + status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset); +#endif + if (status != FLASH_COMPLETE) { + break; + } + } + + status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset, + *(uint32_t *) ((char *) &masterConfig + wordOffset)); + if (status != FLASH_COMPLETE) { + break; + } + } + if (status == FLASH_COMPLETE) { + break; + } + } + FLASH_Lock(); + + // Flash write failed - just die now + if (status != FLASH_COMPLETE || !isEEPROMContentValid()) { + failureMode(FAILURE_FLASH_WRITE_FAILED); + } + + resumeRxSignal(); +} + +void readEEPROM(void) +{ + // Sanity check + if (!isEEPROMContentValid()) + failureMode(FAILURE_INVALID_EEPROM_CONTENTS); + + suspendRxSignal(); + + // Read flash + memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t)); + + if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check + masterConfig.current_profile_index = 0; + + setProfile(masterConfig.current_profile_index); + + if (currentProfile->defaultRateProfileIndex > MAX_CONTROL_RATE_PROFILE_COUNT - 1) // sanity check + currentProfile->defaultRateProfileIndex = 0; + + setControlRateProfile(currentProfile->defaultRateProfileIndex); + + validateAndFixConfig(); + activateConfig(); + + resumeRxSignal(); +} diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h new file mode 100644 index 00000000000..db2e3c1c110 --- /dev/null +++ b/src/main/config/config_eeprom.h @@ -0,0 +1,25 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define EEPROM_CONF_VERSION 122 + +void initEEPROM(void); +void writeEEPROM(void); +void readEEPROM(void); +bool isEEPROMContentValid(void); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 8e1b2a3f0cf..be9d7c9cd8c 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -17,8 +17,51 @@ #pragma once +#include +#include +#include + +#include "common/axis.h" +#include "common/color.h" +#include "common/maths.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" + +#include "fc/rc_controls.h" + +#include "io/gimbal.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/motors.h" +#include "io/serial.h" +#include "io/servos.h" + +#include "rx/rx.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/boardalignment.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" +#include "sensors/gyro.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/servos.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" + +#include "config/config.h" +#include "config/config_profile.h" +#include "config/config_master.h" + // System-wide -typedef struct master_t { +typedef struct master_s { uint8_t version; uint16_t size; uint8_t magic_be; // magic number, should be 0xBE @@ -36,12 +79,10 @@ typedef struct master_t { servoMixer_t customServoMixer[MAX_SERVO_RULES]; #endif // motor/esc/servo related stuff - escAndServoConfig_t escAndServoConfig; + motorConfig_t motorConfig; + servoConfig_t servoConfig; flight3DConfig_t flight3DConfig; - uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz) - uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz) - // global sensor-related stuff sensorAlignmentConfig_t sensorAlignmentConfig; @@ -74,12 +115,14 @@ typedef struct master_t { failsafeConfig_t failsafeConfig; + uint8_t fixed_wing_auto_arm; // Auto-arm fixed wing aircraft on throttle up and never disarm uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0 uint8_t small_angle; // mixer-related configuration mixerConfig_t mixerConfig; + servoMixerConfig_t servoMixerConfig; #ifdef GPS gpsConfig_t gpsConfig; diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index 50f9e660851..afcdf88dbb2 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -44,5 +44,6 @@ typedef struct profile_s { uint16_t flaperon_throw_offset; uint8_t flaperon_throw_inverted; + #endif } profile_t; diff --git a/src/main/config/feature.c b/src/main/config/feature.c new file mode 100755 index 00000000000..e98de617960 --- /dev/null +++ b/src/main/config/feature.c @@ -0,0 +1,112 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "common/axis.h" +#include "common/color.h" +#include "common/maths.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/gpio.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/rx_spi.h" +#include "drivers/serial.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/motors.h" +#include "io/servos.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" + +#include "rx/rx.h" +#include "rx/rx_spi.h" + +#include "blackbox/blackbox_io.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/servos.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/navigation_rewrite.h" + +#include "fc/runtime_config.h" + +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +static uint32_t activeFeaturesLatch = 0; + +void latchActiveFeatures() +{ + activeFeaturesLatch = masterConfig.enabledFeatures; +} + +bool featureConfigured(uint32_t mask) +{ + return masterConfig.enabledFeatures & mask; +} + +bool feature(uint32_t mask) +{ + return activeFeaturesLatch & mask; +} + +void featureSet(uint32_t mask) +{ + masterConfig.enabledFeatures |= mask; +} + +void featureClear(uint32_t mask) +{ + masterConfig.enabledFeatures &= ~(mask); +} + +void featureClearAll() +{ + masterConfig.enabledFeatures = 0; +} + +uint32_t featureMask(void) +{ + return masterConfig.enabledFeatures; +} diff --git a/src/main/config/feature.h b/src/main/config/feature.h new file mode 100644 index 00000000000..526d73c8391 --- /dev/null +++ b/src/main/config/feature.h @@ -0,0 +1,26 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +void latchActiveFeatures(void); +bool featureConfigured(uint32_t mask); +bool feature(uint32_t mask); +void featureSet(uint32_t mask); +void featureClear(uint32_t mask); +void featureClearAll(void); +uint32_t featureMask(void); diff --git a/src/main/config/parameter_group.c b/src/main/config/parameter_group.c new file mode 100644 index 00000000000..38d19962e9b --- /dev/null +++ b/src/main/config/parameter_group.c @@ -0,0 +1,118 @@ +/* + * 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 "parameter_group.h" +#include "common/maths.h" + +const pgRegistry_t* pgFind(pgn_t pgn) +{ + PG_FOREACH(reg) { + if (pgN(reg) == pgn) { + return reg; + } + } + return NULL; +} + +const pgRegistry_t* pgMatcher(pgMatcherFuncPtr matcher, const void *criteria) +{ + PG_FOREACH(candidate) { + if (matcher(candidate, criteria)) { + return candidate; + } + } + return NULL; +} + +static uint8_t *pgOffset(const pgRegistry_t* reg, uint8_t profileIndex) +{ + const uint16_t regSize = pgSize(reg); + + uint8_t *base = reg->address; + if (!pgIsSystem(reg)) { + base += (regSize * profileIndex); + } + return base; +} + +static void pgResetInstance(const pgRegistry_t *reg, uint8_t *base) +{ + const uint16_t regSize = pgSize(reg); + + memset(base, 0, regSize); + if(reg->reset.ptr >= (void*)__pg_resetdata_start && reg->reset.ptr < (void*)__pg_resetdata_end) { + // pointer points to resetdata section, to it is data template + memcpy(base, reg->reset.ptr, regSize); + } else if (reg->reset.fn) { + // reset function, call it + reg->reset.fn(base, regSize); + } +} + +void pgResetCurrent(const pgRegistry_t *reg) +{ + if(pgIsSystem(reg)) { + pgResetInstance(reg, reg->address); + } else { + pgResetInstance(reg, *reg->ptr); + } +} + +void pgLoad(const pgRegistry_t* reg, const void *from, int size, uint8_t profileIndex) +{ + pgResetInstance(reg,pgOffset(reg, profileIndex)); + + const int take = MIN(size, pgSize(reg)); + memcpy(pgOffset(reg, profileIndex), from, take); +} + +int pgStore(const pgRegistry_t* reg, void *to, int size, uint8_t profileIndex) +{ + const int take = MIN(size, pgSize(reg)); + memcpy(to, pgOffset(reg, profileIndex), take); + return take; +} + + +void pgResetAll(uint8_t profileCount) +{ + PG_FOREACH(reg) { + if (pgIsSystem(reg)) { + pgResetInstance(reg, reg->address); + } else { + // reset one instance for each profile + for (uint8_t profileIndex = 0; profileIndex < profileCount; profileIndex++) { + pgResetInstance(reg, pgOffset(reg, profileIndex)); + } + } + } +} + +void pgActivateProfile(uint8_t profileIndexToActivate) +{ + PG_FOREACH(reg) { + if (!pgIsSystem(reg)) { + uint8_t *ptr = pgOffset(reg, profileIndexToActivate); + *(reg->ptr) = ptr; + } + } +} + diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h new file mode 100644 index 00000000000..ddf3f4bb7b6 --- /dev/null +++ b/src/main/config/parameter_group.h @@ -0,0 +1,230 @@ +/* + * 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 uint16_t pgn_t; + +// parameter group registry flags +typedef enum { + PGRF_NONE = 0, + PGRF_CLASSIFICATON_BIT = (1 << 0), +} pgRegistryFlags_e; + +typedef enum { + PGR_PGN_MASK = 0x0fff, + PGR_PGN_VERSION_MASK = 0xf000, + PGR_SIZE_MASK = 0x0fff, + PGR_SIZE_SYSTEM_FLAG = 0x0000, // documentary + PGR_SIZE_PROFILE_FLAG = 0x8000, // start using flags from the top bit down +} pgRegistryInternal_e; + +// function that resets a single parameter group instance +typedef void (pgResetFunc)(void * /* base */, int /* size */); + +typedef struct pgRegistry_s { + pgn_t pgn; // The parameter group number, the top 4 bits are reserved for version + uint16_t size; // Size of the group in RAM, the top 4 bits are reserved for flags + uint8_t *address; // Address of the group in RAM. + uint8_t **ptr; // The pointer to update after loading the record into ram. + union { + void *ptr; // Pointer to init template + pgResetFunc *fn; // Popinter to pgResetFunc + } reset; +} pgRegistry_t; + +static inline uint16_t pgN(const pgRegistry_t* reg) {return reg->pgn & PGR_PGN_MASK;} +static inline uint8_t pgVersion(const pgRegistry_t* reg) {return reg->pgn >> 12;} +static inline uint16_t pgSize(const pgRegistry_t* reg) {return reg->size & PGR_SIZE_MASK;} +static inline uint16_t pgIsSystem(const pgRegistry_t* reg) {return (reg->size & PGR_SIZE_PROFILE_FLAG) == 0;} + +#define PG_PACKED __attribute__((packed)) + +#ifdef __APPLE__ +extern const pgRegistry_t __pg_registry_start[] __asm("section$start$__DATA$__pg_registry"); +extern const pgRegistry_t __pg_registry_end[] __asm("section$end$__DATA$__pg_registry"); +#define PG_REGISTER_ATTRIBUTES __attribute__ ((section("__DATA,__pg_registry"), used, aligned(4))) + +extern const uint8_t __pg_resetdata_start[] __asm("section$start$__DATA$__pg_resetdata"); +extern const uint8_t __pg_resetdata_end[] __asm("section$end$__DATA$__pg_resetdata"); +#define PG_RESETDATA_ATTRIBUTES __attribute__ ((section("__DATA,__pg_resetdata"), used, aligned(2))) +#else +extern const pgRegistry_t __pg_registry_start[]; +extern const pgRegistry_t __pg_registry_end[]; +#define PG_REGISTER_ATTRIBUTES __attribute__ ((section(".pg_registry"), used, aligned(4))) + +extern const uint8_t __pg_resetdata_start[]; +extern const uint8_t __pg_resetdata_end[]; +#define PG_RESETDATA_ATTRIBUTES __attribute__ ((section(".pg_resetdata"), used, aligned(2))) +#endif + +#define PG_REGISTRY_SIZE (__pg_registry_end - __pg_registry_start) + +// Helper to iterate over the PG register. Cheaper than a visitor style callback. +#define PG_FOREACH(_name) \ + for (const pgRegistry_t *(_name) = __pg_registry_start; (_name) < __pg_registry_end; _name++) + +#define PG_FOREACH_PROFILE(_name) \ + PG_FOREACH(_name) \ + if(pgIsSystem(_name)) \ + continue; \ + else \ + /**/ + +// Reset configuration to default (by name) +// Only current profile is reset for profile based configs +#define PG_RESET_CURRENT(_name) \ + do { \ + extern const pgRegistry_t _name ##_Registry; \ + pgResetCurrent(&_name ## _Registry); \ + } while(0) \ + /**/ + +// Declare system config +#define PG_DECLARE(_type, _name) \ + extern _type _name ## _System; \ + static inline _type* _name(void) { return &_name ## _System; } \ + struct _dummy \ + /**/ + +// Declare system config array +#define PG_DECLARE_ARR(_type, _size, _name) \ + extern _type _name ## _SystemArray[_size]; \ + static inline _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \ + static inline _type (* _name ## _arr(void))[_size] { return &_name ## _SystemArray; } \ + struct _dummy \ + /**/ + +// Declare profile config +#define PG_DECLARE_PROFILE(_type, _name) \ + extern _type *_name ## _ProfileCurrent; \ + static inline _type* _name(void) { return _name ## _ProfileCurrent; } \ + struct _dummy \ + /**/ + +// Register system config +#define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \ + _type _name ## _System; \ + /* Force external linkage for g++. Catch multi registration */ \ + extern const pgRegistry_t _name ## _Registry; \ + const pgRegistry_t _name ##_Registry PG_REGISTER_ATTRIBUTES = { \ + .pgn = _pgn | (_version << 12), \ + .size = sizeof(_type) | PGR_SIZE_SYSTEM_FLAG, \ + .address = (uint8_t*)&_name ## _System, \ + .ptr = 0, \ + _reset, \ + } \ + /**/ + +#define PG_REGISTER(_type, _name, _pgn, _version) \ + PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = 0}) \ + /**/ + +#define PG_REGISTER_WITH_RESET_FN(_type, _name, _pgn, _version) \ + extern void pgResetFn_ ## _name(_type *); \ + PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name }) \ + /**/ + +#define PG_REGISTER_WITH_RESET_TEMPLATE(_type, _name, _pgn, _version) \ + extern const _type pgResetTemplate_ ## _name; \ + PG_REGISTER_I(_type, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ + /**/ + +// Register system config array +#define PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, _reset) \ + _type _name ## _SystemArray[_size]; \ + extern const pgRegistry_t _name ##_Registry; \ + const pgRegistry_t _name ## _Registry PG_REGISTER_ATTRIBUTES = { \ + .pgn = _pgn | (_version << 12), \ + .size = (sizeof(_type) * _size) | PGR_SIZE_SYSTEM_FLAG, \ + .address = (uint8_t*)&_name ## _SystemArray, \ + .ptr = 0, \ + _reset, \ + } \ + /**/ + +#define PG_REGISTER_ARR(_type, _size, _name, _pgn, _version) \ + PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = 0}) \ + /**/ + +#define PG_REGISTER_ARR_WITH_RESET_FN(_type, _size, _name, _pgn, _version) \ + extern void pgResetFn_ ## _name(_type *); \ + PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \ + /**/ + +#if 0 +// ARRAY reset mechanism is not implemented yet, only few places in code would benefit from it. +#define PG_REGISTER_ARR_WITH_RESET_TEMPLATE(_type, _size, _name, _pgn, _version) \ + extern const _type pgResetTemplate_ ## _name; \ + PG_REGISTER_ARR_I(_type, _size, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ + /**/ +#endif + +#ifdef UNIT_TEST +# define _PG_PROFILE_CURRENT_DECL(_type, _name) \ + _type *_name ## _ProfileCurrent = &_name ## _Storage[0]; +#else +# define _PG_PROFILE_CURRENT_DECL(_type, _name) \ + _type *_name ## _ProfileCurrent; +#endif + +// register profile config +#define PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, _reset) \ + STATIC_UNIT_TESTED _type _name ## _Storage[MAX_PROFILE_COUNT]; \ + _PG_PROFILE_CURRENT_DECL(_type, _name) \ + extern const pgRegistry_t _name ## _Registry; \ + const pgRegistry_t _name ## _Registry PG_REGISTER_ATTRIBUTES = { \ + .pgn = _pgn | (_version << 12), \ + .size = sizeof(_type) | PGR_SIZE_PROFILE_FLAG, \ + .address = (uint8_t*)&_name ## _Storage, \ + .ptr = (uint8_t **)&_name ## _ProfileCurrent, \ + _reset, \ + } \ + /**/ + +#define PG_REGISTER_PROFILE(_type, _name, _pgn, _version) \ + PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, .reset = {.ptr = 0}) \ + /**/ + +#define PG_REGISTER_PROFILE_WITH_RESET_FN(_type, _name, _pgn, _version) \ + extern void pgResetFn_ ## _name(_type *); \ + PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, .reset = {.fn = (pgResetFunc*)&pgResetFn_ ## _name}) \ + /**/ + +#define PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(_type, _name, _pgn, _version) \ + extern const _type pgResetTemplate_ ## _name; \ + PG_REGISTER_PROFILE_I(_type, _name, _pgn, _version, .reset = {.ptr = (void*)&pgResetTemplate_ ## _name}) \ + /**/ + + +// Emit reset defaults for config. +// Config must be registered with PG_REGISTER__WITH_RESET_TEMPLATE macro +#define PG_RESET_TEMPLATE(_type, _name, ...) \ + const _type pgResetTemplate_ ## _name PG_RESETDATA_ATTRIBUTES = { \ + __VA_ARGS__ \ + } \ + /**/ + +typedef uint8_t (*pgMatcherFuncPtr)(const pgRegistry_t *candidate, const void *criteria); + +const pgRegistry_t* pgFind(pgn_t pgn); +const pgRegistry_t* pgMatcher(pgMatcherFuncPtr matcher, const void *criteria); +void pgLoad(const pgRegistry_t* reg, const void *from, int size, uint8_t profileIndex); +int pgStore(const pgRegistry_t* reg, void *to, int size, uint8_t profileIndex); +void pgResetAll(uint8_t profileCount); +void pgActivateProfile(uint8_t profileIndexToActivate); +void pgResetCurrent(const pgRegistry_t *reg); diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h new file mode 100644 index 00000000000..7ae8dd2576b --- /dev/null +++ b/src/main/config/parameter_group_ids.h @@ -0,0 +1,79 @@ +/* + * 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 . + */ + +// FC configuration +#define PG_FAILSAFE_CONFIG 1 +#define PG_BOARD_ALIGNMENT 2 +#define PG_GIMBAL_CONFIG 3 +#define PG_MOTOR_MIXER 4 +#define PG_BLACKBOX_CONFIG 5 +#define PG_MOTOR_AND_SERVO_CONFIG 6 +#define PG_SENSOR_SELECTION_CONFIG 7 +#define PG_SENSOR_ALIGNMENT_CONFIG 8 +#define PG_SENSOR_TRIMS 9 +#define PG_GYRO_CONFIG 10 +#define PG_BATTERY_CONFIG 11 +#define PG_CONTROL_RATE_PROFILES 12 +#define PG_SERIAL_CONFIG 13 +#define PG_PID_PROFILE 14 +#define PG_GTUNE_CONFIG 15 +#define PG_ARMING_CONFIG 16 +#define PG_TRANSPONDER_CONFIG 17 +#define PG_SYSTEM_CONFIG 18 +#define PG_FEATURE_CONFIG 19 +#define PG_MIXER_CONFIG 20 +#define PG_SERVO_MIXER 21 +#define PG_IMU_CONFIG 22 +#define PG_PROFILE_SELECTION 23 +#define PG_RX_CONFIG 24 +#define PG_RC_CONTROLS_CONFIG 25 +#define PG_MOTOR_3D_CONFIG 26 +#define PG_LED_STRIP_CONFIG 27 +#define PG_COLOR_CONFIG 28 +#define PG_AIRPLANE_ALT_HOLD_CONFIG 29 +#define PG_GPS_CONFIG 30 +#define PG_TELEMETRY_CONFIG 31 +#define PG_FRSKY_TELEMETRY_CONFIG 32 +#define PG_HOTT_TELEMETRY_CONFIG 33 +#define PG_NAVIGATION_CONFIG 34 +#define PG_ACCELEROMETER_CONFIG 35 +#define PG_RATE_PROFILE_SELECTION 36 +#define PG_ADJUSTMENT_PROFILE 37 +#define PG_BAROMETER_CONFIG 38 +#define PG_THROTTLE_CORRECTION_CONFIG 39 +#define PG_COMPASS_CONFIGURATION 40 +#define PG_MODE_ACTIVATION_PROFILE 41 +#define PG_SERVO_PROFILE 42 +#define PG_FAILSAFE_CHANNEL_CONFIG 43 +#define PG_CHANNEL_RANGE_CONFIG 44 +#define PG_MODE_COLOR_CONFIG 45 +#define PG_SPECIAL_COLOR_CONFIG 46 + +// Driver configuration +#define PG_DRIVER_PWM_RX_CONFIG 100 +#define PG_DRIVER_FLASHCHIP_CONFIG 101 + +// OSD configuration (subject to change) +#define PG_OSD_FONT_CONFIG 32768 +#define PG_OSD_VIDEO_CONFIG 32769 +#define PG_OSD_ELEMENT_CONFIG 32770 + + +#define PG_RESERVED_FOR_TESTING_1 65533 +#define PG_RESERVED_FOR_TESTING_2 65534 +#define PG_RESERVED_FOR_TESTING_3 65535 + diff --git a/src/main/drivers/barometer_spi_bmp280.c b/src/main/drivers/barometer_spi_bmp280.c new file mode 100644 index 00000000000..3183157499a --- /dev/null +++ b/src/main/drivers/barometer_spi_bmp280.c @@ -0,0 +1,93 @@ +/* + * This file is part of Betaflight. + * + * Betaflight 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. + * + * Betaflight 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 Betaflight. If not, see . + */ + +#include +#include + +#include + +#include "io.h" +#include "bus_spi.h" + +#include "barometer.h" +#include "barometer_bmp280.h" + +#ifdef USE_BARO_SPI_BMP280 +#define DISABLE_BMP280 IOHi(bmp280CsPin) +#define ENABLE_BMP280 IOLo(bmp280CsPin) + +extern int32_t bmp280_up; +extern int32_t bmp280_ut; + +static IO_t bmp280CsPin = IO_NONE; + +bool bmp280WriteRegister(uint8_t reg, uint8_t data) +{ + ENABLE_BMP280; + spiTransferByte(BMP280_SPI_INSTANCE, reg & 0x7F); + spiTransferByte(BMP280_SPI_INSTANCE, data); + DISABLE_BMP280; + + return true; +} + +bool bmp280ReadRegister(uint8_t reg, uint8_t length, uint8_t *data) +{ + ENABLE_BMP280; + spiTransferByte(BMP280_SPI_INSTANCE, reg | 0x80); // read transaction + spiTransfer(BMP280_SPI_INSTANCE, data, NULL, length); + DISABLE_BMP280; + + return true; +} + +void bmp280SpiInit(void) +{ + static bool hardwareInitialised = false; + + if (hardwareInitialised) { + return; + } + + bmp280CsPin = IOGetByTag(IO_TAG(BMP280_CS_PIN)); + IOInit(bmp280CsPin, OWNER_BARO, RESOURCE_SPI_CS, 0); + IOConfigGPIO(bmp280CsPin, IOCFG_OUT_PP); + + DISABLE_BMP280; + + spiSetDivisor(BMP280_SPI_INSTANCE, SPI_CLOCK_STANDARD); + + hardwareInitialised = true; +} + +void bmp280_spi_start_up(void) +{ + // start measurement + // set oversampling + power mode (forced), and start sampling + bmp280WriteRegister(BMP280_CTRL_MEAS_REG, BMP280_MODE); +} + +void bmp280_spi_get_up(void) +{ + uint8_t data[BMP280_DATA_FRAME_SIZE]; + + // read data from sensor + bmp280ReadRegister(BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data); + bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4)); + bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4)); +} +#endif diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index 5876651f48b..10c7e3d55d5 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -19,7 +19,7 @@ #define I2C_SHORT_TIMEOUT ((uint32_t)0x1000) #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) -#define I2C_DEFAULT_TIMEOUT I2C_SHORT_TIMEOUT +#define I2C_DEFAULT_TIMEOUT I2C_LONG_TIMEOUT #include "drivers/io.h" #include "drivers/rcc.h" @@ -51,6 +51,7 @@ typedef struct i2cDevice_s { typedef struct i2cState_s { volatile bool initialised; volatile bool error; + volatile bool busError; volatile bool busy; volatile uint8_t addr; volatile uint8_t reg; diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c index f9882169f54..ef8b57ff295 100644 --- a/src/main/drivers/bus_i2c_stm32f10x.c +++ b/src/main/drivers/bus_i2c_stm32f10x.c @@ -94,16 +94,16 @@ static i2cDevice_t i2cHardwareMap[] = { static volatile uint16_t i2cErrorCount = 0; static i2cState_t i2cState[] = { - { false, false, false, 0, 0, 0, 0, 0, 0, 0 }, - { false, false, false, 0, 0, 0, 0, 0, 0, 0 }, - { false, false, false, 0, 0, 0, 0, 0, 0, 0 } + { false, false, false, 0, 0, 0, 0, 0, 0, 0, 0 }, + { false, false, false, 0, 0, 0, 0, 0, 0, 0, 0 }, + { false, false, false, 0, 0, 0, 0, 0, 0, 0, 0 } }; -static bool i2cOverClock; - void i2cSetOverclock(uint8_t overClock) { - i2cOverClock = overClock ? true : false; + for (unsigned int i = 0; i < sizeof(i2cHardwareMap) / sizeof(i2cHardwareMap[0]); i++) { + i2cHardwareMap[i].overClock = overClock; + } } void I2C1_ER_IRQHandler(void) { @@ -134,9 +134,15 @@ void I2C3_EV_IRQHandler(void) { static bool i2cHandleHardwareFailure(I2CDevice device) { + const i2cState_t *state = &(i2cState[device]); + i2cErrorCount++; + // reinit peripheral + clock out garbage - i2cInit(device); + if (state->busError) { + i2cInit(device); + } + return false; } @@ -166,11 +172,12 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, state->bytes = len_; state->busy = 1; state->error = false; + state->busError = false; if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver if (!(I2Cx->CR1 & I2C_CR1_START)) { // ensure sending a start while (I2Cx->CR1 & I2C_CR1_STOP && --timeout > 0) {; } // wait for any stop to finish sending - if (timeout == 0) + if (state->error || timeout == 0) return i2cHandleHardwareFailure(device); I2C_GenerateSTART(I2Cx, ENABLE); // send the start for the new job } @@ -182,7 +189,7 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, if (timeout == 0) return i2cHandleHardwareFailure(device); - return !(state->error); + return !state->error; } bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data) @@ -215,11 +222,12 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t state->bytes = len; state->busy = 1; state->error = false; + state->busError = false; if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver if (!(I2Cx->CR1 & I2C_CR1_START)) { // ensure sending a start while (I2Cx->CR1 & I2C_CR1_STOP && --timeout > 0) {; } // wait for any stop to finish sending - if (timeout == 0) + if (state->error || timeout == 0) return i2cHandleHardwareFailure(device); I2C_GenerateSTART(I2Cx, ENABLE); // send the start for the new job } @@ -228,10 +236,10 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t timeout = I2C_DEFAULT_TIMEOUT; while (state->busy && --timeout > 0) {; } - if (timeout == 0) + if (state->error || timeout == 0) return i2cHandleHardwareFailure(device); - return !(state->error); + return true; } static void i2c_er_handler(I2CDevice device) { @@ -245,9 +253,14 @@ static void i2c_er_handler(I2CDevice device) { // Read the I2C1 status register volatile uint32_t SR1Register = I2Cx->SR1; - if (SR1Register & 0x0F00) // an error + if (SR1Register & 0x0F00) { // an error state->error = true; + // Only re-initialise bus if bus error indicated, don't reset bus when ARLO or AF + if (SR1Register & I2C_SR1_BERR) + state->busError = true; + } + // If AF, BERR or ARLO, abandon the current job and commence new if there are jobs if (SR1Register & 0x0700) { (void)I2Cx->SR2; // read second status register to clear ADDR if it is set (note that BTF will not be set after a NACK) @@ -468,7 +481,6 @@ uint16_t i2cGetErrorCounter(void) static void i2cUnstick(IO_t scl, IO_t sda) { int i; - int timeout = 100; IOHi(scl); IOHi(sda); @@ -476,27 +488,31 @@ static void i2cUnstick(IO_t scl, IO_t sda) IOConfigGPIO(scl, IOCFG_OUT_OD); IOConfigGPIO(sda, IOCFG_OUT_OD); - for (i = 0; i < 8; i++) { + // Analog Devices AN-686 + // We need 9 clock pulses + STOP condition + for (i = 0; i < 9; i++) { // Wait for any clock stretching to finish + int timeout = 100; while (!IORead(scl) && timeout) { - delayMicroseconds(10); + delayMicroseconds(5); timeout--; } // Pull low IOLo(scl); // Set bus low - delayMicroseconds(10); + delayMicroseconds(5); IOHi(scl); // Set bus high - delayMicroseconds(10); + delayMicroseconds(5); } - // Generate a start then stop condition - IOLo(sda); // Set bus data low - delayMicroseconds(10); - IOLo(scl); // Set bus scl low - delayMicroseconds(10); + // Generate a stop condition in case there was none + IOLo(scl); + delayMicroseconds(5); + IOLo(sda); + delayMicroseconds(5); + IOHi(scl); // Set bus scl high - delayMicroseconds(10); + delayMicroseconds(5); IOHi(sda); // Set bus sda high } diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 531b431874f..9434b09e83c 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -70,11 +70,11 @@ static bool deviceInitialised[] = { false }; -static bool i2cOverClock; - void i2cSetOverclock(uint8_t overClock) { - i2cOverClock = overClock ? true : false; + for (unsigned i = 0; i < sizeof(i2cHardwareMap) / sizeof(i2cHardwareMap[0]); i++) { + i2cHardwareMap[i].overClock = overClock; + } } /////////////////////////////////////////////////////////////////////////////// diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 3006ccdc7cd..cf0bd143b6a 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -108,8 +108,8 @@ void spiInitDevice(SPIDevice device) spi->sdcard = true; } #endif -#ifdef NRF24_SPI_INSTANCE - if (spi->dev == NRF24_SPI_INSTANCE) { +#ifdef RX_SPI_INSTANCE + if (spi->dev == RX_SPI_INSTANCE) { spi->nrf24l01 = true; } #endif diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index 8838f35e282..b58dc7d5613 100755 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -342,9 +342,9 @@ bool ak8963Read(int16_t *magData) return lastReadResult; } - magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magGain[X]; - magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magGain[Y]; - magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magGain[Z]; + magData[X] = (int16_t)(buf[1] << 8 | buf[0]) * magGain[X]; + magData[Y] = (int16_t)(buf[3] << 8 | buf[2]) * magGain[Y]; + magData[Z] = (int16_t)(buf[5] << 8 | buf[4]) * magGain[Z]; #if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE) // cache mag data for reuse diff --git a/src/main/drivers/io_pca9685.c b/src/main/drivers/io_pca9685.c new file mode 100644 index 00000000000..a002505015c --- /dev/null +++ b/src/main/drivers/io_pca9685.c @@ -0,0 +1,129 @@ +#include +#include + +#include + +#include "gpio.h" +#include "system.h" +#include "bus_i2c.h" + +#include "common/maths.h" + +#include "config/config.h" +#include "fc/runtime_config.h" + +#define PCA9685_ADDR 0x40 +#define PCA9685_MODE1 0x00 +#define PCA9685_PRESCALE 0xFE + +#define LED0_ON_L 0x06 +#define LED0_ON_H 0x07 +#define LED0_OFF_L 0x08 +#define LED0_OFF_H 0x09 + +#define PCA9685_SERVO_FREQUENCY 60 +#define PCA9685_SERVO_COUNT 16 +#define PCA9685_SYNC_THRESHOLD 5 + +uint16_t currentOutputState[PCA9685_SERVO_FREQUENCY] = {0}; +uint16_t temporaryOutputState[PCA9685_SERVO_FREQUENCY] = {0}; + +void pca9685setPWMOn(uint8_t servoIndex, uint16_t on) { + if (servoIndex < PCA9685_SERVO_COUNT) { + i2cWrite(I2C_DEVICE, PCA9685_ADDR, LED0_ON_L + (servoIndex * 4), on); + i2cWrite(I2C_DEVICE, PCA9685_ADDR, LED0_ON_H + (servoIndex * 4), on>>8); + } +} + +void pca9685setPWMOff(uint8_t servoIndex, uint16_t off) { + if (servoIndex < PCA9685_SERVO_COUNT) { + i2cWrite(I2C_DEVICE, PCA9685_ADDR, LED0_OFF_L + (servoIndex * 4), off); + i2cWrite(I2C_DEVICE, PCA9685_ADDR, LED0_OFF_H + (servoIndex * 4), off>>8); + } +} + +/* +Writing new state every cycle for each servo is extremely time consuming +and does not makes sense. +On Flip32/Naze32 trying to sync 5 servos every 2000us extends looptime +to 3500us. Very, very bad... +Instead of that, write desired values to temporary +table and write it to PCA9685 only when there a need. +Also, because of resultion of PCA9685 internal counter of about 5us, do not write +small changes, since thwy will only clog the bandwidch and will not +be represented on output +PWM Driver runs at 200Hz, every cycle every 4th servo output is updated: +cycle 0: servo 0, 4, 8, 12 +cycle 1: servo 1, 5, 9, 13 +cycle 2: servo 2, 6, 10, 14 +cycle 3: servo3, 7, 11, 15 +*/ +void pca9685sync(uint8_t cycleIndex) { + uint8_t i; + + for (i = 0; i < PCA9685_SERVO_COUNT; i++) { + if (cycleIndex == i % 4 && ABS(temporaryOutputState[i] - currentOutputState[i]) > PCA9685_SYNC_THRESHOLD) { + pca9685setPWMOff(i, temporaryOutputState[i]); + currentOutputState[i] = temporaryOutputState[i]; + } + } +} + +void pca9685setServoPulse(uint8_t servoIndex, uint16_t pulse) { + + static double pulselength = 0; + + if (pulselength == 0) { + pulselength = 1000000; // 1,000,000 us per second + pulselength /= PCA9685_SERVO_FREQUENCY; + pulselength /= 4096; // 12 bits of resolution + } + pulse /= pulselength; + + temporaryOutputState[servoIndex] = pulse; +} + +void pca9685setPWMFreq(uint16_t freq) { + + uint32_t prescaleval = 25000000; + prescaleval /= 4096; + prescaleval /= freq; + prescaleval -= 1; + + i2cWrite(I2C_DEVICE, PCA9685_ADDR, PCA9685_MODE1, 16); + delay(1); + i2cWrite(I2C_DEVICE, PCA9685_ADDR, PCA9685_PRESCALE, (uint8_t) prescaleval); + delay(1); + i2cWrite(I2C_DEVICE, PCA9685_ADDR, PCA9685_MODE1, 128); +} + +bool pca9685Initialize(void) { + + bool ack = false; + uint8_t sig; + + ack = i2cRead(I2C_DEVICE, PCA9685_ADDR, PCA9685_MODE1, 1, &sig); + + if (!ack) { + return false; + } else { + /* + Reset device + */ + i2cWrite(I2C_DEVICE, PCA9685_ADDR, PCA9685_MODE1, 0x0); + + /* + Set refresh rate + */ + pca9685setPWMFreq(PCA9685_SERVO_FREQUENCY); + + delay(1); + + for (uint8_t i = 0; i < PCA9685_SERVO_COUNT; i++) { + pca9685setPWMOn(i, 0); + pca9685setPWMOff(i, 1500); + } + + return true; + } +} diff --git a/src/main/drivers/io_pca9685.h b/src/main/drivers/io_pca9685.h new file mode 100644 index 00000000000..2e37b34a6f5 --- /dev/null +++ b/src/main/drivers/io_pca9685.h @@ -0,0 +1,6 @@ +bool pca9685Initialize(void); +void pca9685setPWMOn(uint8_t servoIndex, uint16_t on); +void pca9685setPWMOff(uint8_t servoIndex, uint16_t off); +void pca9685setPWMFreq(float freq); +void pca9685setServoPulse(uint8_t servoIndex, uint16_t pulse); +void pca9685sync(uint8_t cycleIndex); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 62e0de1bcde..164380a691d 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -32,9 +32,7 @@ #include "pwm_rx.h" #include "pwm_mapping.h" -void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex); +void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto); void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); /* @@ -331,12 +329,12 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) if (type == MAP_TO_PPM_INPUT) { #ifndef SKIP_RX_PWM_PPM #ifdef CC3D_PPM1 - if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) { + if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) { ppmAvoidPWMTimerClash(timerHardwarePtr, TIM4); } #endif #if defined(SPARKY) || defined(ALIENFLIGHTF3) - if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) { + if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) { ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2); } #endif @@ -363,7 +361,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) } #if defined(CC3D) && !defined(CC3D_PPM1) - if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) { + if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) { // Skip it if it would cause PPM capture timer to be reconfigured or manually overflowed if (timerHardwarePtr->tim == TIM2) { addBootlogEvent6(BOOT_EVENT_TIMER_CH_SKIPPED, BOOT_EVENT_FLAGS_WARNING, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 3); @@ -371,19 +369,13 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) } } #endif - if (init->useOneshot) { - pwmOneshotMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount); - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_ONESHOT|PWM_PF_OUTPUT_PROTOCOL_PWM; - - } else if (isMotorBrushed(init->motorPwmRate)) { - - pwmBrushedMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->idlePulse); + pwmMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->pwmProtocolType); + if (init->useFastPwm) { + pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_FASTPWM | PWM_PF_OUTPUT_PROTOCOL_PWM; + } else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) { pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM; - } else { - - pwmBrushlessMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->idlePulse); pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM ; } diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 1e265d3ffcb..84497c6fca0 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -38,16 +38,12 @@ #define MAX_SERVOS 8 #endif +#define PWM_TIMER_MHZ 1 #define PULSE_1MS (1000) // 1ms pulse width #define MAX_INPUTS 8 -#define PWM_TIMER_MHZ 1 -#define ONESHOT125_TIMER_MHZ 8 -#define PWM_BRUSHED_TIMER_MHZ 8 - - typedef struct sonarIOConfig_s { ioTag_t triggerTag; ioTag_t echoTag; @@ -63,7 +59,7 @@ typedef struct drv_pwm_config_s { bool useUART3; bool useUART6; bool useVbat; - bool useOneshot; + bool useFastPwm; bool useSoftSerial; bool useLEDStrip; #ifdef SONAR @@ -76,6 +72,7 @@ typedef struct drv_pwm_config_s { uint16_t servoCenterPulse; #endif bool airplane; // fixed wing hardware config, lots of servos etc + uint8_t pwmProtocolType; uint16_t motorPwmRate; uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), // some higher value (used by 3d mode), or 0, for brushed pwm drivers. @@ -96,7 +93,7 @@ typedef enum { PWM_PF_SERVO = (1 << 1), PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2), PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3), - PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4), + PWM_PF_OUTPUT_PROTOCOL_FASTPWM = (1 << 4), PWM_PF_PPM = (1 << 5), PWM_PF_PWM = (1 << 6) } pwmPortFlags_e; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index d5038c2d09a..ce6e0f1b787 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -17,17 +17,37 @@ #include #include - -#include +#include #include "platform.h" -#include "gpio.h" #include "io.h" -#include "io_impl.h" #include "timer.h" #include "pwm_mapping.h" #include "pwm_output.h" +#include "io_pca9685.h" + +#include "io/pwmdriver_i2c.h" + +#include "config/config.h" +#include "config/feature.h" + +#include "fc/runtime_config.h" + +#if defined(STM32F40_41xxx) // must be multiples of timer clock +#define ONESHOT125_TIMER_MHZ 12 +#define ONESHOT42_TIMER_MHZ 21 +#define MULTISHOT_TIMER_MHZ 84 +#define PWM_BRUSHED_TIMER_MHZ 21 +#else +#define ONESHOT125_TIMER_MHZ 8 +#define ONESHOT42_TIMER_MHZ 24 +#define MULTISHOT_TIMER_MHZ 72 +#define PWM_BRUSHED_TIMER_MHZ 24 +#endif + +#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) +#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors @@ -88,22 +108,15 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 } } -static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode) -{ - gpio_config_t cfg; - - cfg.pin = pin; - cfg.mode = mode; - cfg.speed = Speed_2MHz; - gpioInit(gpio, &cfg); -} - static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value) { pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++]; configTimeBase(timerHardware->tim, period, mhz); - pwmGPIOConfig(IO_GPIOBYTAG(timerHardware->tag), IO_PINBYTAG(timerHardware->tag), Mode_AF_PP); + + const IO_t io = IOGetByTag(timerHardware->tag); + IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount); + IOConfigGPIO(io, IOCFG_AF_PP); pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output & TIMER_OUTPUT_INVERTED); if (timerHardware->output & TIMER_OUTPUT_ENABLED) { @@ -129,7 +142,7 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 p->tim = timerHardware->tim; *p->ccr = 0; - + return p; } @@ -143,6 +156,21 @@ static void pwmWriteStandard(uint8_t index, uint16_t value) *motors[index]->ccr = value; } +static void pwmWriteOneShot125(uint8_t index, uint16_t value) +{ + *motors[index]->ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f)); +} + +static void pwmWriteOneShot42(uint8_t index, uint16_t value) +{ + *motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); +} + +static void pwmWriteMultiShot(uint8_t index, uint16_t value) +{ + *motors[index]->ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); +} + void pwmWriteMotor(uint8_t index, uint16_t value) { if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) { @@ -168,47 +196,48 @@ void pwmEnableMotors(void) pwmMotorsEnabled = true; } -void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) -{ - TIM_TypeDef *lastTimerPtr = NULL; - - for (int index = 0; index < motorCount; index++) { - - // Force the timer to overflow if it's the first motor to output, or if we change timers - if (motors[index]->tim != lastTimerPtr) { - lastTimerPtr = motors[index]->tim; - timerForceOverflow(motors[index]->tim); - } - - // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. - // This compare register will be set to the output value on the next main loop. - *motors[index]->ccr = 0; - } -} - bool isMotorBrushed(uint16_t motorPwmRate) { return (motorPwmRate > 500); } -void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) +void pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto) { - const uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; - motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, idlePulse); - motors[motorIndex]->pwmWritePtr = pwmWriteBrushed; -} + uint32_t timerMhzCounter; + pwmWriteFuncPtr pwmWritePtr; -void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) -{ - const uint32_t hz = PWM_TIMER_MHZ * 1000000; - motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse); - motors[motorIndex]->pwmWritePtr = pwmWriteStandard; -} + switch (proto) { + case PWM_TYPE_BRUSHED: + timerMhzCounter = PWM_BRUSHED_TIMER_MHZ; + pwmWritePtr = pwmWriteBrushed; + idlePulse = 0; + break; + + case PWM_TYPE_ONESHOT125: + timerMhzCounter = ONESHOT125_TIMER_MHZ; + pwmWritePtr = pwmWriteOneShot125; + break; + + case PWM_TYPE_ONESHOT42: + timerMhzCounter = ONESHOT42_TIMER_MHZ; + pwmWritePtr = pwmWriteOneShot42; + break; + + case PWM_TYPE_MULTISHOT: + timerMhzCounter = MULTISHOT_TIMER_MHZ; + pwmWritePtr = pwmWriteMultiShot; + break; + + case PWM_TYPE_STANDARD: + default: + timerMhzCounter = PWM_TIMER_MHZ; + pwmWritePtr = pwmWriteStandard; + break; + } -void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex) -{ - motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT125_TIMER_MHZ, 0xFFFF, 0); - motors[motorIndex]->pwmWritePtr = pwmWriteStandard; + const uint32_t hz = timerMhzCounter * 1000000; + motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse); + motors[motorIndex]->pwmWritePtr = pwmWritePtr; } #ifdef USE_SERVOS @@ -219,8 +248,18 @@ void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, ui void pwmWriteServo(uint8_t index, uint16_t value) { +#ifdef USE_PMW_SERVO_DRIVER + + if (feature(FEATURE_PWM_SERVO_DRIVER)) { + pwmDriverSetPulse(index, value); + } else if (servos[index] && index < MAX_SERVOS) { + *servos[index]->ccr = value; + } + +#else if (servos[index] && index < MAX_SERVOS) { *servos[index]->ccr = value; } +#endif } #endif diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 7106f94a37f..ecdd0768a11 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -17,13 +17,18 @@ #pragma once +typedef enum { + PWM_TYPE_STANDARD = 0, + PWM_TYPE_ONESHOT125, + PWM_TYPE_ONESHOT42, + PWM_TYPE_MULTISHOT, + PWM_TYPE_BRUSHED +} motorPwmProtocolTypes_e; + void pwmWriteMotor(uint8_t index, uint16_t value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); -void pwmCompleteOneshotMotorUpdate(uint8_t motorCount); void pwmWriteServo(uint8_t index, uint16_t value); -bool isMotorBrushed(uint16_t motorPwmRate); - void pwmDisableMotors(void); void pwmEnableMotors(void); diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 38fa90d1132..225fb35ae1a 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -180,7 +180,6 @@ static void ppmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t ca if (capture == PPM_TIMER_PERIOD - 1) { ppmDev.overflowed = true; } - } static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture) diff --git a/src/main/drivers/pwm_rx.h b/src/main/drivers/pwm_rx.h index 46afd388c5e..ff9b7076898 100644 --- a/src/main/drivers/pwm_rx.h +++ b/src/main/drivers/pwm_rx.h @@ -24,11 +24,11 @@ typedef enum { #define PPM_RCVR_TIMEOUT 0 +struct timerHardware_s; +void ppmInConfig(const struct timerHardware_s *timerHardwarePtr); +void ppmAvoidPWMTimerClash(const struct timerHardware_s *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer); -void ppmInConfig(const timerHardware_t *timerHardwarePtr); -void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer); - -void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel); +void pwmInConfig(const struct timerHardware_s *timerHardwarePtr, uint8_t channel); uint16_t pwmRead(uint8_t channel); uint16_t ppmRead(uint8_t channel); diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 6e0314e097e..636f5db1465 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -31,7 +31,7 @@ typedef enum { OWNER_RX, OWNER_TX, OWNER_SOFTSPI, - OWNER_NRF24, + OWNER_RX_SPI, OWNER_TOTAL_COUNT } resourceOwner_t; @@ -48,7 +48,7 @@ typedef enum { RESOURCE_I2C_SCL, RESOURCE_I2C_SDA, RESOURCE_SPI_SCK, RESOURCE_SPI_MOSI, RESOURCE_SPI_MISO, RESOURCE_SPI_CS, RESOURCE_ADC_BATTERY, RESOURCE_ADC_RSSI, RESOURCE_ADC_EXTERNAL1, RESOURCE_ADC_CURRENT, - RESOURCE_NRF24_CE, + RESOURCE_RX_CE, RESOURCE_TOTAL_COUNT } resourceType_t; diff --git a/src/main/drivers/rx_nrf24l01.c b/src/main/drivers/rx_nrf24l01.c index 55b92e2ce3d..9f13b6362cb 100644 --- a/src/main/drivers/rx_nrf24l01.c +++ b/src/main/drivers/rx_nrf24l01.c @@ -33,94 +33,12 @@ #include "io.h" #include "io_impl.h" #include "rcc.h" +#include "rx_spi.h" #include "rx_nrf24l01.h" - -#ifdef UNIT_TEST - -#define NRF24_CE_HI() {} -#define NRF24_CE_LO() {} -void NRF24L01_SpiInit(void) {} - -#else - #include "bus_spi.h" -#include "bus_spi_soft.h" - -#define DISABLE_NRF24() {IOHi(IOGetByTag(IO_TAG(NRF24_CSN_PIN)));} -#define ENABLE_NRF24() {IOLo(IOGetByTag(IO_TAG(NRF24_CSN_PIN)));} -#define NRF24_CE_HI() {IOHi(IOGetByTag(IO_TAG(NRF24_CE_PIN)));} -#define NRF24_CE_LO() {IOLo(IOGetByTag(IO_TAG(NRF24_CE_PIN)));} -#ifdef USE_NRF24_SOFTSPI -static const softSPIDevice_t softSPIDevice = { - .sckTag = IO_TAG(NRF24_SCK_PIN), - .mosiTag = IO_TAG(NRF24_MOSI_PIN), - .misoTag = IO_TAG(NRF24_MISO_PIN), - // Note: Nordic Semiconductor uses 'CSN', STM uses 'NSS' - .nssTag = IO_TAG(NRF24_CSN_PIN), -}; -#endif // USE_NRF24_SOFTSPI - -#ifdef USE_NRF24_SOFTSPI -static bool useSoftSPI = false; -#endif -void NRF24L01_SpiInit(nfr24l01_spi_type_e spiType) -{ - static bool hardwareInitialised = false; - - if (hardwareInitialised) { - return; - } - -#ifdef USE_NRF24_SOFTSPI - if (spiType == NFR24L01_SOFTSPI) { - useSoftSPI = true; - softSpiInit(&softSPIDevice); - } - const SPIDevice nrf24SPIDevice = SOFT_SPIDEV_1; -#else - UNUSED(spiType); - const SPIDevice nrf24SPIDevice = spiDeviceByInstance(NRF24_SPI_INSTANCE); - IOInit(IOGetByTag(IO_TAG(NRF24_CSN_PIN)), OWNER_SPI, RESOURCE_SPI_CS, nrf24SPIDevice + 1); -#endif // USE_NRF24_SOFTSPI - -#if defined(STM32F10X) - RCC_AHBPeriphClockCmd(NRF24_CSN_GPIO_CLK_PERIPHERAL, ENABLE); - RCC_AHBPeriphClockCmd(NRF24_CE_GPIO_CLK_PERIPHERAL, ENABLE); -#endif - - // CE as OUTPUT - IOInit(IOGetByTag(IO_TAG(NRF24_CE_PIN)), OWNER_NRF24, RESOURCE_NRF24_CE, nrf24SPIDevice + 1); -#if defined(STM32F10X) - IOConfigGPIO(IOGetByTag(IO_TAG(NRF24_CE_PIN)), SPI_IO_CS_CFG); -#elif defined(STM32F3) || defined(STM32F4) - IOConfigGPIOAF(IOGetByTag(IO_TAG(NRF24_CE_PIN)), SPI_IO_CS_CFG, 0); -#endif - - DISABLE_NRF24(); - NRF24_CE_LO(); - -#ifdef NRF24_SPI_INSTANCE - spiSetDivisor(NRF24_SPI_INSTANCE, SPI_CLOCK_STANDARD); -#endif - hardwareInitialised = true; -} - -uint8_t nrf24TransferByte(uint8_t data) -{ -#ifdef USE_NRF24_SOFTSPI - if (useSoftSPI) { - return softSpiTransferByte(&softSPIDevice, data); - } else -#endif - { -#ifdef NRF24_SPI_INSTANCE - return spiTransferByte(NRF24_SPI_INSTANCE, data); -#else - return 0; -#endif - } -} +#define NRF24_CE_HI() {IOHi(IOGetByTag(IO_TAG(RX_CE_PIN)));} +#define NRF24_CE_LO() {IOLo(IOGetByTag(IO_TAG(RX_CE_PIN)));} // Instruction Mnemonics // nRF24L01: Table 16. Command set for the nRF24L01 SPI. Product Specification, p46 @@ -140,27 +58,12 @@ uint8_t nrf24TransferByte(uint8_t data) uint8_t NRF24L01_WriteReg(uint8_t reg, uint8_t data) { - ENABLE_NRF24(); - nrf24TransferByte(W_REGISTER | (REGISTER_MASK & reg)); - nrf24TransferByte(data); - DISABLE_NRF24(); - return true; -} - -static uint8_t NRF24L01_WriteMulti(uint8_t type, const uint8_t *data, uint8_t length) -{ - ENABLE_NRF24(); - const uint8_t ret = nrf24TransferByte(type); - for (uint8_t i = 0; i < length; i++) { - nrf24TransferByte(data[i]); - } - DISABLE_NRF24(); - return ret; + return rxSpiWriteCommand(W_REGISTER | (REGISTER_MASK & reg), data); } uint8_t NRF24L01_WriteRegisterMulti(uint8_t reg, const uint8_t *data, uint8_t length) { - return NRF24L01_WriteMulti(W_REGISTER | ( REGISTER_MASK & reg), data, length); + return rxSpiWriteCommandMulti(W_REGISTER | ( REGISTER_MASK & reg), data, length); } /* @@ -170,37 +73,22 @@ uint8_t NRF24L01_WriteRegisterMulti(uint8_t reg, const uint8_t *data, uint8_t le */ uint8_t NRF24L01_WritePayload(const uint8_t *data, uint8_t length) { - return NRF24L01_WriteMulti(W_TX_PAYLOAD, data, length); + return rxSpiWriteCommandMulti(W_TX_PAYLOAD, data, length); } uint8_t NRF24L01_WriteAckPayload(const uint8_t *data, uint8_t length, uint8_t pipe) { - return NRF24L01_WriteMulti(W_ACK_PAYLOAD | (pipe & 0x07), data, length); + return rxSpiWriteCommandMulti(W_ACK_PAYLOAD | (pipe & 0x07), data, length); } uint8_t NRF24L01_ReadReg(uint8_t reg) { - ENABLE_NRF24(); - nrf24TransferByte(R_REGISTER | (REGISTER_MASK & reg)); - const uint8_t ret = nrf24TransferByte(NOP); - DISABLE_NRF24(); - return ret; -} - -static uint8_t NRF24L01_ReadMulti(uint8_t type, uint8_t *data, uint8_t length) -{ - ENABLE_NRF24(); - const uint8_t ret = nrf24TransferByte(type); - for (uint8_t i = 0; i < length; i++) { - data[i] = nrf24TransferByte(NOP); - } - DISABLE_NRF24(); - return ret; + return rxSpiReadCommand(R_REGISTER | (REGISTER_MASK & reg), NOP); } uint8_t NRF24L01_ReadRegisterMulti(uint8_t reg, uint8_t *data, uint8_t length) { - return NRF24L01_ReadMulti(R_REGISTER | (REGISTER_MASK & reg), data, length); + return rxSpiReadCommandMulti(R_REGISTER | (REGISTER_MASK & reg), NOP, data, length); } /* @@ -208,7 +96,7 @@ uint8_t NRF24L01_ReadRegisterMulti(uint8_t reg, uint8_t *data, uint8_t length) */ uint8_t NRF24L01_ReadPayload(uint8_t *data, uint8_t length) { - return NRF24L01_ReadMulti(R_RX_PAYLOAD, data, length); + return rxSpiReadCommandMulti(R_RX_PAYLOAD, NOP, data, length); } /* @@ -216,9 +104,7 @@ uint8_t NRF24L01_ReadPayload(uint8_t *data, uint8_t length) */ void NRF24L01_FlushTx() { - ENABLE_NRF24(); - nrf24TransferByte(FLUSH_TX); - DISABLE_NRF24(); + rxSpiWriteByte(FLUSH_TX); } /* @@ -226,12 +112,13 @@ void NRF24L01_FlushTx() */ void NRF24L01_FlushRx() { - ENABLE_NRF24(); - nrf24TransferByte(FLUSH_RX); - DISABLE_NRF24(); + rxSpiWriteByte(FLUSH_RX); } -#endif // UNIT_TEST +uint8_t NRF24L01_Activate(uint8_t code) +{ + return rxSpiWriteCommand(ACTIVATE, code); +} // standby configuration, used to simplify switching between RX, TX, and Standby modes static uint8_t standbyConfig; @@ -325,6 +212,8 @@ bool NRF24L01_ReadPayloadIfAvailable(uint8_t *data, uint8_t length) } #ifndef UNIT_TEST +#define DISABLE_RX() {IOHi(IOGetByTag(IO_TAG(RX_NSS_PIN)));} +#define ENABLE_RX() {IOLo(IOGetByTag(IO_TAG(RX_NSS_PIN)));} /* * Fast read of payload, for use in interrupt service routine */ @@ -334,21 +223,21 @@ bool NRF24L01_ReadPayloadIfAvailableFast(uint8_t *data, uint8_t length) // for 16 byte payload, that is 8*19 = 152 // at 50MHz clock rate that is approximately 3 microseconds bool ret = false; - ENABLE_NRF24(); - nrf24TransferByte(R_REGISTER | (REGISTER_MASK & NRF24L01_07_STATUS)); - const uint8_t status = nrf24TransferByte(NOP); + ENABLE_RX(); + rxSpiTransferByte(R_REGISTER | (REGISTER_MASK & NRF24L01_07_STATUS)); + const uint8_t status = rxSpiTransferByte(NOP); if ((status & BV(NRF24L01_07_STATUS_RX_DR)) == 0) { ret = true; // clear RX_DR flag - nrf24TransferByte(W_REGISTER | (REGISTER_MASK & NRF24L01_07_STATUS)); - nrf24TransferByte(BV(NRF24L01_07_STATUS_RX_DR)); - nrf24TransferByte(R_RX_PAYLOAD); + rxSpiTransferByte(W_REGISTER | (REGISTER_MASK & NRF24L01_07_STATUS)); + rxSpiTransferByte(BV(NRF24L01_07_STATUS_RX_DR)); + rxSpiTransferByte(R_RX_PAYLOAD); for (uint8_t i = 0; i < length; i++) { - data[i] = nrf24TransferByte(NOP); + data[i] = rxSpiTransferByte(NOP); } } - DISABLE_NRF24(); + DISABLE_RX(); return ret; } -#endif -#endif +#endif // UNIT_TEST +#endif // USE_RX_NRF24 diff --git a/src/main/drivers/rx_nrf24l01.h b/src/main/drivers/rx_nrf24l01.h index 585fda7dc4c..08d2c8afa74 100644 --- a/src/main/drivers/rx_nrf24l01.h +++ b/src/main/drivers/rx_nrf24l01.h @@ -23,6 +23,8 @@ #include #include +#include "rx_spi.h" + #define NRF24L01_MAX_PAYLOAD_SIZE 32 #define BV(x) (1<<(x)) // bit value @@ -173,12 +175,6 @@ enum { NRF24L01_PIPE5 = 5 }; -typedef enum { - NFR24L01_SOFTSPI, - NFR24L01_SPI, -} nfr24l01_spi_type_e; - -void NRF24L01_SpiInit(nfr24l01_spi_type_e spiType); void NRF24L01_Initialize(uint8_t baseConfig); uint8_t NRF24L01_WriteReg(uint8_t reg, uint8_t data); uint8_t NRF24L01_WriteRegisterMulti(uint8_t reg, const uint8_t *data, uint8_t length); @@ -188,12 +184,13 @@ uint8_t NRF24L01_ReadReg(uint8_t reg); uint8_t NRF24L01_ReadRegisterMulti(uint8_t reg, uint8_t *data, uint8_t length); uint8_t NRF24L01_ReadPayload(uint8_t *data, uint8_t length); -void NRF24L01_FlushTx(void); -void NRF24L01_FlushRx(void); - // Utility functions +void NRF24L01_FlushTx(void); +void NRF24L01_FlushRx(void); +uint8_t NRF24L01_Activate(uint8_t code); + void NRF24L01_SetupBasic(void); void NRF24L01_SetStandbyMode(void); void NRF24L01_SetRxMode(void); diff --git a/src/main/drivers/rx_spi.c b/src/main/drivers/rx_spi.c new file mode 100644 index 00000000000..e7e8f70d4fc --- /dev/null +++ b/src/main/drivers/rx_spi.c @@ -0,0 +1,166 @@ +/* + * 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 . + */ + +// This file is copied with modifications from project Deviation, +// see http://deviationtx.com + +#include +#include +#include + +#include + +#ifdef USE_RX_SPI + +#include "build/build_config.h" + +#include "system.h" +#include "gpio.h" +#include "io.h" +#include "io_impl.h" +#include "rcc.h" +#include "rx_spi.h" + +#include "bus_spi.h" +#include "bus_spi_soft.h" + +#define DISABLE_RX() {IOHi(IOGetByTag(IO_TAG(RX_NSS_PIN)));} +#define ENABLE_RX() {IOLo(IOGetByTag(IO_TAG(RX_NSS_PIN)));} +#ifdef RX_CE_PIN +#define RX_CE_HI() {IOHi(IOGetByTag(IO_TAG(RX_CE_PIN)));} +#define RX_CE_LO() {IOLo(IOGetByTag(IO_TAG(RX_CE_PIN)));} +#endif + +#ifdef USE_RX_SOFTSPI +static const softSPIDevice_t softSPIDevice = { + .sckTag = IO_TAG(RX_SCK_PIN), + .mosiTag = IO_TAG(RX_MOSI_PIN), + .misoTag = IO_TAG(RX_MISO_PIN), + // Note: Nordic Semiconductor uses 'CSN', STM uses 'NSS' + .nssTag = IO_TAG(RX_NSS_PIN), +}; +static bool useSoftSPI = false; +#endif // USE_RX_SOFTSPI + +void rxSpiDeviceInit(rx_spi_type_e spiType) +{ + static bool hardwareInitialised = false; + + if (hardwareInitialised) { + return; + } + +#ifdef USE_RX_SOFTSPI + if (spiType == RX_SPI_SOFTSPI) { + useSoftSPI = true; + softSpiInit(&softSPIDevice); + } + const SPIDevice rxSPIDevice = SOFT_SPIDEV_1; +#else + UNUSED(spiType); + const SPIDevice rxSPIDevice = spiDeviceByInstance(RX_SPI_INSTANCE); + IOInit(IOGetByTag(IO_TAG(RX_NSS_PIN)), OWNER_SPI, RESOURCE_SPI_CS, rxSPIDevice + 1); +#endif // USE_RX_SOFTSPI + +#if defined(STM32F10X) + RCC_AHBPeriphClockCmd(RX_NSS_GPIO_CLK_PERIPHERAL, ENABLE); + RCC_AHBPeriphClockCmd(RX_CE_GPIO_CLK_PERIPHERAL, ENABLE); +#endif + +#ifdef RX_CE_PIN + // CE as OUTPUT + IOInit(IOGetByTag(IO_TAG(RX_CE_PIN)), OWNER_RX_SPI, RESOURCE_RX_CE, rxSPIDevice + 1); +#if defined(STM32F10X) + IOConfigGPIO(IOGetByTag(IO_TAG(RX_CE_PIN)), SPI_IO_CS_CFG); +#elif defined(STM32F3) || defined(STM32F4) + IOConfigGPIOAF(IOGetByTag(IO_TAG(RX_CE_PIN)), SPI_IO_CS_CFG, 0); +#endif + RX_CE_LO(); +#endif // RX_CE_PIN + DISABLE_RX(); + +#ifdef RX_SPI_INSTANCE + spiSetDivisor(RX_SPI_INSTANCE, SPI_CLOCK_STANDARD); +#endif + hardwareInitialised = true; +} + +uint8_t rxSpiTransferByte(uint8_t data) +{ +#ifdef USE_RX_SOFTSPI + if (useSoftSPI) { + return softSpiTransferByte(&softSPIDevice, data); + } else +#endif + { +#ifdef RX_SPI_INSTANCE + return spiTransferByte(RX_SPI_INSTANCE, data); +#else + return 0; +#endif + } +} + +uint8_t rxSpiWriteByte(uint8_t data) +{ + ENABLE_RX(); + const uint8_t ret = rxSpiTransferByte(data); + DISABLE_RX(); + return ret; +} + +uint8_t rxSpiWriteCommand(uint8_t command, uint8_t data) +{ + ENABLE_RX(); + const uint8_t ret = rxSpiTransferByte(command); + rxSpiTransferByte(data); + DISABLE_RX(); + return ret; +} + +uint8_t rxSpiWriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length) +{ + ENABLE_RX(); + const uint8_t ret = rxSpiTransferByte(command); + for (uint8_t i = 0; i < length; i++) { + rxSpiTransferByte(data[i]); + } + DISABLE_RX(); + return ret; +} + +uint8_t rxSpiReadCommand(uint8_t command, uint8_t data) +{ + ENABLE_RX(); + rxSpiTransferByte(command); + const uint8_t ret = rxSpiTransferByte(data); + DISABLE_RX(); + return ret; +} + +uint8_t rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retData, uint8_t length) +{ + ENABLE_RX(); + const uint8_t ret = rxSpiTransferByte(command); + for (uint8_t i = 0; i < length; i++) { + retData[i] = rxSpiTransferByte(commandData); + } + DISABLE_RX(); + return ret; +} +#endif + diff --git a/src/main/drivers/rx_spi.h b/src/main/drivers/rx_spi.h new file mode 100644 index 00000000000..f083ca5058a --- /dev/null +++ b/src/main/drivers/rx_spi.h @@ -0,0 +1,35 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ +#pragma once + +#include + +typedef enum { + RX_SPI_SOFTSPI, + RX_SPI_HARDSPI, +} rx_spi_type_e; + +#define RX_SPI_MAX_PAYLOAD_SIZE 32 + +void rxSpiDeviceInit(rx_spi_type_e spiType); +uint8_t rxSpiTransferByte(uint8_t data); +uint8_t rxSpiWriteByte(uint8_t data); +uint8_t rxSpiWriteCommand(uint8_t command, uint8_t data); +uint8_t rxSpiWriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length); +uint8_t rxSpiReadCommand(uint8_t command, uint8_t commandData); +uint8_t rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retData, uint8_t length); + diff --git a/src/main/drivers/rx_xn297.c b/src/main/drivers/rx_xn297.c index 8a2b2bf1dcd..eba751cad59 100644 --- a/src/main/drivers/rx_xn297.c +++ b/src/main/drivers/rx_xn297.c @@ -21,7 +21,8 @@ #include #include -#include "drivers/rx_nrf24l01.h" +#include "rx_spi.h" +#include "rx_nrf24l01.h" #include "common/maths.h" diff --git a/src/main/drivers/serial.c b/src/main/drivers/serial.c index 8a17ce67933..24c80c3dfae 100644 --- a/src/main/drivers/serial.c +++ b/src/main/drivers/serial.c @@ -41,12 +41,12 @@ void serialWrite(serialPort_t *instance, uint8_t ch) } -void serialWriteBuf(serialPort_t *instance, uint8_t *data, int count) +void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) { if (instance->vTable->writeBuf) { instance->vTable->writeBuf(instance, data, count); } else { - for (uint8_t *p = data; count > 0; count--, p++) { + for (const uint8_t *p = data; count > 0; count--, p++) { while (!serialTxBytesFree(instance)) { }; @@ -56,12 +56,12 @@ void serialWriteBuf(serialPort_t *instance, uint8_t *data, int count) } } -uint32_t serialRxBytesWaiting(serialPort_t *instance) +uint32_t serialRxBytesWaiting(const serialPort_t *instance) { return instance->vTable->serialTotalRxWaiting(instance); } -uint8_t serialTxBytesFree(serialPort_t *instance) +uint8_t serialTxBytesFree(const serialPort_t *instance) { return instance->vTable->serialTotalTxFree(instance); } @@ -76,7 +76,7 @@ void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate) instance->vTable->serialSetBaudRate(instance, baudRate); } -bool isSerialTransmitBufferEmpty(serialPort_t *instance) +bool isSerialTransmitBufferEmpty(const serialPort_t *instance) { return instance->vTable->isSerialTransmitBufferEmpty(instance); } @@ -86,7 +86,7 @@ void serialSetMode(serialPort_t *instance, portMode_t mode) instance->vTable->setMode(instance, mode); } -void serialWriteBufShim(void *instance, uint8_t *data, int count) +void serialWriteBufShim(void *instance, const uint8_t *data, int count) { serialWriteBuf((serialPort_t *)instance, data, count); } diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index 1f75e931d3e..499ca9c7a4b 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -55,43 +55,42 @@ typedef struct serialPort_s { uint32_t txBufferHead; uint32_t txBufferTail; - // FIXME rename member to rxCallback - serialReceiveCallbackPtr callback; + serialReceiveCallbackPtr rxCallback; } serialPort_t; struct serialPortVTable { void (*serialWrite)(serialPort_t *instance, uint8_t ch); - uint32_t (*serialTotalRxWaiting)(serialPort_t *instance); - uint8_t (*serialTotalTxFree)(serialPort_t *instance); + uint32_t (*serialTotalRxWaiting)(const serialPort_t *instance); + uint8_t (*serialTotalTxFree)(const serialPort_t *instance); uint8_t (*serialRead)(serialPort_t *instance); // Specified baud rate may not be allowed by an implementation, use serialGetBaudRate to determine actual baud rate in use. void (*serialSetBaudRate)(serialPort_t *instance, uint32_t baudRate); - bool (*isSerialTransmitBufferEmpty)(serialPort_t *instance); + bool (*isSerialTransmitBufferEmpty)(const serialPort_t *instance); void (*setMode)(serialPort_t *instance, portMode_t mode); - void (*writeBuf)(serialPort_t *instance, void *data, int count); + void (*writeBuf)(serialPort_t *instance, const void *data, int count); // Optional functions used to buffer large writes. void (*beginWrite)(serialPort_t *instance); void (*endWrite)(serialPort_t *instance); }; void serialWrite(serialPort_t *instance, uint8_t ch); -uint32_t serialRxBytesWaiting(serialPort_t *instance); -uint8_t serialTxBytesFree(serialPort_t *instance); -void serialWriteBuf(serialPort_t *instance, uint8_t *data, int count); +uint32_t serialRxBytesWaiting(const serialPort_t *instance); +uint8_t serialTxBytesFree(const serialPort_t *instance); +void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count); uint8_t serialRead(serialPort_t *instance); void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate); void serialSetMode(serialPort_t *instance, portMode_t mode); -bool isSerialTransmitBufferEmpty(serialPort_t *instance); +bool isSerialTransmitBufferEmpty(const serialPort_t *instance); void serialPrint(serialPort_t *instance, const char *str); uint32_t serialGetBaudRate(serialPort_t *instance); // A shim that adapts the bufWriter API to the serialWriteBuf() API. -void serialWriteBufShim(void *instance, uint8_t *data, int count); +void serialWriteBufShim(void *instance, const uint8_t *data, int count); void serialBeginWrite(serialPort_t *instance); void serialEndWrite(serialPort_t *instance); diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index a5e41c8ec72..e06e9d60dbd 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -177,7 +177,7 @@ static void resetBuffers(softSerial_t *softSerial) softSerial->port.txBufferHead = 0; } -serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint32_t baud, portOptions_t options) +serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr rxCallback, uint32_t baud, portOptions_t options) { softSerial_t *softSerial = &(softSerialPorts[portIndex]); @@ -199,7 +199,7 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb softSerial->port.baudRate = baud; softSerial->port.mode = MODE_RXTX; softSerial->port.options = options; - softSerial->port.callback = callback; + softSerial->port.rxCallback = rxCallback; resetBuffers(softSerial); @@ -315,8 +315,8 @@ void extractAndStoreRxByte(softSerial_t *softSerial) uint8_t rxByte = (softSerial->internalRxBuffer >> 1) & 0xFF; - if (softSerial->port.callback) { - softSerial->port.callback(rxByte); + if (softSerial->port.rxCallback) { + softSerial->port.rxCallback(rxByte); } else { softSerial->port.rxBuffer[softSerial->port.rxBufferHead] = rxByte; softSerial->port.rxBufferHead = (softSerial->port.rxBufferHead + 1) % softSerial->port.rxBufferSize; @@ -401,7 +401,7 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) } } -uint32_t softSerialRxBytesWaiting(serialPort_t *instance) +uint32_t softSerialRxBytesWaiting(const serialPort_t *instance) { if ((instance->mode & MODE_RX) == 0) { return 0; @@ -412,7 +412,7 @@ uint32_t softSerialRxBytesWaiting(serialPort_t *instance) return (s->port.rxBufferHead - s->port.rxBufferTail) & (s->port.rxBufferSize - 1); } -uint8_t softSerialTxBytesFree(serialPort_t *instance) +uint8_t softSerialTxBytesFree(const serialPort_t *instance) { if ((instance->mode & MODE_TX) == 0) { return 0; @@ -455,7 +455,7 @@ void softSerialWriteByte(serialPort_t *s, uint8_t ch) void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate) { softSerial_t *softSerial = (softSerial_t *)s; - openSoftSerial(softSerial->softSerialPortIndex, s->callback, baudRate, softSerial->port.options); + openSoftSerial(softSerial->softSerialPortIndex, s->rxCallback, baudRate, softSerial->port.options); } void softSerialSetMode(serialPort_t *instance, portMode_t mode) @@ -463,7 +463,7 @@ void softSerialSetMode(serialPort_t *instance, portMode_t mode) instance->mode = mode; } -bool isSoftSerialTransmitBufferEmpty(serialPort_t *instance) +bool isSoftSerialTransmitBufferEmpty(const serialPort_t *instance) { return instance->txBufferHead == instance->txBufferTail; } diff --git a/src/main/drivers/serial_softserial.h b/src/main/drivers/serial_softserial.h index af80423b078..0d477cfaca7 100644 --- a/src/main/drivers/serial_softserial.h +++ b/src/main/drivers/serial_softserial.h @@ -24,13 +24,13 @@ typedef enum { SOFTSERIAL2 } softSerialPortIndex_e; -serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint32_t baud, portOptions_t options); +serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr rxCallback, uint32_t baud, portOptions_t options); // serialPort API void softSerialWriteByte(serialPort_t *instance, uint8_t ch); -uint32_t softSerialRxBytesWaiting(serialPort_t *instance); -uint8_t softSerialTxBytesFree(serialPort_t *instance); +uint32_t softSerialRxBytesWaiting(const serialPort_t *instance); +uint8_t softSerialTxBytesFree(const serialPort_t *instance); uint8_t softSerialReadByte(serialPort_t *instance); void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate); -bool isSoftSerialTransmitBufferEmpty(serialPort_t *s); +bool isSoftSerialTransmitBufferEmpty(const serialPort_t *s); diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index 68a43289e23..c756433e63f 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -93,7 +93,7 @@ static void uartReconfigure(uartPort_t *uartPort) USART_Cmd(uartPort->USARTx, ENABLE); } -serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options) +serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s = NULL; @@ -129,7 +129,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, s->port.rxBufferHead = s->port.rxBufferTail = 0; s->port.txBufferHead = s->port.txBufferTail = 0; // callback works for IRQ-based RX ONLY - s->port.callback = callback; + s->port.rxCallback = rxCallback; s->port.mode = mode; s->port.baudRate = baudRate; s->port.options = options; @@ -291,9 +291,9 @@ void uartStartTxDMA(uartPort_t *s) #endif } -uint32_t uartTotalRxBytesWaiting(serialPort_t *instance) +uint32_t uartTotalRxBytesWaiting(const serialPort_t *instance) { - uartPort_t *s = (uartPort_t*)instance; + const uartPort_t *s = (const uartPort_t*)instance; #ifdef STM32F4 if (s->rxDMAStream) { uint32_t rxDMAHead = s->rxDMAStream->NDTR; @@ -315,9 +315,9 @@ uint32_t uartTotalRxBytesWaiting(serialPort_t *instance) } } -uint8_t uartTotalTxBytesFree(serialPort_t *instance) +uint8_t uartTotalTxBytesFree(const serialPort_t *instance) { - uartPort_t *s = (uartPort_t*)instance; + const uartPort_t *s = (const uartPort_t*)instance; uint32_t bytesUsed; @@ -358,9 +358,9 @@ uint8_t uartTotalTxBytesFree(serialPort_t *instance) return (s->port.txBufferSize - 1) - bytesUsed; } -bool isUartTransmitBufferEmpty(serialPort_t *instance) +bool isUartTransmitBufferEmpty(const serialPort_t *instance) { - uartPort_t *s = (uartPort_t *)instance; + const uartPort_t *s = (const uartPort_t *)instance; #ifdef STM32F4 if (s->txDMAStream) #else diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index 33dde32167c..641c9ab3a8d 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -61,12 +61,12 @@ typedef struct { USART_TypeDef *USARTx; } uartPort_t; -serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options); +serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options); // serialPort API void uartWrite(serialPort_t *instance, uint8_t ch); -uint32_t uartTotalRxBytesWaiting(serialPort_t *instance); -uint8_t uartTotalTxBytesFree(serialPort_t *instance); +uint32_t uartTotalRxBytesWaiting(const serialPort_t *instance); +uint8_t uartTotalTxBytesFree(const serialPort_t *instance); uint8_t uartRead(serialPort_t *instance); void uartSetBaudRate(serialPort_t *s, uint32_t baudRate); -bool isUartTransmitBufferEmpty(serialPort_t *s); +bool isUartTransmitBufferEmpty(const serialPort_t *s); diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index 4774ab33e62..8b422df008c 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -55,8 +55,8 @@ void uartIrqCallback(uartPort_t *s) if (SR & USART_FLAG_RXNE && !s->rxDMAChannel) { // If we registered a callback, pass crap there - if (s->port.callback) { - s->port.callback(s->USARTx->DR); + if (s->port.rxCallback) { + s->port.rxCallback(s->USARTx->DR); } else { s->port.rxBuffer[s->port.rxBufferHead++] = s->USARTx->DR; if (s->port.rxBufferHead >= s->port.rxBufferSize) { diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 29bebfb954b..304c702d51c 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -369,8 +369,8 @@ void usartIrqHandler(uartPort_t *s) uint32_t ISR = s->USARTx->ISR; if (!s->rxDMAChannel && (ISR & USART_FLAG_RXNE)) { - if (s->port.callback) { - s->port.callback(s->USARTx->RDR); + if (s->port.rxCallback) { + s->port.rxCallback(s->USARTx->RDR); } else { s->port.rxBuffer[s->port.rxBufferHead++] = s->USARTx->RDR; if (s->port.rxBufferHead >= s->port.rxBufferSize) { diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index d6bd58024ec..eb20fc8ec85 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -237,8 +237,8 @@ static uartDevice_t* uartHardwareMap[] = { void uartIrqHandler(uartPort_t *s) { if (!s->rxDMAStream && (USART_GetITStatus(s->USARTx, USART_IT_RXNE) == SET)) { - if (s->port.callback) { - s->port.callback(s->USARTx->DR); + if (s->port.rxCallback) { + s->port.rxCallback(s->USARTx->DR); } else { s->port.rxBuffer[s->port.rxBufferHead] = s->USARTx->DR; s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize; diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 81d71a7ed37..79e6db8c789 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -59,13 +59,13 @@ static void usbVcpSetMode(serialPort_t *instance, portMode_t mode) // TODO implement } -static bool isUsbVcpTransmitBufferEmpty(serialPort_t *instance) +static bool isUsbVcpTransmitBufferEmpty(const serialPort_t *instance) { UNUSED(instance); return true; } -static uint32_t usbVcpAvailable(serialPort_t *instance) +static uint32_t usbVcpAvailable(const serialPort_t *instance) { UNUSED(instance); @@ -84,7 +84,7 @@ static uint8_t usbVcpRead(serialPort_t *instance) } } -static void usbVcpWriteBuf(serialPort_t *instance, void *data, int count) +static void usbVcpWriteBuf(serialPort_t *instance, const void *data, int count) { UNUSED(instance); @@ -93,7 +93,7 @@ static void usbVcpWriteBuf(serialPort_t *instance, void *data, int count) } uint32_t start = millis(); - uint8_t *p = data; + const uint8_t *p = data; uint32_t txed = 0; while (count > 0) { txed = CDC_Send_DATA(p, count); diff --git a/src/main/drivers/stack_check.c b/src/main/drivers/stack_check.c new file mode 100644 index 00000000000..242b6ffbb09 --- /dev/null +++ b/src/main/drivers/stack_check.c @@ -0,0 +1,79 @@ +/* + * stack_check.c + * + * Created on: 23 Aug 2016 + * Author: martinbudden + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef STACK_CHECK + +#include "build/debug.h" + +#define STACK_FILL_CHAR 0xa5 + +extern char _estack; // end of stack, declared in .LD file +extern char _Min_Stack_Size; // declared in .LD file +static uint32_t _Used_Stack_Size; + +/* + * The ARM processor uses a full descending stack. This means the stack pointer holds the address + * of the last stacked item in memory. When the processor pushes a new item onto the stack, + * it decrements the stack pointer and then writes the item to the new memory location. + * + * + * RAM layout is generally as below, although some targets vary + * + * F1 Boards + * RAM is origin 0x20000000 length 20K that is: + * 0x20000000 to 0x20005000 + * + * F3 Boards + * RAM is origin 0x20000000 length 40K that is: + * 0x20000000 to 0x2000a000 + * + * F4 Boards + * RAM is origin 0x20000000 length 128K that is: + * 0x20000000 to 0x20020000 + * + */ +void taskStackCheck(void) +{ + char * const stackHighMem = &_estack; + const uint32_t stackSize = (uint32_t)&_Min_Stack_Size; + char * const stackLowMem = stackHighMem - stackSize; + const char * const stackCurrent = (char *)&stackLowMem; + + char *p; + for (p = stackLowMem; p < stackCurrent; ++p) { + if (*p != STACK_FILL_CHAR) { + break; + } + } + + _Used_Stack_Size = (uint32_t)stackHighMem - (uint32_t)p; + +#ifdef DEBUG_STACK + debug[0] = (uint32_t)stackHighMem & 0xffff; + debug[1] = (uint32_t)stackLowMem & 0xffff; + debug[2] = (uint32_t)stackCurrent & 0xffff; + debug[3] = (uint32_t)p & 0xffff; +#endif +} + +uint32_t getTotalStackSize(void) +{ + return (uint32_t)&_Min_Stack_Size; +} + +uint32_t getUsedStackSize(void) +{ + return _Used_Stack_Size; +} + +#endif diff --git a/src/main/drivers/stack_check.h b/src/main/drivers/stack_check.h new file mode 100755 index 00000000000..7df3f6016ea --- /dev/null +++ b/src/main/drivers/stack_check.h @@ -0,0 +1,23 @@ +/* + * 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 + +#ifdef STACK_CHECK +uint32_t getTotalStackSize(void); +uint32_t getUsedStackSize(void); +#endif \ No newline at end of file diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index 962b6373499..180e4e7f348 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -24,6 +24,7 @@ #include "light_led.h" #include "sound_beeper.h" #include "nvic.h" +#include "build/atomic.h" #include "system.h" @@ -61,15 +62,57 @@ void cycleCounterInit(void) } // SysTick + +static volatile int sysTickPending = 0; + void SysTick_Handler(void) { - sysTickUptime++; + ATOMIC_BLOCK(NVIC_PRIO_MAX) { + sysTickUptime++; + sysTickPending = 0; + (void)(SysTick->CTRL); + } } // Return system uptime in microseconds (rollover in 70minutes) + +uint32_t microsISR(void) +{ + register uint32_t ms, pending, cycle_cnt; + + ATOMIC_BLOCK(NVIC_PRIO_MAX) { + cycle_cnt = SysTick->VAL; + + if (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) { + // Update pending. + // Record it for multiple calls within the same rollover period + // (Will be cleared when serviced). + // Note that multiple rollovers are not considered. + + sysTickPending = 1; + + // Read VAL again to ensure the value is read after the rollover. + + cycle_cnt = SysTick->VAL; + } + + ms = sysTickUptime; + pending = sysTickPending; + } + + return ((ms + pending) * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks; +} + uint32_t micros(void) { register uint32_t ms, cycle_cnt; + + // Call microsISR() in interrupt and elevated (non-zero) BASEPRI context + + if ((SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) || (__get_BASEPRI())) { + return microsISR(); + } + do { ms = sysTickUptime; cycle_cnt = SysTick->VAL; diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 105a584c446..b2e4e45a836 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -22,6 +22,7 @@ void delayMicroseconds(uint32_t us); void delay(uint32_t ms); uint32_t micros(void); +uint32_t microsISR(void); uint32_t millis(void); typedef enum { diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index 3e33c545b80..bd81852fe06 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -28,7 +28,7 @@ #define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000) // from system_stm32f10x.c -void SetSysClock(bool overclock); +void SetSysClock(void); void systemReset(void) { @@ -70,7 +70,7 @@ void systemInit(void) { checkForBootLoaderRequest(); - SetSysClock(false); + SetSysClock(); #ifdef CC3D /* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */ diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c new file mode 100755 index 00000000000..f4436e97ec2 --- /dev/null +++ b/src/main/fc/fc_tasks.c @@ -0,0 +1,388 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "common/axis.h" +#include "common/color.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/io_pca9685.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" + +#include "fc/fc_tasks.h" +#include "fc/mw.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "io/beeper.h" +#include "io/display.h" +#include "io/gimbal.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/motors.h" +#include "io/servos.h" +#include "io/pwmdriver_i2c.h" +#include "io/serial.h" +#include "io/serial_cli.h" + +#include "msp/msp_serial.h" + +#include "rx/rx.h" + +#include "scheduler/scheduler.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" +#include "sensors/boardalignment.h" +#include "sensors/compass.h" +#include "sensors/gyro.h" +#include "sensors/rangefinder.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/servos.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/hil.h" +#include "flight/failsafe.h" +#include "flight/navigation_rewrite.h" + +#include "config/config.h" +#include "config/feature.h" +#include "config/config_profile.h" +#include "config/config_master.h" + +extern uint32_t currentTime; + +/* VBAT monitoring interval (in microseconds) - 1s*/ +#define VBATINTERVAL (6 * 3500) +/* IBat monitoring interval (in microseconds) - 6 default looptimes */ +#define IBATINTERVAL (6 * 3500) + +cfTask_t cfTasks[TASK_COUNT] = { + [TASK_SYSTEM] = { + .taskName = "SYSTEM", + .taskFunc = taskSystem, + .desiredPeriod = 1000000 / 10, // run every 100 ms + .staticPriority = TASK_PRIORITY_HIGH, + }, + + [TASK_GYROPID] = { + .taskName = "GYRO/PID", + .taskFunc = taskMainPidLoopChecker, + .desiredPeriod = 1000, + .staticPriority = TASK_PRIORITY_REALTIME, + }, + + [TASK_SERIAL] = { + .taskName = "SERIAL", + .taskFunc = taskHandleSerial, + .desiredPeriod = 1000000 / 100, // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud + .staticPriority = TASK_PRIORITY_LOW, + }, + + [TASK_BEEPER] = { + .taskName = "BEEPER", + .taskFunc = taskUpdateBeeper, + .desiredPeriod = 1000000 / 100, // 100 Hz + .staticPriority = TASK_PRIORITY_MEDIUM, + }, + + [TASK_BATTERY] = { + .taskName = "BATTERY", + .taskFunc = taskUpdateBattery, + .desiredPeriod = 1000000 / 50, // 50 Hz + .staticPriority = TASK_PRIORITY_MEDIUM, + }, + + [TASK_RX] = { + .taskName = "RX", + .checkFunc = taskUpdateRxCheck, + .taskFunc = taskUpdateRxMain, + .desiredPeriod = 1000000 / 50, // If event-based scheduling doesn't work, fallback to periodic scheduling + .staticPriority = TASK_PRIORITY_HIGH, + }, + +#ifdef GPS + [TASK_GPS] = { + .taskName = "GPS", + .taskFunc = taskProcessGPS, + .desiredPeriod = 1000000 / 25, // GPS usually don't go raster than 10Hz + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + +#ifdef MAG + [TASK_COMPASS] = { + .taskName = "COMPASS", + .taskFunc = taskUpdateCompass, + .desiredPeriod = 1000000 / 10, // Compass is updated at 10 Hz + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + +#ifdef BARO + [TASK_BARO] = { + .taskName = "BARO", + .taskFunc = taskUpdateBaro, + .desiredPeriod = 1000000 / 20, + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + +#ifdef SONAR + [TASK_SONAR] = { + .taskName = "SONAR", + .taskFunc = taskUpdateSonar, + .desiredPeriod = 70000, // every 70 ms, approximately 14 Hz + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + +#ifdef DISPLAY + [TASK_DISPLAY] = { + .taskName = "DISPLAY", + .taskFunc = taskUpdateDisplay, + .desiredPeriod = 1000000 / 10, + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef TELEMETRY + [TASK_TELEMETRY] = { + .taskName = "TELEMETRY", + .taskFunc = taskTelemetry, + .desiredPeriod = 1000000 / 250, // 250 Hz + .staticPriority = TASK_PRIORITY_IDLE, + }, +#endif + +#ifdef LED_STRIP + [TASK_LEDSTRIP] = { + .taskName = "LEDSTRIP", + .taskFunc = taskLedStrip, + .desiredPeriod = 1000000 / 100, // 100 Hz + .staticPriority = TASK_PRIORITY_IDLE, + }, +#endif + +#ifdef USE_PMW_SERVO_DRIVER + [TASK_PWMDRIVER] = { + .taskName = "PWMDRIVER", + .taskFunc = taskSyncPwmDriver, + .desiredPeriod = 1000000 / 200, // 200 Hz + .staticPriority = TASK_PRIORITY_HIGH, + }, +#endif + +#ifdef STACK_CHECK + [TASK_STACK_CHECK] = { + .taskName = "STACKCHECK", + .taskFunc = taskStackCheck, + .desiredPeriod = 1000000 / 10, // 10 Hz + .staticPriority = TASK_PRIORITY_IDLE, + }, +#endif +}; + +void taskHandleSerial(void) +{ +#ifdef USE_CLI + // in cli mode, all serial stuff goes to here. enter cli mode by sending # + if (cliMode) { + cliProcess(); + return; + } +#endif + mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA); +} + +void taskUpdateBeeper(void) +{ + beeperUpdate(); //call periodic beeper handler +} + +void taskUpdateBattery(void) +{ + static uint32_t vbatLastServiced = 0; + static uint32_t ibatLastServiced = 0; + + if (feature(FEATURE_VBAT)) { + if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) { + uint32_t vbatTimeDelta = currentTime - vbatLastServiced; + vbatLastServiced = currentTime; + updateBattery(vbatTimeDelta); + } + } + + if (feature(FEATURE_CURRENT_METER)) { + int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced); + + if (ibatTimeSinceLastServiced >= IBATINTERVAL) { + ibatLastServiced = currentTime; + updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); + } + } +} + +#ifdef GPS +void taskProcessGPS(void) +{ + // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck + // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will + // change this based on available hardware + if (feature(FEATURE_GPS)) { + gpsThread(); + } + + if (sensors(SENSOR_GPS)) { + updateGpsIndicator(currentTime); + } +} +#endif + +#ifdef MAG +void taskUpdateCompass(void) +{ + if (sensors(SENSOR_MAG)) { + updateCompass(&masterConfig.magZero); + } +} +#endif + +#ifdef BARO +void taskUpdateBaro(void) +{ + if (sensors(SENSOR_BARO)) { + const uint32_t newDeadline = baroUpdate(); + if (newDeadline != 0) { + rescheduleTask(TASK_SELF, newDeadline); + } + } + + //updatePositionEstimator_BaroTopic(currentTime); +} +#endif + +#ifdef SONAR +void taskUpdateSonar(void) +{ + if (sensors(SENSOR_SONAR)) { + rangefinderUpdate(); + } + + //updatePositionEstimator_SonarTopic(currentTime); +} +#endif + +#ifdef DISPLAY +void taskUpdateDisplay(void) +{ + if (feature(FEATURE_DISPLAY)) { + updateDisplay(); + } +} +#endif + +#ifdef TELEMETRY +void taskTelemetry(void) +{ + telemetryCheckState(); + + if (!cliMode && feature(FEATURE_TELEMETRY)) { + telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); + } +} +#endif + +#ifdef LED_STRIP +void taskLedStrip(void) +{ + if (feature(FEATURE_LED_STRIP)) { + updateLedStrip(); + } +} +#endif + +#ifdef USE_PMW_SERVO_DRIVER +void taskSyncPwmDriver(void) { + + if (feature(FEATURE_PWM_SERVO_DRIVER)) { + pwmDriverSync(); + } +} +#endif + +void fcTasksInit(void) +{ + schedulerInit(); + + rescheduleTask(TASK_GYROPID, gyro.targetLooptime); + setTaskEnabled(TASK_GYROPID, true); + + setTaskEnabled(TASK_SERIAL, true); +#ifdef BEEPER + setTaskEnabled(TASK_BEEPER, true); +#endif + setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); + setTaskEnabled(TASK_RX, true); +#ifdef GPS + setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); +#endif +#ifdef MAG + setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); +#if (defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE)) && defined(USE_MAG_AK8963) + // fixme temporary solution for AK6983 via slave I2C on MPU9250 + rescheduleTask(TASK_COMPASS, 1000000 / 40); +#endif +#endif +#ifdef BARO + setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); +#endif +#ifdef SONAR + setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); +#endif +#ifdef DISPLAY + setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); +#endif +#ifdef TELEMETRY + setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); +#endif +#ifdef LED_STRIP + setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); +#endif +#ifdef STACK_CHECK + setTaskEnabled(TASK_STACK_CHECK, true); +#endif + +#ifdef USE_PMW_SERVO_DRIVER + setTaskEnabled(TASK_PWMDRIVER, feature(FEATURE_PWM_SERVO_DRIVER)); +#endif +} diff --git a/src/main/scheduler/scheduler_tasks.h b/src/main/fc/fc_tasks.h similarity index 90% rename from src/main/scheduler/scheduler_tasks.h rename to src/main/fc/fc_tasks.h index 995e2b30371..ebb2a3027de 100644 --- a/src/main/scheduler/scheduler_tasks.h +++ b/src/main/fc/fc_tasks.h @@ -33,4 +33,9 @@ void taskUpdateDisplay(void); void taskTelemetry(void); void taskLedStrip(void); void taskSystem(void); +#ifdef USE_PMW_SERVO_DRIVER +void taskSyncPwmDriver(void); +#endif +void taskStackCheck(void); +void fcTasksInit(void); diff --git a/src/main/fc/msp_fc.c b/src/main/fc/msp_fc.c new file mode 100644 index 00000000000..ce0bbc00ff1 --- /dev/null +++ b/src/main/fc/msp_fc.c @@ -0,0 +1,1608 @@ +/* + * 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 "platform.h" + +#include "blackbox/blackbox.h" + +#include "build/build_config.h" +#include "build/debug.h" +#include "build/version.h" + +#include "common/axis.h" +#include "common/color.h" +#include "common/maths.h" +#include "common/streambuf.h" + +#include "drivers/system.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/pwm_mapping.h" + +#include "drivers/serial.h" +#include "drivers/bus_i2c.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/sdcard.h" + +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "io/motors.h" +#include "io/servos.h" + +#include "io/gps.h" +#include "io/gimbal.h" +#include "io/serial.h" +#include "io/ledstrip.h" +#include "io/flashfs.h" +#include "io/asyncfatfs/asyncfatfs.h" +#include "io/serial_4way.h" + +#include "msp/msp_protocol.h" +#include "msp/msp.h" + +#include "rx/rx.h" +#include "rx/msp.h" + +#include "scheduler/scheduler.h" + +#include "sensors/boardalignment.h" +#include "sensors/sensors.h" +#include "sensors/battery.h" +#include "sensors/rangefinder.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/compass.h" +#include "sensors/gyro.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/servos.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/hil.h" +#include "flight/failsafe.h" +#include "flight/navigation_rewrite.h" + +#include "config/config.h" +#include "config/config_eeprom.h" +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +#ifdef NAZE +#include "hardware_revision.h" +#endif + +extern uint16_t cycleTime; // FIXME dependency on mw.c +extern uint16_t rssi; // FIXME dependency on mw.c + +static const char * const flightControllerIdentifier = INAV_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. +static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; + +typedef struct box_e { + const uint8_t boxId; // see boxId_e + const char *boxName; // GUI-readable box name + const uint8_t permanentId; // +} box_t; + +// FIXME remove ;'s +static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { + { BOXARM, "ARM;", 0 }, + { BOXANGLE, "ANGLE;", 1 }, + { BOXHORIZON, "HORIZON;", 2 }, + { BOXNAVALTHOLD, "NAV ALTHOLD;", 3 }, // old BARO + { BOXMAG, "MAG;", 5 }, + { BOXHEADFREE, "HEADFREE;", 6 }, + { BOXHEADADJ, "HEADADJ;", 7 }, + { BOXCAMSTAB, "CAMSTAB;", 8 }, + { BOXNAVRTH, "NAV RTH;", 10 }, // old GPS HOME + { BOXNAVPOSHOLD, "NAV POSHOLD;", 11 }, // old GPS HOLD + { BOXPASSTHRU, "PASSTHRU;", 12 }, + { BOXBEEPERON, "BEEPER;", 13 }, + { BOXLEDLOW, "LEDLOW;", 15 }, + { BOXLLIGHTS, "LLIGHTS;", 16 }, + { BOXOSD, "OSD SW;", 19 }, + { BOXTELEMETRY, "TELEMETRY;", 20 }, + //{ BOXGTUNE, "GTUNE;", 21 }, + { BOXBLACKBOX, "BLACKBOX;", 26 }, + { BOXFAILSAFE, "FAILSAFE;", 27 }, + { BOXNAVWP, "NAV WP;", 28 }, + { BOXAIRMODE, "AIR MODE;", 29 }, + { BOXHOMERESET, "HOME RESET;", 30 }, + { BOXGCSNAV, "GCS NAV;", 31 }, + { BOXHEADINGLOCK, "HEADING LOCK;", 32 }, + { BOXSURFACE, "SURFACE;", 33 }, + { BOXFLAPERON, "FLAPERON;", 34 }, + { BOXTURNASSIST, "TURN ASSIST;", 35 }, + { CHECKBOX_ITEM_COUNT, NULL, 0xFF } +}; + +// this is calculated at startup based on enabled features. +static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT]; +// this is the number of filled indexes in above array +static uint8_t activeBoxIdCount = 0; +// from mixer.c +extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; + +static const char pidnames[] = + "ROLL;" + "PITCH;" + "YAW;" + "ALT;" + "Pos;" + "PosR;" + "NavR;" + "LEVEL;" + "MAG;" + "VEL;"; + +typedef enum { + MSP_SDCARD_STATE_NOT_PRESENT = 0, + MSP_SDCARD_STATE_FATAL = 1, + MSP_SDCARD_STATE_CARD_INIT = 2, + MSP_SDCARD_STATE_FS_INIT = 3, + MSP_SDCARD_STATE_READY = 4, +} mspSDCardState_e; + +typedef enum { + MSP_SDCARD_FLAG_SUPPORTTED = 1, +} mspSDCardFlags_e; + +typedef enum { + MSP_FLASHFS_BIT_READY = 1, + MSP_FLASHFS_BIT_SUPPORTED = 2, +} mspFlashfsFlags_e; + +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE +void msp4WayIfFn(serialPort_t *serialPort) +{ + // rem: App: Wait at least appx. 500 ms for BLHeli to jump into + // bootloader mode before try to connect any ESC + // Start to activate here + esc4wayProcess(serialPort); + // former used MSP uart is still active + // proceed as usual with MSP commands +} +#endif + +static void mspRebootFn(serialPort_t *serialPort) +{ + UNUSED(serialPort); + + stopMotors(); + stopPwmAllMotors(); + + // extra delay before reboot to give ESCs chance to reset + delay(1000); + systemReset(); + + // control should never return here. + while(1) ; +} + +static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId) +{ + uint8_t boxIndex; + const box_t *candidate; + for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) { + candidate = &boxes[boxIndex]; + if (candidate->boxId == activeBoxId) { + return candidate; + } + } + return NULL; +} + +static const box_t *findBoxByPermenantId(uint8_t permenantId) +{ + uint8_t boxIndex; + const box_t *candidate; + for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) { + candidate = &boxes[boxIndex]; + if (candidate->permanentId == permenantId) { + return candidate; + } + } + return NULL; +} + +static void serializeBoxNamesReply(sbuf_t *dst) +{ + // in first run of the loop, we grab total size of junk to be sent + // then come back and actually send it + for (int i = 0; i < activeBoxIdCount; i++) { + const int activeBoxId = activeBoxIds[i]; + const box_t *box = findBoxByActiveBoxId(activeBoxId); + if (box) { + const int len = strlen(box->boxName); + for (int j = 0; j < len; j++) { + sbufWriteU8(dst, box->boxName[j]); + } + } + } +} + +static void initActiveBoxIds(void) +{ + // calculate used boxes based on features and fill availableBoxes[] array + memset(activeBoxIds, 0xFF, sizeof(activeBoxIds)); + + activeBoxIdCount = 0; + activeBoxIds[activeBoxIdCount++] = BOXARM; + + if (sensors(SENSOR_ACC)) { + activeBoxIds[activeBoxIdCount++] = BOXANGLE; + activeBoxIds[activeBoxIdCount++] = BOXHORIZON; + activeBoxIds[activeBoxIdCount++] = BOXTURNASSIST; + } + + activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; + activeBoxIds[activeBoxIdCount++] = BOXHEADINGLOCK; + + if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { + activeBoxIds[activeBoxIdCount++] = BOXMAG; + activeBoxIds[activeBoxIdCount++] = BOXHEADFREE; + activeBoxIds[activeBoxIdCount++] = BOXHEADADJ; + } + + if (feature(FEATURE_SERVO_TILT)) + activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; + + bool isFixedWing = masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE; + +#ifdef GPS + if (sensors(SENSOR_BARO) || (isFixedWing && feature(FEATURE_GPS))) { + activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD; + activeBoxIds[activeBoxIdCount++] = BOXSURFACE; + } + if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (isFixedWing && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) { + activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; + activeBoxIds[activeBoxIdCount++] = BOXNAVRTH; + activeBoxIds[activeBoxIdCount++] = BOXNAVWP; + activeBoxIds[activeBoxIdCount++] = BOXHOMERESET; + activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; + } +#endif + + if (isFixedWing) { + activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU; + } + + /* + * FLAPERON mode active only in case of airplane and custom airplane. Activating on + * flying wing can cause bad thing + */ + if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) { + activeBoxIds[activeBoxIdCount++] = BOXFLAPERON; + } + + activeBoxIds[activeBoxIdCount++] = BOXBEEPERON; + +#ifdef LED_STRIP + if (feature(FEATURE_LED_STRIP)) { + activeBoxIds[activeBoxIdCount++] = BOXLEDLOW; + } +#endif + + activeBoxIds[activeBoxIdCount++] = BOXOSD; + +#ifdef TELEMETRY + if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch) + activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY; +#endif + +#ifdef BLACKBOX + if (feature(FEATURE_BLACKBOX)){ + activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX; + } +#endif + + if (feature(FEATURE_FAILSAFE)){ + activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE; + } +} + +#define IS_ENABLED(mask) (mask == 0 ? 0 : 1) + +static uint32_t packFlightModeFlags(void) +{ + // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES + // Requires new Multiwii protocol version to fix + // It would be preferable to setting the enabled bits based on BOXINDEX. + const uint32_t tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE | + IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON | + IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG | + IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB | + IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | + IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | + IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | + IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)) << BOXNAVALTHOLD | + IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)) << BOXNAVPOSHOLD | + IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)) << BOXNAVRTH | + IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)) << BOXNAVWP | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)) << BOXGCSNAV | + IS_ENABLED(FLIGHT_MODE(HEADING_LOCK)) << BOXHEADINGLOCK | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSURFACE)) << BOXSURFACE | + IS_ENABLED(FLIGHT_MODE(FLAPERON)) << BOXFLAPERON | + IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)) << BOXTURNASSIST | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)) << BOXHOMERESET; + + uint32_t ret = 0; + for (uint32_t i = 0; i < activeBoxIdCount; i++) { + const int flag = (tmp & (1 << activeBoxIds[i])); + if (flag) { + ret |= 1 << i; + } + } + return ret; +} + +static void serializeSDCardSummaryReply(sbuf_t *dst) +{ +#ifdef USE_SDCARD + uint8_t flags = MSP_SDCARD_FLAG_SUPPORTTED; + uint8_t state; + + sbufWriteU8(dst, flags); + + // Merge the card and filesystem states together + if (!sdcard_isInserted()) { + state = MSP_SDCARD_STATE_NOT_PRESENT; + } else if (!sdcard_isFunctional()) { + state = MSP_SDCARD_STATE_FATAL; + } else { + switch (afatfs_getFilesystemState()) { + case AFATFS_FILESYSTEM_STATE_READY: + state = MSP_SDCARD_STATE_READY; + break; + case AFATFS_FILESYSTEM_STATE_INITIALIZATION: + if (sdcard_isInitialized()) { + state = MSP_SDCARD_STATE_FS_INIT; + } else { + state = MSP_SDCARD_STATE_CARD_INIT; + } + break; + case AFATFS_FILESYSTEM_STATE_FATAL: + case AFATFS_FILESYSTEM_STATE_UNKNOWN: + default: + state = MSP_SDCARD_STATE_FATAL; + break; + } + } + + sbufWriteU8(dst, state); + sbufWriteU8(dst, afatfs_getLastError()); + // Write free space and total space in kilobytes + sbufWriteU32(dst, afatfs_getContiguousFreeSpace() / 1024); + sbufWriteU32(dst, sdcard_getMetadata()->numBlocks / 2); // Block size is half a kilobyte +#else + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU32(dst, 0); + sbufWriteU32(dst, 0); +#endif +} + +static void serializeDataflashSummaryReply(sbuf_t *dst) +{ +#ifdef USE_FLASHFS + const flashGeometry_t *geometry = flashfsGetGeometry(); + sbufWriteU8(dst, flashfsIsReady() ? 1 : 0); + sbufWriteU32(dst, geometry->sectors); + sbufWriteU32(dst, geometry->totalSize); + sbufWriteU32(dst, flashfsGetOffset()); // Effectively the current number of bytes stored on the volume +#else + sbufWriteU8(dst, 0); + sbufWriteU32(dst, 0); + sbufWriteU32(dst, 0); + sbufWriteU32(dst, 0); +#endif +} + +#ifdef USE_FLASHFS +static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, uint8_t size) +{ + uint8_t buffer[128]; + int bytesRead; + + if (size > sizeof(buffer)) { + size = sizeof(buffer); + } + + sbufWriteU32(dst, address); + + // bytesRead will be lower than that requested if we reach end of volume + bytesRead = flashfsReadAbs(address, buffer, size); + + for (int i = 0; i < bytesRead; i++) { + sbufWriteU8(dst, buffer[i]); + } +} +#endif + +/* + * Returns true if the command was processd, false otherwise. + * May set mspPostProcessFunc to a function to be called once the command has been processed + */ +static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn) +{ +#if !defined(NAV) && !defined(USE_FLASHFS) + UNUSED(src); +#endif + uint32_t i; + +#ifdef NAV + int8_t msp_wp_no; + navWaypoint_t msp_wp; +#endif + + switch (cmdMSP) { + case MSP_API_VERSION: + sbufWriteU8(dst, MSP_PROTOCOL_VERSION); + + sbufWriteU8(dst, API_VERSION_MAJOR); + sbufWriteU8(dst, API_VERSION_MINOR); + break; + + case MSP_FC_VARIANT: + for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) { + sbufWriteU8(dst, flightControllerIdentifier[i]); + } + break; + + case MSP_FC_VERSION: + sbufWriteU8(dst, FC_VERSION_MAJOR); + sbufWriteU8(dst, FC_VERSION_MINOR); + sbufWriteU8(dst, FC_VERSION_PATCH_LEVEL); + break; + + case MSP_BOARD_INFO: + for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) { + sbufWriteU8(dst, boardIdentifier[i]); + } +#ifdef NAZE + sbufWriteU16(dst, hardwareRevision); +#else + sbufWriteU16(dst, 0); // No other build targets currently have hardware revision detection. +#endif + break; + + case MSP_BUILD_INFO: + for (i = 0; i < BUILD_DATE_LENGTH; i++) { + sbufWriteU8(dst, buildDate[i]); + } + for (i = 0; i < BUILD_TIME_LENGTH; i++) { + sbufWriteU8(dst, buildTime[i]); + } + + for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) { + sbufWriteU8(dst, shortGitRevision[i]); + } + break; + + // DEPRECATED - Use MSP_API_VERSION + case MSP_IDENT: + sbufWriteU8(dst, MW_VERSION); + sbufWriteU8(dst, masterConfig.mixerMode); + sbufWriteU8(dst, MSP_PROTOCOL_VERSION); + sbufWriteU32(dst, CAP_PLATFORM_32BIT | CAP_DYNBALANCE | CAP_FLAPS | CAP_NAVCAP | CAP_EXTAUX); // "capability" + break; + +#ifdef HIL + case MSP_HIL_STATE: + sbufWriteU16(dst, hilToSIM.pidCommand[ROLL]); + sbufWriteU16(dst, hilToSIM.pidCommand[PITCH]); + sbufWriteU16(dst, hilToSIM.pidCommand[YAW]); + sbufWriteU16(dst, hilToSIM.pidCommand[THROTTLE]); + break; +#endif + + case MSP_STATUS_EX: + sbufWriteU16(dst, cycleTime); +#ifdef USE_I2C + sbufWriteU16(dst, i2cGetErrorCounter()); +#else + sbufWriteU16(dst, 0); +#endif + sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); + sbufWriteU32(dst, packFlightModeFlags()); + sbufWriteU8(dst, masterConfig.current_profile_index); + sbufWriteU16(dst, averageSystemLoadPercent); + break; + + case MSP_STATUS: + sbufWriteU16(dst, cycleTime); +#ifdef USE_I2C + sbufWriteU16(dst, i2cGetErrorCounter()); +#else + sbufWriteU16(dst, 0); +#endif + sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); + sbufWriteU32(dst, packFlightModeFlags()); + sbufWriteU8(dst, masterConfig.current_profile_index); + break; + + case MSP_RAW_IMU: + { + // Hack scale due to choice of units for sensor data in multiwii + const uint8_t scale = (acc.acc_1G > 1024) ? 8 : 1; + for (i = 0; i < 3; i++) + sbufWriteU16(dst, accADC[i] / scale); + for (i = 0; i < 3; i++) + sbufWriteU16(dst, gyroADC[i]); + for (i = 0; i < 3; i++) + sbufWriteU16(dst, magADC[i]); + } + break; + + +#ifdef USE_SERVOS + case MSP_SERVO: + sbufWriteData(dst, &servo, MAX_SUPPORTED_SERVOS * 2); + break; + case MSP_SERVO_CONFIGURATIONS: + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + sbufWriteU16(dst, currentProfile->servoConf[i].min); + sbufWriteU16(dst, currentProfile->servoConf[i].max); + sbufWriteU16(dst, currentProfile->servoConf[i].middle); + sbufWriteU8(dst, currentProfile->servoConf[i].rate); + sbufWriteU8(dst, currentProfile->servoConf[i].angleAtMin); + sbufWriteU8(dst, currentProfile->servoConf[i].angleAtMax); + sbufWriteU8(dst, currentProfile->servoConf[i].forwardFromChannel); + sbufWriteU32(dst, currentProfile->servoConf[i].reversedSources); + } + break; + case MSP_SERVO_MIX_RULES: + for (i = 0; i < MAX_SERVO_RULES; i++) { + sbufWriteU8(dst, masterConfig.customServoMixer[i].targetChannel); + sbufWriteU8(dst, masterConfig.customServoMixer[i].inputSource); + sbufWriteU8(dst, masterConfig.customServoMixer[i].rate); + sbufWriteU8(dst, masterConfig.customServoMixer[i].speed); + sbufWriteU8(dst, masterConfig.customServoMixer[i].min); + sbufWriteU8(dst, masterConfig.customServoMixer[i].max); + sbufWriteU8(dst, 0); + } + break; +#endif + + case MSP_MOTOR: + for (unsigned i = 0; i < 8; i++) { + sbufWriteU16(dst, i < MAX_SUPPORTED_MOTORS ? motor[i] : 0); + } + break; + + case MSP_RC: + for (i = 0; i < rxRuntimeConfig.channelCount; i++) + sbufWriteU16(dst, rcData[i]); + break; + + case MSP_ATTITUDE: + sbufWriteU16(dst, attitude.values.roll); + sbufWriteU16(dst, attitude.values.pitch); + sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw)); + break; + + case MSP_ALTITUDE: +#if defined(NAV) + sbufWriteU32(dst, (uint32_t)lrintf(getEstimatedActualPosition(Z))); + sbufWriteU16(dst, (uint32_t)lrintf(getEstimatedActualVelocity(Z))); +#else + sbufWriteU32(dst, 0); + sbufWriteU16(dst, 0); +#endif + break; + + case MSP_SONAR_ALTITUDE: +#if defined(SONAR) + sbufWriteU32(dst, rangefinderGetLatestAltitude()); +#else + sbufWriteU32(dst, 0); +#endif + break; + + case MSP_ANALOG: + sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255)); + sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery + sbufWriteU16(dst, rssi); + if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) { + sbufWriteU16(dst, (uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero + } else + sbufWriteU16(dst, (int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A + break; + + case MSP_ARMING_CONFIG: + sbufWriteU8(dst, masterConfig.auto_disarm_delay); + sbufWriteU8(dst, masterConfig.disarm_kill_switch); + break; + + case MSP_LOOP_TIME: + sbufWriteU16(dst, masterConfig.looptime); + break; + + case MSP_RC_TUNING: + sbufWriteU8(dst, 100); //rcRate8 kept for compatibity reasons, this setting is no longer used + sbufWriteU8(dst, currentControlRateProfile->rcExpo8); + for (i = 0 ; i < 3; i++) { + sbufWriteU8(dst, currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t + } + sbufWriteU8(dst, currentControlRateProfile->dynThrPID); + sbufWriteU8(dst, currentControlRateProfile->thrMid8); + sbufWriteU8(dst, currentControlRateProfile->thrExpo8); + sbufWriteU16(dst, currentControlRateProfile->tpa_breakpoint); + sbufWriteU8(dst, currentControlRateProfile->rcYawExpo8); + break; + + case MSP_PID: + for (i = 0; i < PID_ITEM_COUNT; i++) { + sbufWriteU8(dst, currentProfile->pidProfile.P8[i]); + sbufWriteU8(dst, currentProfile->pidProfile.I8[i]); + sbufWriteU8(dst, currentProfile->pidProfile.D8[i]); + } + break; + + case MSP_PIDNAMES: + for (const char *c = pidnames; *c; c++) { + sbufWriteU8(dst, *c); + } + break; + + case MSP_PID_CONTROLLER: + sbufWriteU8(dst, 2); // FIXME: Report as LuxFloat + break; + + case MSP_MODE_RANGES: + for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + modeActivationCondition_t *mac = ¤tProfile->modeActivationConditions[i]; + const box_t *box = findBoxByActiveBoxId(mac->modeId); + sbufWriteU8(dst, box ? box->permanentId : 0); + sbufWriteU8(dst, mac->auxChannelIndex); + sbufWriteU8(dst, mac->range.startStep); + sbufWriteU8(dst, mac->range.endStep); + } + break; + + case MSP_ADJUSTMENT_RANGES: + for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { + adjustmentRange_t *adjRange = ¤tProfile->adjustmentRanges[i]; + sbufWriteU8(dst, adjRange->adjustmentIndex); + sbufWriteU8(dst, adjRange->auxChannelIndex); + sbufWriteU8(dst, adjRange->range.startStep); + sbufWriteU8(dst, adjRange->range.endStep); + sbufWriteU8(dst, adjRange->adjustmentFunction); + sbufWriteU8(dst, adjRange->auxSwitchChannelIndex); + } + break; + + case MSP_BOXNAMES: + serializeBoxNamesReply(dst); + break; + + case MSP_BOXIDS: + for (i = 0; i < activeBoxIdCount; i++) { + const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]); + if (!box) { + continue; + } + sbufWriteU8(dst, box->permanentId); + } + break; + + case MSP_MISC: + sbufWriteU16(dst, masterConfig.rxConfig.midrc); + + sbufWriteU16(dst, masterConfig.motorConfig.minthrottle); + sbufWriteU16(dst, masterConfig.motorConfig.maxthrottle); + sbufWriteU16(dst, masterConfig.motorConfig.mincommand); + + sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle); + +#ifdef GPS + sbufWriteU8(dst, masterConfig.gpsConfig.provider); // gps_type + sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t + sbufWriteU8(dst, masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas +#else + sbufWriteU8(dst, 0); // gps_type + sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t + sbufWriteU8(dst, 0); // gps_ubx_sbas +#endif + sbufWriteU8(dst, masterConfig.batteryConfig.multiwiiCurrentMeterOutput); + sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel); + sbufWriteU8(dst, 0); + + sbufWriteU16(dst, currentProfile->mag_declination / 10); + + sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale); + sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage); + sbufWriteU8(dst, masterConfig.batteryConfig.vbatmaxcellvoltage); + sbufWriteU8(dst, masterConfig.batteryConfig.vbatwarningcellvoltage); + break; + + case MSP_MOTOR_PINS: + // FIXME This is hardcoded and should not be. + for (i = 0; i < 8; i++) + sbufWriteU8(dst, i + 1); + break; + +#ifdef GPS + case MSP_RAW_GPS: + sbufWriteU8(dst, gpsSol.fixType); + sbufWriteU8(dst, gpsSol.numSat); + sbufWriteU32(dst, gpsSol.llh.lat); + sbufWriteU32(dst, gpsSol.llh.lon); + sbufWriteU16(dst, gpsSol.llh.alt/100); // meters + sbufWriteU16(dst, gpsSol.groundSpeed); + sbufWriteU16(dst, gpsSol.groundCourse); + sbufWriteU16(dst, gpsSol.hdop); + break; + + case MSP_COMP_GPS: + sbufWriteU16(dst, GPS_distanceToHome); + sbufWriteU16(dst, GPS_directionToHome); + sbufWriteU8(dst, gpsSol.flags.gpsHeartbeat ? 1 : 0); + break; +#ifdef NAV + case MSP_NAV_STATUS: + sbufWriteU8(dst, NAV_Status.mode); + sbufWriteU8(dst, NAV_Status.state); + sbufWriteU8(dst, NAV_Status.activeWpAction); + sbufWriteU8(dst, NAV_Status.activeWpNumber); + sbufWriteU8(dst, NAV_Status.error); + //sbufWriteU16(dst, (int16_t)(target_bearing/100)); + sbufWriteU16(dst, getMagHoldHeading()); + break; + case MSP_WP: + msp_wp_no = sbufReadU8(src); // get the wp number + getWaypoint(msp_wp_no, &msp_wp); + sbufWriteU8(dst, msp_wp_no); // wp_no + sbufWriteU8(dst, msp_wp.action); // action (WAYPOINT) + sbufWriteU32(dst, msp_wp.lat); // lat + sbufWriteU32(dst, msp_wp.lon); // lon + sbufWriteU32(dst, msp_wp.alt); // altitude (cm) + sbufWriteU16(dst, msp_wp.p1); // P1 + sbufWriteU16(dst, msp_wp.p2); // P2 + sbufWriteU16(dst, msp_wp.p3); // P3 + sbufWriteU8(dst, msp_wp.flag); // flags + break; +#endif + + case MSP_GPSSVINFO: + /* Compatibility stub - return zero SVs */ + sbufWriteU8(dst, 1); + + // HDOP + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, gpsSol.hdop / 100); + sbufWriteU8(dst, gpsSol.hdop / 100); + break; + + case MSP_GPSSTATISTICS: + sbufWriteU16(dst, gpsStats.lastMessageDt); + sbufWriteU32(dst, gpsStats.errors); + sbufWriteU32(dst, gpsStats.timeouts); + sbufWriteU32(dst, gpsStats.packetCount); + sbufWriteU16(dst, gpsSol.hdop); + sbufWriteU16(dst, gpsSol.eph); + sbufWriteU16(dst, gpsSol.epv); + break; +#endif + case MSP_DEBUG: + // output some useful QA statistics + // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] + + for (i = 0; i < DEBUG16_VALUE_COUNT; i++) + sbufWriteU16(dst, debug[i]); // 4 variables are here for general monitoring purpose + break; + + case MSP_UID: + sbufWriteU32(dst, U_ID_0); + sbufWriteU32(dst, U_ID_1); + sbufWriteU32(dst, U_ID_2); + break; + + case MSP_FEATURE: + sbufWriteU32(dst, featureMask()); + break; + + case MSP_BOARD_ALIGNMENT: + sbufWriteU16(dst, masterConfig.boardAlignment.rollDeciDegrees); + sbufWriteU16(dst, masterConfig.boardAlignment.pitchDeciDegrees); + sbufWriteU16(dst, masterConfig.boardAlignment.yawDeciDegrees); + break; + + case MSP_VOLTAGE_METER_CONFIG: + sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale); + sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage); + sbufWriteU8(dst, masterConfig.batteryConfig.vbatmaxcellvoltage); + sbufWriteU8(dst, masterConfig.batteryConfig.vbatwarningcellvoltage); + break; + + case MSP_CURRENT_METER_CONFIG: + sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale); + sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset); + sbufWriteU8(dst, masterConfig.batteryConfig.currentMeterType); + sbufWriteU16(dst, masterConfig.batteryConfig.batteryCapacity); + break; + + case MSP_MIXER: + sbufWriteU8(dst, masterConfig.mixerMode); + break; + + case MSP_RX_CONFIG: + sbufWriteU8(dst, masterConfig.rxConfig.serialrx_provider); + sbufWriteU16(dst, masterConfig.rxConfig.maxcheck); + sbufWriteU16(dst, masterConfig.rxConfig.midrc); + sbufWriteU16(dst, masterConfig.rxConfig.mincheck); + sbufWriteU8(dst, masterConfig.rxConfig.spektrum_sat_bind); + sbufWriteU16(dst, masterConfig.rxConfig.rx_min_usec); + sbufWriteU16(dst, masterConfig.rxConfig.rx_max_usec); + sbufWriteU8(dst, 0); // for compatibility with betaflight + sbufWriteU8(dst, 0); // for compatibility with betaflight + sbufWriteU16(dst, 0); // for compatibility with betaflight + sbufWriteU8(dst, masterConfig.rxConfig.rx_spi_protocol); + sbufWriteU32(dst, masterConfig.rxConfig.rx_spi_id); + sbufWriteU8(dst, masterConfig.rxConfig.rx_spi_rf_channel_count); + break; + + case MSP_FAILSAFE_CONFIG: + sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_delay); + sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_off_delay); + sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle); + sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_kill_switch); + sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle_low_delay); + sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_procedure); + break; + + case MSP_RXFAIL_CONFIG: + for (i = 0; i < rxRuntimeConfig.channelCount; i++) { + sbufWriteU8(dst, masterConfig.rxConfig.failsafe_channel_configurations[i].mode); + sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_channel_configurations[i].step)); + } + break; + + case MSP_RSSI_CONFIG: + sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel); + break; + + case MSP_RX_MAP: + for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) + sbufWriteU8(dst, masterConfig.rxConfig.rcmap[i]); + break; + + case MSP_BF_CONFIG: + sbufWriteU8(dst, masterConfig.mixerMode); + + sbufWriteU32(dst, featureMask()); + + sbufWriteU8(dst, masterConfig.rxConfig.serialrx_provider); + + sbufWriteU16(dst, masterConfig.boardAlignment.rollDeciDegrees); + sbufWriteU16(dst, masterConfig.boardAlignment.pitchDeciDegrees); + sbufWriteU16(dst, masterConfig.boardAlignment.yawDeciDegrees); + + sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale); + sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset); + break; + + case MSP_CF_SERIAL_CONFIG: + for (i = 0; i < SERIAL_PORT_COUNT; i++) { + if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) { + continue; + }; + sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].identifier); + sbufWriteU16(dst, masterConfig.serialConfig.portConfigs[i].functionMask); + sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex); + sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex); + sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex); + sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex); + } + break; + +#ifdef LED_STRIP + case MSP_LED_COLORS: + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { + hsvColor_t *color = &masterConfig.colors[i]; + sbufWriteU16(dst, color->h); + sbufWriteU8(dst, color->s); + sbufWriteU8(dst, color->v); + } + break; + + case MSP_LED_STRIP_CONFIG: + for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { + ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + sbufWriteU32(dst, *ledConfig); + } + break; + + case MSP_LED_STRIP_MODECOLOR: + for (int i = 0; i < LED_MODE_COUNT; i++) { + for (int j = 0; j < LED_DIRECTION_COUNT; j++) { + sbufWriteU8(dst, i); + sbufWriteU8(dst, j); + sbufWriteU8(dst, masterConfig.modeColors[i].color[j]); + } + } + + for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + sbufWriteU8(dst, LED_MODE_COUNT); + sbufWriteU8(dst, j); + sbufWriteU8(dst, masterConfig.specialColors.color[j]); + } + break; +#endif + + case MSP_DATAFLASH_SUMMARY: + serializeDataflashSummaryReply(dst); + break; + +#ifdef USE_FLASHFS + case MSP_DATAFLASH_READ: + { + const uint32_t readAddress = sbufReadU32(src); + serializeDataflashReadReply(dst, readAddress, 128); + } + break; +#endif + case MSP_BLACKBOX_CONFIG: +#ifdef BLACKBOX + sbufWriteU8(dst, 1); //Blackbox supported + sbufWriteU8(dst, masterConfig.blackbox_device); + sbufWriteU8(dst, masterConfig.blackbox_rate_num); + sbufWriteU8(dst, masterConfig.blackbox_rate_denom); +#else + sbufWriteU8(dst, 0); // Blackbox not supported + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); +#endif + break; + + case MSP_SDCARD_SUMMARY: + serializeSDCardSummaryReply(dst); + break; + + case MSP_BF_BUILD_INFO: + for (i = 0; i < 11; i++) + sbufWriteU8(dst, buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc + sbufWriteU32(dst, 0); // future exp + sbufWriteU32(dst, 0); // future exp + break; + + case MSP_3D: + sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_low); + sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_high); + sbufWriteU16(dst, masterConfig.flight3DConfig.neutral3d); + break; + + case MSP_RC_DEADBAND: + sbufWriteU8(dst, currentProfile->rcControlsConfig.deadband); + sbufWriteU8(dst, currentProfile->rcControlsConfig.yaw_deadband); + sbufWriteU8(dst, currentProfile->rcControlsConfig.alt_hold_deadband); + sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_throttle); + break; + + case MSP_SENSOR_ALIGNMENT: + sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.gyro_align); + sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.acc_align); + sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.mag_align); + break; + + case MSP_ADVANCED_CONFIG: + sbufWriteU8(dst, masterConfig.gyroSyncDenominator); + sbufWriteU8(dst, 1); // BF: masterConfig.pid_process_denom + sbufWriteU8(dst, 1); // BF: masterConfig.motorConfig.useUnsyncedPwm + sbufWriteU8(dst, masterConfig.motorConfig.motorPwmProtocol); + sbufWriteU16(dst, masterConfig.motorConfig.motorPwmRate); + sbufWriteU16(dst, masterConfig.servoConfig.servoPwmRate); + sbufWriteU8(dst, masterConfig.gyroSync); + break; + + case MSP_REBOOT: + if (mspPostProcessFn) { + *mspPostProcessFn = mspRebootFn; + } + break; + + +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE + case MSP_SET_4WAY_IF: + // get channel number + // switch all motor lines HI + // reply with the count of ESC found + sbufWriteU8(dst, esc4wayInit()); + if (mspPostProcessFn) { + *mspPostProcessFn = msp4WayIfFn; + } + break; +#endif + + default: + return false; + } + return true; +} + +static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) +{ + uint32_t i; + uint16_t tmp; + uint8_t rate; + + const unsigned int dataSize = sbufBytesRemaining(src); +#ifdef NAV + uint8_t msp_wp_no; + navWaypoint_t msp_wp; +#endif + + switch (cmdMSP) { +#ifdef HIL + case MSP_SET_HIL_STATE: + hilToFC.rollAngle = sbufReadU16(src); + hilToFC.pitchAngle = sbufReadU16(src); + hilToFC.yawAngle = sbufReadU16(src); + hilToFC.baroAlt = sbufReadU32(src); + hilToFC.bodyAccel[0] = sbufReadU16(src); + hilToFC.bodyAccel[1] = sbufReadU16(src); + hilToFC.bodyAccel[2] = sbufReadU16(src); + hilActive = true; + break; +#endif + case MSP_SELECT_SETTING: + if (!ARMING_FLAG(ARMED)) { + masterConfig.current_profile_index = sbufReadU8(src); + if (masterConfig.current_profile_index > 2) { + masterConfig.current_profile_index = 0; + } + writeEEPROM(); + readEEPROM(); + } + break; + + case MSP_SET_HEAD: + updateMagHoldHeading(sbufReadU16(src)); + break; + + case MSP_SET_RAW_RC: +#ifndef SKIP_RX_MSP + { + uint8_t channelCount = dataSize / sizeof(uint16_t); + if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) { + return MSP_RESULT_ERROR; + } else { + uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + + for (i = 0; i < channelCount; i++) { + frame[i] = sbufReadU16(src); + } + + rxMspFrameReceive(frame, channelCount); + } + } +#endif + break; + + case MSP_SET_ARMING_CONFIG: + masterConfig.auto_disarm_delay = sbufReadU8(src); + masterConfig.disarm_kill_switch = sbufReadU8(src); + break; + + case MSP_SET_LOOP_TIME: + masterConfig.looptime = sbufReadU16(src); + break; + + case MSP_SET_PID_CONTROLLER: + // FIXME: Do nothing + break; + + case MSP_SET_PID: + for (i = 0; i < PID_ITEM_COUNT; i++) { + currentProfile->pidProfile.P8[i] = sbufReadU8(src); + currentProfile->pidProfile.I8[i] = sbufReadU8(src); + currentProfile->pidProfile.D8[i] = sbufReadU8(src); + } + break; + + case MSP_SET_MODE_RANGE: + i = sbufReadU8(src); + if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { + modeActivationCondition_t *mac = ¤tProfile->modeActivationConditions[i]; + i = sbufReadU8(src); + const box_t *box = findBoxByPermenantId(i); + if (box) { + mac->modeId = box->boxId; + mac->auxChannelIndex = sbufReadU8(src); + mac->range.startStep = sbufReadU8(src); + mac->range.endStep = sbufReadU8(src); + + useRcControlsConfig(currentProfile->modeActivationConditions, &masterConfig.motorConfig, ¤tProfile->pidProfile); + } else { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } + break; + + case MSP_SET_ADJUSTMENT_RANGE: + i = sbufReadU8(src); + if (i < MAX_ADJUSTMENT_RANGE_COUNT) { + adjustmentRange_t *adjRange = ¤tProfile->adjustmentRanges[i]; + i = sbufReadU8(src); + if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { + adjRange->adjustmentIndex = i; + adjRange->auxChannelIndex = sbufReadU8(src); + adjRange->range.startStep = sbufReadU8(src); + adjRange->range.endStep = sbufReadU8(src); + adjRange->adjustmentFunction = sbufReadU8(src); + adjRange->auxSwitchChannelIndex = sbufReadU8(src); + } else { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } + break; + + case MSP_SET_RC_TUNING: + if (dataSize >= 10) { + sbufReadU8(src); //Read rcRate8, kept for protocol compatibility reasons + currentControlRateProfile->rcExpo8 = sbufReadU8(src); + for (i = 0; i < 3; i++) { + rate = sbufReadU8(src); + if (i == FD_YAW) { + currentControlRateProfile->rates[i] = constrain(rate, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + } + else { + currentControlRateProfile->rates[i] = constrain(rate, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + } + } + rate = sbufReadU8(src); + currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); + currentControlRateProfile->thrMid8 = sbufReadU8(src); + currentControlRateProfile->thrExpo8 = sbufReadU8(src); + currentControlRateProfile->tpa_breakpoint = sbufReadU16(src); + if (dataSize >= 11) { + currentControlRateProfile->rcYawExpo8 = sbufReadU8(src); + } + } else { + return MSP_RESULT_ERROR; + } + break; + + case MSP_SET_MISC: + tmp = sbufReadU16(src); + if (tmp < 1600 && tmp > 1400) + masterConfig.rxConfig.midrc = tmp; + + masterConfig.motorConfig.minthrottle = sbufReadU16(src); + masterConfig.motorConfig.maxthrottle = sbufReadU16(src); + masterConfig.motorConfig.mincommand = sbufReadU16(src); + + masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src); + +#ifdef GPS + masterConfig.gpsConfig.provider = sbufReadU8(src); // gps_type + sbufReadU8(src); // gps_baudrate + masterConfig.gpsConfig.sbasMode = sbufReadU8(src); // gps_ubx_sbas +#else + sbufReadU8(src); // gps_type + sbufReadU8(src); // gps_baudrate + sbufReadU8(src); // gps_ubx_sbas +#endif + masterConfig.batteryConfig.multiwiiCurrentMeterOutput = sbufReadU8(src); + masterConfig.rxConfig.rssi_channel = sbufReadU8(src); + sbufReadU8(src); + + currentProfile->mag_declination = sbufReadU16(src) * 10; + + masterConfig.batteryConfig.vbatscale = sbufReadU8(src); // actual vbatscale as intended + masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI + masterConfig.batteryConfig.vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI + masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert + break; + + case MSP_SET_MOTOR: + for (i = 0; i < 8; i++) { + const int16_t disarmed = sbufReadU16(src); + if (i < MAX_SUPPORTED_MOTORS) { + motor_disarmed[i] = disarmed; + } + } + break; + + case MSP_SET_SERVO_CONFIGURATION: +#ifdef USE_SERVOS + if (dataSize != 1 + sizeof(servoParam_t)) { + return MSP_RESULT_ERROR; + break; + } + i = sbufReadU8(src); + if (i >= MAX_SUPPORTED_SERVOS) { + return MSP_RESULT_ERROR; + } else { + currentProfile->servoConf[i].min = sbufReadU16(src); + currentProfile->servoConf[i].max = sbufReadU16(src); + currentProfile->servoConf[i].middle = sbufReadU16(src); + currentProfile->servoConf[i].rate = sbufReadU8(src); + currentProfile->servoConf[i].angleAtMin = sbufReadU8(src); + currentProfile->servoConf[i].angleAtMax = sbufReadU8(src); + currentProfile->servoConf[i].forwardFromChannel = sbufReadU8(src); + currentProfile->servoConf[i].reversedSources = sbufReadU32(src); + } +#endif + break; + + case MSP_SET_SERVO_MIX_RULE: +#ifdef USE_SERVOS + i = sbufReadU8(src); + if (i >= MAX_SERVO_RULES) { + return MSP_RESULT_ERROR; + } else { + masterConfig.customServoMixer[i].targetChannel = sbufReadU8(src); + masterConfig.customServoMixer[i].inputSource = sbufReadU8(src); + masterConfig.customServoMixer[i].rate = sbufReadU8(src); + masterConfig.customServoMixer[i].speed = sbufReadU8(src); + masterConfig.customServoMixer[i].min = sbufReadU8(src); + masterConfig.customServoMixer[i].max = sbufReadU8(src); + sbufReadU8(src); //Read 1 byte for `box` and ignore it + loadCustomServoMixer(); + } +#endif + break; + + case MSP_SET_3D: + masterConfig.flight3DConfig.deadband3d_low = sbufReadU16(src); + masterConfig.flight3DConfig.deadband3d_high = sbufReadU16(src); + masterConfig.flight3DConfig.neutral3d = sbufReadU16(src); + masterConfig.flight3DConfig.deadband3d_throttle = sbufReadU16(src); + break; + + case MSP_SET_RC_DEADBAND: + currentProfile->rcControlsConfig.deadband = sbufReadU8(src); + currentProfile->rcControlsConfig.yaw_deadband = sbufReadU8(src); + currentProfile->rcControlsConfig.alt_hold_deadband = sbufReadU8(src); + break; + + case MSP_SET_RESET_CURR_PID: + resetPidProfile(¤tProfile->pidProfile); + break; + + case MSP_SET_SENSOR_ALIGNMENT: + masterConfig.sensorAlignmentConfig.gyro_align = sbufReadU8(src); + masterConfig.sensorAlignmentConfig.acc_align = sbufReadU8(src); + masterConfig.sensorAlignmentConfig.mag_align = sbufReadU8(src); + break; + + case MSP_SET_ADVANCED_CONFIG: + masterConfig.gyroSyncDenominator = sbufReadU8(src); + sbufReadU8(src); // BF: masterConfig.pid_process_denom + sbufReadU8(src); // BF: masterConfig.motorConfig.useUnsyncedPwm + masterConfig.motorConfig.motorPwmProtocol = sbufReadU8(src); + masterConfig.motorConfig.motorPwmRate = sbufReadU16(src); + masterConfig.servoConfig.servoPwmRate = sbufReadU16(src); + masterConfig.gyroSync = sbufReadU8(src); + break; + + case MSP_RESET_CONF: + if (!ARMING_FLAG(ARMED)) { + resetEEPROM(); + readEEPROM(); + } + break; + + case MSP_ACC_CALIBRATION: + if (!ARMING_FLAG(ARMED)) + accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); + break; + + case MSP_MAG_CALIBRATION: + if (!ARMING_FLAG(ARMED)) + ENABLE_STATE(CALIBRATE_MAG); + break; + + case MSP_EEPROM_WRITE: + if (ARMING_FLAG(ARMED)) { + return MSP_RESULT_ERROR; + } + writeEEPROM(); + readEEPROM(); + break; + +#ifdef BLACKBOX + case MSP_SET_BLACKBOX_CONFIG: + // Don't allow config to be updated while Blackbox is logging + if (!blackboxMayEditConfig()) + return false; + masterConfig.blackbox_device = sbufReadU8(src); + masterConfig.blackbox_rate_num = sbufReadU8(src); + masterConfig.blackbox_rate_denom = sbufReadU8(src); + break; +#endif + +#ifdef USE_FLASHFS + case MSP_DATAFLASH_ERASE: + flashfsEraseCompletely(); + break; +#endif + +#ifdef GPS + case MSP_SET_RAW_GPS: + if (sbufReadU8(src)) { + ENABLE_STATE(GPS_FIX); + } else { + DISABLE_STATE(GPS_FIX); + } + gpsSol.flags.validVelNE = 0; + gpsSol.flags.validVelD = 0; + gpsSol.flags.validEPE = 0; + gpsSol.numSat = sbufReadU8(src); + gpsSol.llh.lat = sbufReadU32(src); + gpsSol.llh.lon = sbufReadU32(src); + gpsSol.llh.alt = sbufReadU16(src); + gpsSol.groundSpeed = sbufReadU16(src); + gpsSol.velNED[X] = 0; + gpsSol.velNED[Y] = 0; + gpsSol.velNED[Z] = 0; + gpsSol.eph = 100; + gpsSol.epv = 100; + // Feed data to navigation + sensorsSet(SENSOR_GPS); + onNewGPSData(); + break; +#endif +#ifdef NAV + case MSP_SET_WP: + msp_wp_no = sbufReadU8(src); // get the wp number + msp_wp.action = sbufReadU8(src); // action + msp_wp.lat = sbufReadU32(src); // lat + msp_wp.lon = sbufReadU32(src); // lon + msp_wp.alt = sbufReadU32(src); // to set altitude (cm) + msp_wp.p1 = sbufReadU16(src); // P1 + msp_wp.p2 = sbufReadU16(src); // P2 + msp_wp.p3 = sbufReadU16(src); // P3 + msp_wp.flag = sbufReadU8(src); // future: to set nav flag + setWaypoint(msp_wp_no, &msp_wp); + break; +#endif + case MSP_SET_FEATURE: + featureClearAll(); + featureSet(sbufReadU32(src)); // features bitmap + break; + + case MSP_SET_BOARD_ALIGNMENT: + masterConfig.boardAlignment.rollDeciDegrees = sbufReadU16(src); + masterConfig.boardAlignment.pitchDeciDegrees = sbufReadU16(src); + masterConfig.boardAlignment.yawDeciDegrees = sbufReadU16(src); + break; + + case MSP_SET_VOLTAGE_METER_CONFIG: + masterConfig.batteryConfig.vbatscale = sbufReadU8(src); // actual vbatscale as intended + masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI + masterConfig.batteryConfig.vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI + masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert + break; + + case MSP_SET_CURRENT_METER_CONFIG: + masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src); + masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src); + masterConfig.batteryConfig.currentMeterType = sbufReadU8(src); + masterConfig.batteryConfig.batteryCapacity = sbufReadU16(src); + break; + +#ifndef USE_QUAD_MIXER_ONLY + case MSP_SET_MIXER: + masterConfig.mixerMode = sbufReadU8(src); + break; +#endif + + case MSP_SET_RX_CONFIG: + masterConfig.rxConfig.serialrx_provider = sbufReadU8(src); + masterConfig.rxConfig.maxcheck = sbufReadU16(src); + masterConfig.rxConfig.midrc = sbufReadU16(src); + masterConfig.rxConfig.mincheck = sbufReadU16(src); + masterConfig.rxConfig.spektrum_sat_bind = sbufReadU8(src); + if (dataSize > 8) { + masterConfig.rxConfig.rx_min_usec = sbufReadU16(src); + masterConfig.rxConfig.rx_max_usec = sbufReadU16(src); + } + if (dataSize > 12) { + // for compatibility with betaflight + sbufReadU8(src); + sbufReadU8(src); + sbufReadU16(src); + } + if (dataSize > 16) { + masterConfig.rxConfig.rx_spi_protocol = sbufReadU8(src); + } + if (dataSize > 17) { + masterConfig.rxConfig.rx_spi_id = sbufReadU32(src); + } + if (dataSize > 21) { + masterConfig.rxConfig.rx_spi_rf_channel_count = sbufReadU8(src); + } + break; + + case MSP_SET_FAILSAFE_CONFIG: + masterConfig.failsafeConfig.failsafe_delay = sbufReadU8(src); + masterConfig.failsafeConfig.failsafe_off_delay = sbufReadU8(src); + masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src); + masterConfig.failsafeConfig.failsafe_kill_switch = sbufReadU8(src); + masterConfig.failsafeConfig.failsafe_throttle_low_delay = sbufReadU16(src); + masterConfig.failsafeConfig.failsafe_procedure = sbufReadU8(src); + break; + + case MSP_SET_RXFAIL_CONFIG: + i = sbufReadU8(src); + if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) { + masterConfig.rxConfig.failsafe_channel_configurations[i].mode = sbufReadU8(src); + masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src)); + } else { + return MSP_RESULT_ERROR; + } + break; + + case MSP_SET_RSSI_CONFIG: + masterConfig.rxConfig.rssi_channel = sbufReadU8(src); + break; + + case MSP_SET_RX_MAP: + for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { + masterConfig.rxConfig.rcmap[i] = sbufReadU8(src); + } + break; + + case MSP_SET_BF_CONFIG: +#ifdef USE_QUAD_MIXER_ONLY + sbufReadU8(src); // mixerMode ignored +#else + masterConfig.mixerMode = sbufReadU8(src); // mixerMode +#endif + + featureClearAll(); + featureSet(sbufReadU32(src)); // features bitmap + + masterConfig.rxConfig.serialrx_provider = sbufReadU8(src); // serialrx_type + + masterConfig.boardAlignment.rollDeciDegrees = sbufReadU16(src); // board_align_roll + masterConfig.boardAlignment.pitchDeciDegrees = sbufReadU16(src); // board_align_pitch + masterConfig.boardAlignment.yawDeciDegrees = sbufReadU16(src); // board_align_yaw + + masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src); + masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src); + break; + + case MSP_SET_CF_SERIAL_CONFIG: + { + uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4); + + if (dataSize % portConfigSize != 0) { + return MSP_RESULT_ERROR; + break; + } + + uint8_t remainingPortsInPacket = dataSize / portConfigSize; + + while (remainingPortsInPacket--) { + uint8_t identifier = sbufReadU8(src); + + serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier); + if (!portConfig) { + return MSP_RESULT_ERROR; + break; + } + + portConfig->identifier = identifier; + portConfig->functionMask = sbufReadU16(src); + portConfig->msp_baudrateIndex = sbufReadU8(src); + portConfig->gps_baudrateIndex = sbufReadU8(src); + portConfig->telemetry_baudrateIndex = sbufReadU8(src); + portConfig->blackbox_baudrateIndex = sbufReadU8(src); + } + } + break; + +#ifdef LED_STRIP + case MSP_SET_LED_COLORS: + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { + hsvColor_t *color = &masterConfig.colors[i]; + color->h = sbufReadU16(src); + color->s = sbufReadU8(src); + color->v = sbufReadU8(src); + } + break; + + case MSP_SET_LED_STRIP_CONFIG: + { + i = sbufReadU8(src); + if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) { + return MSP_RESULT_ERROR; + break; + } + ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; + *ledConfig = sbufReadU32(src); + reevaluateLedConfig(); + } + break; + + case MSP_SET_LED_STRIP_MODECOLOR: + { + ledModeIndex_e modeIdx = sbufReadU8(src); + int funIdx = sbufReadU8(src); + int color = sbufReadU8(src); + + if (!setModeColor(modeIdx, funIdx, color)) + return MSP_RESULT_ERROR; + } + break; +#endif + + default: + return MSP_RESULT_ERROR; + } + return MSP_RESULT_ACK; +} + +/* + * Returns MSP_RESULT_ACK, MSP_RESULT_ERROR or MSP_RESULT_NO_REPLY + */ +mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) +{ + int ret = MSP_RESULT_ACK; + sbuf_t *dst = &reply->buf; + sbuf_t *src = &cmd->buf; + const uint8_t cmdMSP = cmd->cmd; + // initialize reply by default + reply->cmd = cmd->cmd; + + if (mspFcProcessOutCommand(cmdMSP, dst, src, mspPostProcessFn)) { + ret = MSP_RESULT_ACK; + } else { + ret = mspFcProcessInCommand(cmdMSP, src); + } + reply->result = ret; + return ret; +} + +/* + * Return a pointer to the process command function + */ +mspProcessCommandFnPtr mspFcInit(void) +{ + initActiveBoxIds(); + return mspFcProcessCommand; +} diff --git a/src/main/fc/msp_fc.h b/src/main/fc/msp_fc.h new file mode 100644 index 00000000000..7c5356c8673 --- /dev/null +++ b/src/main/fc/msp_fc.h @@ -0,0 +1,21 @@ +/* + * 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 "msp/msp.h" + +mspProcessCommandFnPtr mspFcInit(void); + diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index ab7ed7a58ba..e999be18c11 100755 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -18,14 +18,12 @@ #include #include #include -#include #include "platform.h" -#include "build/debug.h" +#include "blackbox/blackbox.h" -#include "scheduler/scheduler.h" -#include "scheduler/scheduler_tasks.h" +#include "build/debug.h" #include "common/maths.h" #include "common/axis.h" @@ -35,20 +33,15 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" -#include "drivers/compass.h" #include "drivers/light_led.h" -#include "drivers/gpio.h" #include "drivers/system.h" #include "drivers/serial.h" -#include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/gyro_sync.h" #include "sensors/sensors.h" #include "sensors/boardalignment.h" -#include "sensors/rangefinder.h" -#include "sensors/compass.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/gyro.h" @@ -60,32 +53,35 @@ #include "io/beeper.h" #include "io/display.h" -#include "io/escservo.h" +#include "io/motors.h" +#include "io/servos.h" #include "io/gimbal.h" #include "io/gps.h" -#include "io/ledstrip.h" #include "io/serial.h" #include "io/serial_cli.h" -#include "io/serial_msp.h" #include "io/statusindicator.h" #include "io/asyncfatfs/asyncfatfs.h" +#include "msp/msp_serial.h" + #include "rx/rx.h" #include "rx/msp.h" +#include "scheduler/scheduler.h" + #include "telemetry/telemetry.h" -#include "blackbox/blackbox.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" -#include "flight/hil.h" #include "flight/failsafe.h" #include "flight/navigation_rewrite.h" #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" // June 2013 V2.2-dev @@ -95,10 +91,6 @@ enum { ALIGN_MAG = 2 }; -/* VBAT monitoring interval (in microseconds) - 1s*/ -#define VBATINTERVAL (6 * 3500) -/* IBat monitoring interval (in microseconds) - 6 default looptimes */ -#define IBATINTERVAL (6 * 3500) #define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro uint16_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 @@ -214,12 +206,12 @@ void mwDisarm(void) } } -#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK) +#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) void releaseSharedTelemetryPorts(void) { serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); while (sharedPort) { - mspReleasePortIfAllocated(sharedPort); + mspSerialReleasePortIfAllocated(sharedPort); sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); } } @@ -238,11 +230,13 @@ void mwArm(void) ENABLE_ARMING_FLAG(WAS_EVER_ARMED); headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); + resetMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); + #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); if (sharedBlackboxAndMspPort) { - mspReleasePortIfAllocated(sharedBlackboxAndMspPort); + mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort); } startBlackbox(); } @@ -337,7 +331,7 @@ void processRx(void) } } - processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.disarm_kill_switch); + processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.disarm_kill_switch, masterConfig.fixed_wing_auto_arm); updateActivatedModes(currentProfile->modeActivationConditions, currentProfile->modeActivationOperator); @@ -407,8 +401,8 @@ void processRx(void) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { + resetMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); ENABLE_FLIGHT_MODE(MAG_MODE); - updateMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); } } else { DISABLE_FLIGHT_MODE(MAG_MODE); @@ -476,7 +470,7 @@ void processRx(void) } else { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); - mspAllocateSerialPorts(); + mspSerialAllocatePorts(); } } #endif @@ -498,7 +492,7 @@ void filterRc(bool isRXDataNew) // Calculate average cycle time (1Hz LPF on cycle time) if (!filterInitialised) { - biquadFilterInit(&filteredCycleTimeState, 1, gyro.targetLooptime); + biquadFilterInitLPF(&filteredCycleTimeState, 1, gyro.targetLooptime); filterInitialised = true; } @@ -561,7 +555,7 @@ void taskMainPidLoop(void) if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS - && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo) + && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo) #endif && masterConfig.mixerMode != MIXER_AIRPLANE && masterConfig.mixerMode != MIXER_FLYING_WING @@ -583,10 +577,10 @@ void taskMainPidLoop(void) } if (thrTiltCompStrength) { - rcCommand[THROTTLE] = constrain(masterConfig.escAndServoConfig.minthrottle - + (rcCommand[THROTTLE] - masterConfig.escAndServoConfig.minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength), - masterConfig.escAndServoConfig.minthrottle, - masterConfig.escAndServoConfig.maxthrottle); + rcCommand[THROTTLE] = constrain(masterConfig.motorConfig.minthrottle + + (rcCommand[THROTTLE] - masterConfig.motorConfig.minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength), + masterConfig.motorConfig.minthrottle, + masterConfig.motorConfig.maxthrottle); } } @@ -631,6 +625,7 @@ void taskMainPidLoop(void) handleBlackbox(); } #endif + } // Function for loop trigger @@ -650,39 +645,6 @@ void taskMainPidLoopChecker(void) { taskMainPidLoop(); } -void taskHandleSerial(void) -{ - handleSerial(); -} - -void taskUpdateBeeper(void) -{ - beeperUpdate(); //call periodic beeper handler -} - -void taskUpdateBattery(void) -{ - static uint32_t vbatLastServiced = 0; - static uint32_t ibatLastServiced = 0; - - if (feature(FEATURE_VBAT)) { - if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) { - uint32_t vbatTimeDelta = currentTime - vbatLastServiced; - vbatLastServiced = currentTime; - updateBattery(vbatTimeDelta); - } - } - - if (feature(FEATURE_CURRENT_METER)) { - int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced); - - if (ibatTimeSinceLastServiced >= IBATINTERVAL) { - ibatLastServiced = currentTime; - updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); - } - } -} - bool taskUpdateRxCheck(uint32_t currentDeltaTime) { UNUSED(currentDeltaTime); @@ -696,82 +658,3 @@ void taskUpdateRxMain(void) updatePIDCoefficients(¤tProfile->pidProfile, currentControlRateProfile, &masterConfig.rxConfig); isRXDataNew = true; } - -#ifdef GPS -void taskProcessGPS(void) -{ - // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck - // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will - // change this based on available hardware - if (feature(FEATURE_GPS)) { - gpsThread(); - } - - if (sensors(SENSOR_GPS)) { - updateGpsIndicator(currentTime); - } -} -#endif - -#ifdef MAG -void taskUpdateCompass(void) -{ - if (sensors(SENSOR_MAG)) { - updateCompass(&masterConfig.magZero); - } -} -#endif - -#ifdef BARO -void taskUpdateBaro(void) -{ - if (sensors(SENSOR_BARO)) { - const uint32_t newDeadline = baroUpdate(); - if (newDeadline != 0) { - rescheduleTask(TASK_SELF, newDeadline); - } - } - - //updatePositionEstimator_BaroTopic(currentTime); -} -#endif - -#ifdef SONAR -void taskUpdateSonar(void) -{ - if (sensors(SENSOR_SONAR)) { - rangefinderUpdate(); - } - - //updatePositionEstimator_SonarTopic(currentTime); -} -#endif - -#ifdef DISPLAY -void taskUpdateDisplay(void) -{ - if (feature(FEATURE_DISPLAY)) { - updateDisplay(); - } -} -#endif - -#ifdef TELEMETRY -void taskTelemetry(void) -{ - telemetryCheckState(); - - if (!cliMode && feature(FEATURE_TELEMETRY)) { - telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); - } -} -#endif - -#ifdef LED_STRIP -void taskLedStrip(void) -{ - if (feature(FEATURE_LED_STRIP)) { - updateLedStrip(); - } -} -#endif diff --git a/src/main/fc/mw.h b/src/main/fc/mw.h index de19f1e0e1e..420b5fae716 100644 --- a/src/main/fc/mw.h +++ b/src/main/fc/mw.h @@ -17,6 +17,8 @@ #pragma once +extern int16_t telemTemperature1; + void handleInflightCalibrationStickPosition(); void mwDisarm(void); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 300d9dd1723..6557a80654c 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -42,7 +42,7 @@ #include "io/gps.h" #include "io/beeper.h" -#include "io/escservo.h" +#include "io/motors.h" #include "fc/rc_controls.h" #include "fc/rc_curves.h" @@ -55,6 +55,7 @@ #include "flight/failsafe.h" #include "config/config.h" +#include "config/feature.h" #include "blackbox/blackbox.h" @@ -62,7 +63,7 @@ #define AIRMODE_DEADBAND 25 -static escAndServoConfig_t *escAndServoConfig; +static motorConfig_t *motorConfig; static pidProfile_t *pidProfile; // true if arming is done via the sticks (as opposed to a switch) @@ -144,7 +145,7 @@ rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig) return NOT_CENTERED; } -void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch) +void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint8_t rcSticks; // this hold sticks position for command combos @@ -169,7 +170,6 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat // perform actions if (!isUsingSticksToArm) { - if (IS_RC_MODE_ACTIVE(BOXARM)) { // Arming via ARM BOX if (throttleStatus == THROTTLE_LOW) { @@ -194,17 +194,22 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat return; } - if (isUsingSticksToArm) { + if (isUsingSticksToArm) { // Disarm on throttle down + yaw if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { - if (ARMING_FLAG(ARMED)) + // Dont disarm if fixedwing and motorstop + if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && fixed_wing_auto_arm) { + return; + } + else if (ARMING_FLAG(ARMED)) { mwDisarm(); + } else { beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held rcDelayCommand = 0; // reset so disarm tone will repeat } } - } + } if (ARMING_FLAG(ARMED)) { // actions during armed @@ -232,19 +237,31 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat return; } + if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) { saveConfigAndNotify(); } - if (isUsingSticksToArm) { - if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { - // Arm via YAW - mwArm(); - return; + if (isUsingSticksToArm) { + if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && fixed_wing_auto_arm) { + // Auto arm on throttle when using fixedwing and motorstop + if (throttleStatus != THROTTLE_LOW) { + // Arm via YAW + mwArm(); + return; + } + } + else { + if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { + // Arm via YAW + mwArm(); + return; + } } } + if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { // Calibrating Acc accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); @@ -501,7 +518,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_THROTTLE_EXPO: newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c controlRateConfig->thrExpo8 = newValue; - generateThrottleCurve(controlRateConfig, escAndServoConfig); + generateThrottleCurve(controlRateConfig, motorConfig); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); break; case ADJUSTMENT_PITCH_ROLL_RATE: @@ -688,9 +705,9 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { return MIN(ABS(rcData[axis] - midrc), 500); } -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse) +void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse) { - escAndServoConfig = escAndServoConfigToUse; + motorConfig = motorConfigToUse; pidProfile = pidProfileToUse; isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index a29029fc126..2c0535b66d8 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -28,21 +28,15 @@ typedef enum { BOXHEADFREE, BOXHEADADJ, BOXCAMSTAB, - BOXCAMTRIG, BOXNAVRTH, // old GPSHOME BOXNAVPOSHOLD, // old GPSHOLD BOXPASSTHRU, BOXBEEPERON, - BOXLEDMAX, BOXLEDLOW, BOXLLIGHTS, - BOXGOV, BOXOSD, BOXTELEMETRY, //BOXGTUNE, - BOXSERVO1, - BOXSERVO2, - BOXSERVO3, BOXBLACKBOX, BOXFAILSAFE, BOXNAVWP, @@ -174,7 +168,7 @@ bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig); -void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); +void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions, modeActivationOperator_e modeActivationOperator); @@ -268,3 +262,6 @@ bool isUsingNavigationModes(void); int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId); +struct motorConfig_s; +struct pidProfile_s; +void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse); diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c index fcf17238df6..35e8065becd 100644 --- a/src/main/fc/rc_curves.c +++ b/src/main/fc/rc_curves.c @@ -21,7 +21,7 @@ #include "rx/rx.h" #include "fc/rc_controls.h" #include "fc/rc_curves.h" -#include "io/escservo.h" +#include "io/motors.h" #define PITCH_LOOKUP_LENGTH 7 @@ -31,9 +31,9 @@ static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE int16_t lookupThrottleRCMid; // THROTTLE curve mid point -void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig) +void generateThrottleCurve(controlRateConfig_t *controlRateConfig, motorConfig_t *motorConfig) { - lookupThrottleRCMid = escAndServoConfig->minthrottle + (int32_t)(escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle) * controlRateConfig->thrMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE] + lookupThrottleRCMid = motorConfig->minthrottle + (int32_t)(motorConfig->maxthrottle - motorConfig->minthrottle) * controlRateConfig->thrMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE] for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { const int16_t tmp = 10 * i - controlRateConfig->thrMid8; @@ -43,7 +43,7 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo if (tmp < 0) y = controlRateConfig->thrMid8; lookupThrottleRC[i] = 10 * controlRateConfig->thrMid8 + tmp * (100 - controlRateConfig->thrExpo8 + (int32_t) controlRateConfig->thrExpo8 * (tmp * tmp) / (y * y)) / 10; - lookupThrottleRC[i] = escAndServoConfig->minthrottle + (int32_t) (escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] + lookupThrottleRC[i] = motorConfig->minthrottle + (int32_t) (motorConfig->maxthrottle - motorConfig->minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] } } diff --git a/src/main/fc/rc_curves.h b/src/main/fc/rc_curves.h index 1cc9ea8822a..147aef4bb91 100644 --- a/src/main/fc/rc_curves.h +++ b/src/main/fc/rc_curves.h @@ -18,8 +18,8 @@ #pragma once struct controlRateConfig_s; -struct escAndServoConfig_s; -void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, struct escAndServoConfig_s *escAndServoConfig); +struct motorConfig_s; +void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, struct motorConfig_s *motorConfig); int16_t rcLookup(int32_t stickDeflection, uint8_t expo); int16_t rcLookupThrottle(int32_t tmp); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index ef265b16631..77dcf04fc50 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -31,7 +31,7 @@ #include "rx/rx.h" #include "drivers/system.h" #include "io/beeper.h" -#include "io/escservo.h" +#include "io/motors.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index f5b1c24e7d2..5d0a48b8f79 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -41,7 +41,7 @@ #include "rx/rx.h" #include "io/gimbal.h" -#include "io/escservo.h" +#include "io/motors.h" #include "fc/rc_controls.h" @@ -50,6 +50,7 @@ #include "sensors/gyro.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/failsafe.h" #include "flight/pid.h" #include "flight/imu.h" @@ -59,6 +60,7 @@ #include "config/config.h" #include "config/config_profile.h" +#include "config/feature.h" //#define MIXER_DEBUG @@ -69,31 +71,15 @@ int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; bool motorLimitReached = false; -static mixerConfig_t *mixerConfig; +mixerConfig_t *mixerConfig; static flight3DConfig_t *flight3DConfig; -static escAndServoConfig_t *escAndServoConfig; +static motorConfig_t *motorConfig; static rxConfig_t *rxConfig; -static mixerMode_e currentMixerMode; +mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; -#ifdef USE_SERVOS -static uint8_t servoRuleCount = 0; -static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; -static gimbalConfig_t *gimbalConfig; -int16_t servo[MAX_SUPPORTED_SERVOS]; -static int servoOutputEnabled; - -static uint8_t mixerUsesServos; -static uint8_t minServoIndex; -static uint8_t maxServoIndex; - -static servoParam_t *servoConf; -static biquadFilter_t servoFitlerState[MAX_SUPPORTED_SERVOS]; -static bool servoFilterIsSet; -#endif - static const motorMixer_t mixerQuadX[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R @@ -281,117 +267,20 @@ const mixer_t mixers[] = { }; #endif // USE_QUAD_MIXER_ONLY -#ifdef USE_SERVOS - -#define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t)) -// mixer rule format servo, input, rate, speed, min, max, box -static const servoMixer_t servoMixerAirplane[] = { - { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, - { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, - { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, - { SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, - { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 }, -}; - -static const servoMixer_t servoMixerFlyingWing[] = { - { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, - { SERVO_FLAPPERON_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, - { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, -100, 0, 0, 100, 0 }, - { SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, - { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 }, -}; - -static const servoMixer_t servoMixerTri[] = { - { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, -}; - -const mixerRules_t servoMixers[] = { - { 0, 0, 0, NULL }, // entry 0 - { COUNT_SERVO_RULES(servoMixerTri), SERVO_RUDDER, SERVO_RUDDER, servoMixerTri }, // MULTITYPE_TRI - { 0, 0, 0, NULL }, // MULTITYPE_QUADP - { 0, 0, 0, NULL }, // MULTITYPE_QUADX - { 0, 0, 0, NULL }, // MULTITYPE_BI - { 0, 0, 0, NULL }, // MULTITYPE_GIMBAL / MIXER_GIMBAL -> disabled - { 0, 0, 0, NULL }, // MULTITYPE_Y6 - { 0, 0, 0, NULL }, // MULTITYPE_HEX6 - { COUNT_SERVO_RULES(servoMixerFlyingWing), SERVO_FLAPPERONS_MIN, SERVO_FLAPPERONS_MAX, servoMixerFlyingWing },// * MULTITYPE_FLYING_WING - { 0, 0, 0, NULL }, // MULTITYPE_Y4 - { 0, 0, 0, NULL }, // MULTITYPE_HEX6X - { 0, 0, 0, NULL }, // MULTITYPE_OCTOX8 - { 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATP - { 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATX - { COUNT_SERVO_RULES(servoMixerAirplane), SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, servoMixerAirplane }, // * MULTITYPE_AIRPLANE - { 0, 0, 0, NULL }, // * MIXER_HELI_120_CCPM -> disabled, never fully implemented in CF - { 0, 0, 0, NULL }, // * MIXER_HELI_90_DEG -> disabled, never fully implemented in CF - { 0, 0, 0, NULL }, // MULTITYPE_VTAIL4 - { 0, 0, 0, NULL }, // MULTITYPE_HEX6H - { 0, 0, 0, NULL }, // * MULTITYPE_PPM_TO_SERVO - { 0, 0, 0, NULL }, // MULTITYPE_DUALCOPTER - { 0, 0, 0, NULL }, // MULTITYPE_SINGLECOPTER - { 0, 0, 0, NULL }, // MULTITYPE_ATAIL4 - { 0, 2, 5, NULL }, // MULTITYPE_CUSTOM - { 0, SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, NULL }, // MULTITYPE_CUSTOM_PLANE - { 0, SERVO_RUDDER, SERVO_RUDDER, NULL }, // MULTITYPE_CUSTOM_TRI -}; - -static servoMixer_t *customServoMixers; -#endif - - static motorMixer_t *customMixers; void mixerUseConfigs( -#ifdef USE_SERVOS - servoParam_t *servoConfToUse, - gimbalConfig_t *gimbalConfigToUse, -#endif flight3DConfig_t *flight3DConfigToUse, - escAndServoConfig_t *escAndServoConfigToUse, + motorConfig_t *motorConfigToUse, mixerConfig_t *mixerConfigToUse, rxConfig_t *rxConfigToUse) { -#ifdef USE_SERVOS - servoConf = servoConfToUse; - gimbalConfig = gimbalConfigToUse; -#endif flight3DConfig = flight3DConfigToUse; - escAndServoConfig = escAndServoConfigToUse; + motorConfig = motorConfigToUse; mixerConfig = mixerConfigToUse; rxConfig = rxConfigToUse; } -#ifdef USE_SERVOS - -int16_t getFlaperonDirection(uint8_t servoPin) { - if (servoPin == SERVO_FLAPPERON_2) { - return -1; - } else { - return 1; - } -} - -int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex) -{ - uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel; - - if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) { - return rcData[channelToForwardFrom]; - } - - return servoConf[servoIndex].middle; -} - - -int servoDirection(int servoIndex, int inputSource) -{ - // determine the direction (reversed or not) from the direction bitfield of the servo - if (servoConf[servoIndex].reversedSources & (1 << inputSource)) - return -1; - else - return 1; -} -#endif - bool isMixerEnabled(mixerMode_e mixerMode) { #ifdef USE_QUAD_MIXER_ONLY @@ -402,82 +291,12 @@ bool isMixerEnabled(mixerMode_e mixerMode) #endif } -#ifdef USE_SERVOS -void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMotorMixers, servoMixer_t *initialCustomServoMixers) -{ - int i; - - currentMixerMode = mixerMode; - - // set flag that we're on something with wings - if (currentMixerMode == MIXER_FLYING_WING || - currentMixerMode == MIXER_AIRPLANE || - currentMixerMode == MIXER_CUSTOM_AIRPLANE - ) { - ENABLE_STATE(FIXED_WING); - } else { - DISABLE_STATE(FIXED_WING); - } - - if (currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { - ENABLE_STATE(FLAPERON_AVAILABLE); - } else { - DISABLE_STATE(FLAPERON_AVAILABLE); - } - - customMixers = initialCustomMotorMixers; - customServoMixers = initialCustomServoMixers; - - minServoIndex = servoMixers[currentMixerMode].minServoIndex; - maxServoIndex = servoMixers[currentMixerMode].maxServoIndex; - - // enable servos for mixes that require them. note, this shifts motor counts. - mixerUsesServos = mixers[currentMixerMode].useServo || feature(FEATURE_SERVO_TILT); - - // if we want camstab/trig, that also enables servos, even if mixer doesn't - servoOutputEnabled = mixerUsesServos || feature(FEATURE_CHANNEL_FORWARDING); - - // give all servos a default command - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo[i] = DEFAULT_SERVO_MIDDLE; - } - - /* - * If mixer has predefined servo mixes, load them - */ - if (mixerUsesServos) { - servoRuleCount = servoMixers[currentMixerMode].servoRuleCount; - if (servoMixers[currentMixerMode].rule) { - for (i = 0; i < servoRuleCount; i++) - currentServoMixer[i] = servoMixers[currentMixerMode].rule[i]; - } - } - - /* - * When we are dealing with CUSTOM mixers, load mixes defined with - * smix and update internal variables - */ - if (currentMixerMode == MIXER_CUSTOM_AIRPLANE || - currentMixerMode == MIXER_CUSTOM_TRI || - currentMixerMode == MIXER_CUSTOM) - { - loadCustomServoMixer(); - - // If there are servo rules after all, update variables - if (servoRuleCount > 0) { - servoOutputEnabled = 1; - mixerUsesServos = 1; - } - - } -} -#else +// mixerInit must be called before servosInit void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) { currentMixerMode = mixerMode; customMixers = initialCustomMixers; } -#endif #ifdef USE_SERVOS void mixerUsePWMIOConfiguration(void) @@ -531,53 +350,6 @@ void mixerUsePWMIOConfiguration(void) #ifndef USE_QUAD_MIXER_ONLY -#ifdef USE_SERVOS -void loadCustomServoMixer(void) -{ - int i; - - // reset settings - servoRuleCount = 0; - minServoIndex = 255; - maxServoIndex = 0; - memset(currentServoMixer, 0, sizeof(currentServoMixer)); - - // load custom mixer into currentServoMixer - for (i = 0; i < MAX_SERVO_RULES; i++) { - // check if done - if (customServoMixers[i].rate == 0) - break; - - if (customServoMixers[i].targetChannel < minServoIndex) { - minServoIndex = customServoMixers[i].targetChannel; - } - - if (customServoMixers[i].targetChannel > maxServoIndex) { - maxServoIndex = customServoMixers[i].targetChannel; - } - - currentServoMixer[i] = customServoMixers[i]; - servoRuleCount++; - } -} - -void servoMixerLoadMix(int index, servoMixer_t *customServoMixers) -{ - int i; - - // we're 1-based - index++; - // clear existing - for (i = 0; i < MAX_SERVO_RULES; i++) - customServoMixers[i].targetChannel = customServoMixers[i].inputSource = customServoMixers[i].rate = customServoMixers[i].box = 0; - - for (i = 0; i < servoMixers[index].servoRuleCount; i++) { - customServoMixers[i] = servoMixers[index].rule[i]; - } -} -#endif - - void mixerLoadMix(int index, motorMixer_t *customMixers) { int i; @@ -602,59 +374,8 @@ void mixerResetDisarmedMotors(void) int i; // set disarmed motor values for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) - motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand; -} - -#ifdef USE_SERVOS - -STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex) -{ - // start forwarding from this channel - uint8_t channelOffset = AUX1; - - int servoOffset; - for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) { - pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]); - } -} - -void writeServos(void) -{ - int servoIndex = 0; - - bool zeroServoValue = false; - - /* - * in case of tricopters, there might me a need to zero servo output when unarmed - */ - if ((currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI) && !ARMING_FLAG(ARMED) && !mixerConfig->tri_unarmed_servo) { - zeroServoValue = true; - } - - // Write mixer servo outputs - // mixerUsesServos might indicate SERVO_TILT, servoRuleCount indicate if mixer alone uses servos - if (mixerUsesServos && servoRuleCount) { - for (int i = minServoIndex; i <= maxServoIndex; i++) { - if (zeroServoValue) { - pwmWriteServo(servoIndex++, 0); - } else { - pwmWriteServo(servoIndex++, servo[i]); - } - } - } - - if (feature(FEATURE_SERVO_TILT)) { - pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_PITCH]); - pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_ROLL]); - } - - // forward AUX to remaining servo outputs (not constrained) - if (feature(FEATURE_CHANNEL_FORWARDING)) { - forwardAuxChannelsToServos(servoIndex); - servoIndex += MAX_AUX_CHANNEL_COUNT; - } + motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand; } -#endif void writeMotors(void) { @@ -662,11 +383,6 @@ void writeMotors(void) for (i = 0; i < motorCount; i++) pwmWriteMotor(i, motor[i]); - - - if (feature(FEATURE_ONESHOT125)) { - pwmCompleteOneshotMotorUpdate(motorCount); - } } void writeAllMotors(int16_t mc) @@ -681,12 +397,12 @@ void writeAllMotors(int16_t mc) void stopMotors(void) { - writeAllMotors(feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand); + writeAllMotors(feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand); delay(50); // give the timers and ESCs a chance to react. } -void StopPwmAllMotors() +void stopPwmAllMotors() { pwmShutdownPulsesForAllMotors(motorCount); } @@ -727,23 +443,23 @@ void mixTable(void) if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling throttleMax = flight3DConfig->deadband3d_low; - throttleMin = escAndServoConfig->minthrottle; + throttleMin = motorConfig->minthrottle; throttlePrevious = throttleCommand = rcCommand[THROTTLE]; } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling - throttleMax = escAndServoConfig->maxthrottle; + throttleMax = motorConfig->maxthrottle; throttleMin = flight3DConfig->deadband3d_high; throttlePrevious = throttleCommand = rcCommand[THROTTLE]; } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive throttleCommand = throttleMax = flight3DConfig->deadband3d_low; - throttleMin = escAndServoConfig->minthrottle; + throttleMin = motorConfig->minthrottle; } else { // Deadband handling from positive to negative - throttleMax = escAndServoConfig->maxthrottle; + throttleMax = motorConfig->maxthrottle; throttleCommand = throttleMin = flight3DConfig->deadband3d_high; } } else { throttleCommand = rcCommand[THROTTLE]; - throttleMin = escAndServoConfig->minthrottle; - throttleMax = escAndServoConfig->maxthrottle; + throttleMin = motorConfig->minthrottle; + throttleMax = motorConfig->maxthrottle; } throttleRange = throttleMax - throttleMin; @@ -775,21 +491,21 @@ void mixTable(void) motor[i] = rpyMix[i] + constrain(throttleCommand * currentMixer[i].throttle, throttleMin, throttleMax); if (isFailsafeActive) { - motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); + motor[i] = constrain(motor[i], motorConfig->mincommand, motorConfig->maxthrottle); } else if (feature(FEATURE_3D)) { if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) { - motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low); + motor[i] = constrain(motor[i], motorConfig->minthrottle, flight3DConfig->deadband3d_low); } else { - motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle); + motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, motorConfig->maxthrottle); } } else { - motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); + motor[i] = constrain(motor[i], motorConfig->minthrottle, motorConfig->maxthrottle); } // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D)) { if (((rcData[THROTTLE]) < rxConfig->mincheck)) { - motor[i] = escAndServoConfig->mincommand; + motor[i] = motorConfig->mincommand; } } } @@ -800,145 +516,3 @@ void mixTable(void) } } -#ifdef USE_SERVOS - -void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted) -{ - int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500] - static int16_t currentOutput[MAX_SERVO_RULES]; - int i; - - if (FLIGHT_MODE(PASSTHRU_MODE)) { - // Direct passthru from RX - input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL]; - input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH]; - input[INPUT_STABILIZED_YAW] = rcCommand[YAW]; - } else { - // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui - input[INPUT_STABILIZED_ROLL] = axisPID[ROLL]; - input[INPUT_STABILIZED_PITCH] = axisPID[PITCH]; - input[INPUT_STABILIZED_YAW] = axisPID[YAW]; - - // Reverse yaw servo when inverted in 3D mode - if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { - input[INPUT_STABILIZED_YAW] *= -1; - } - } - - input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500); - input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500); - - input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] - - // center the RC input value around the RC middle value - // by subtracting the RC middle value from the RC input value, we get: - // data - middle = input - // 2000 - 1500 = +500 - // 1500 - 1500 = 0 - // 1000 - 1500 = -500 - input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc; - input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc; - input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc; - input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc; - input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc; - input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc; - input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc; - input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc; - - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) - servo[i] = 0; - - // mix servos according to rules - for (i = 0; i < servoRuleCount; i++) { - // consider rule if no box assigned or box is active - if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) { - uint8_t target = currentServoMixer[i].targetChannel; - uint8_t from = currentServoMixer[i].inputSource; - uint16_t servo_width = servoConf[target].max - servoConf[target].min; - int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2; - int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2; - - if (currentServoMixer[i].speed == 0) { - currentOutput[i] = input[from]; - } else { - if (currentOutput[i] < input[from]) { - currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]); - } else if (currentOutput[i] > input[from]) { - currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]); - } - } - - /* - Flaperon fligh mode - */ - if (FLIGHT_MODE(FLAPERON) && (target == SERVO_FLAPPERON_1 || target == SERVO_FLAPPERON_2)) { - int8_t multiplier = 1; - - if (flaperon_throw_inverted == 1) { - multiplier = -1; - } - currentOutput[i] += flaperon_throw_offset * getFlaperonDirection(target) * multiplier; - } - - servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max); - } else { - currentOutput[i] = 0; - } - } - - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; - servo[i] += determineServoMiddleOrForwardFromChannel(i); - } -} - -void processServoTilt(void) { - // center at fixed position, or vary either pitch or roll by RC channel - servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); - servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); - - if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { - if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { - servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; - servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; - } else { - servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; - servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; - } - } -} - -bool isServoOutputEnabled(void) -{ - return servoOutputEnabled; -} - -bool isMixerUsingServos(void) { - return mixerUsesServos; -} - -void filterServos(void) -{ - int servoIdx; - - if (mixerConfig->servo_lowpass_enable) { - // Initialize servo lowpass filter (servos are calculated at looptime rate) - if (!servoFilterIsSet) { - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - biquadFilterInit(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, gyro.targetLooptime); - } - - servoFilterIsSet = true; - } - - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - // Apply servo lowpass filter and do sanity cheching - servo[servoIdx] = (int16_t) biquadFilterApply(&servoFitlerState[servoIdx], (float)servo[servoIdx]); - } - } - - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); - } -} -#endif diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index e2ee9181678..7fb3a6f5ace 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -90,11 +90,6 @@ typedef struct mixer_s { typedef struct mixerConfig_s { int8_t yaw_motor_direction; uint16_t yaw_jump_prevention_limit; // make limit configurable (original fixed value was 100) -#ifdef USE_SERVOS - uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed - int16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq - int8_t servo_lowpass_enable; // enable/disable lowpass filter -#endif } mixerConfig_t; typedef struct flight3DConfig_s { @@ -106,139 +101,22 @@ typedef struct flight3DConfig_s { #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF -#ifdef USE_SERVOS - -// These must be consecutive, see 'reversedSources' -enum { - INPUT_STABILIZED_ROLL = 0, - INPUT_STABILIZED_PITCH, - INPUT_STABILIZED_YAW, - INPUT_STABILIZED_THROTTLE, - INPUT_RC_ROLL, - INPUT_RC_PITCH, - INPUT_RC_YAW, - INPUT_RC_THROTTLE, - INPUT_RC_AUX1, - INPUT_RC_AUX2, - INPUT_RC_AUX3, - INPUT_RC_AUX4, - INPUT_GIMBAL_PITCH, - INPUT_GIMBAL_ROLL, - - INPUT_SOURCE_COUNT -} inputSource_e; - -// target servo channels -typedef enum { - SERVO_GIMBAL_PITCH = 0, - SERVO_GIMBAL_ROLL = 1, - SERVO_ELEVATOR = 2, - SERVO_FLAPPERON_1 = 3, - SERVO_FLAPPERON_2 = 4, - SERVO_RUDDER = 5, - SERVO_THROTTLE = 6, // for internal combustion (IC) planes - SERVO_FLAPS = 7, - - SERVO_BICOPTER_LEFT = 4, - SERVO_BICOPTER_RIGHT = 5, - - SERVO_DUALCOPTER_LEFT = 4, - SERVO_DUALCOPTER_RIGHT = 5, - - SERVO_SINGLECOPTER_1 = 3, - SERVO_SINGLECOPTER_2 = 4, - SERVO_SINGLECOPTER_3 = 5, - SERVO_SINGLECOPTER_4 = 6, - -} servoIndex_e; // FIXME rename to servoChannel_e - -#define SERVO_PLANE_INDEX_MIN SERVO_ELEVATOR -#define SERVO_PLANE_INDEX_MAX SERVO_FLAPS - -#define SERVO_DUALCOPTER_INDEX_MIN SERVO_DUALCOPTER_LEFT -#define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT - -#define SERVO_SINGLECOPTER_INDEX_MIN SERVO_SINGLECOPTER_1 -#define SERVO_SINGLECOPTER_INDEX_MAX SERVO_SINGLECOPTER_4 - -#define SERVO_FLAPPERONS_MIN SERVO_FLAPPERON_1 -#define SERVO_FLAPPERONS_MAX SERVO_FLAPPERON_2 - -#define FLAPERON_THROW_DEFAULT 250 -#define FLAPERON_THROW_MIN 100 -#define FLAPERON_THROW_MAX 400 - -typedef struct servoMixer_s { - uint8_t targetChannel; // servo that receives the output of the rule - uint8_t inputSource; // input channel for this rule - int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction - uint8_t speed; // reduces the speed of the rule, 0=unlimited speed - int8_t min; // lower bound of rule range [0;100]% of servo max-min - int8_t max; // lower bound of rule range [0;100]% of servo max-min - uint8_t box; // active rule if box is enabled, range [0;3], 0=no box, 1=BOXSERVO1, 2=BOXSERVO2, 3=BOXSERVO3 -} servoMixer_t; - -#define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS) -#define MAX_SERVO_SPEED UINT8_MAX -#define MAX_SERVO_BOXES 3 - -// Custom mixer configuration -typedef struct mixerRules_s { - uint8_t servoRuleCount; - uint8_t minServoIndex; - uint8_t maxServoIndex; - const servoMixer_t *rule; -} mixerRules_t; - -typedef struct servoParam_s { - int16_t min; // servo min - int16_t max; // servo max - int16_t middle; // servo middle - int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction - uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value. - uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value. - int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED - uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted -} __attribute__ ((__packed__)) servoParam_t; - -struct gimbalConfig_s; -struct escAndServoConfig_s; -struct rxConfig_s; - -extern int16_t servo[MAX_SUPPORTED_SERVOS]; -bool isServoOutputEnabled(void); -bool isMixerUsingServos(void); -void writeServos(void); -void filterServos(void); -#endif - extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern bool motorLimitReached; -struct escAndServoConfig_s; +struct motorConfig_s; struct rxConfig_s; void mixerUseConfigs( -#ifdef USE_SERVOS - servoParam_t *servoConfToUse, - struct gimbalConfig_s *gimbalConfigToUse, -#endif flight3DConfig_t *flight3DConfigToUse, - struct escAndServoConfig_s *escAndServoConfigToUse, + struct motorConfig_s *motorConfigToUse, mixerConfig_t *mixerConfigToUse, struct rxConfig_s *rxConfigToUse); void writeAllMotors(int16_t mc); void mixerLoadMix(int index, motorMixer_t *customMixers); -#ifdef USE_SERVOS -void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); -void loadCustomServoMixer(void); -int servoDirection(int servoIndex, int fromChannel); -void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMotorMixers, servoMixer_t *initialCustomServoMixers); -#else -void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers); -#endif +void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); void mixerUsePWMIOConfiguration(void); void mixerResetDisarmedMotors(void); void mixTable(void); @@ -246,6 +124,6 @@ void writeMotors(void); void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted); void processServoTilt(void); void stopMotors(void); -void StopPwmAllMotors(void); +void stopPwmAllMotors(void); bool isMixerEnabled(mixerMode_e mixerMode); diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index 7ad99fbaa87..f06e12ecdae 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -2363,9 +2363,9 @@ void navigationUseRxConfig(rxConfig_t * initialRxConfig) posControl.rxConfig = initialRxConfig; } -void navigationUseEscAndServoConfig(escAndServoConfig_t * initialEscAndServoConfig) +void navigationUsemotorConfig(motorConfig_t * initialmotorConfig) { - posControl.escAndServoConfig = initialEscAndServoConfig; + posControl.motorConfig = initialmotorConfig; } void navigationUsePIDs(pidProfile_t *initialPidProfile) @@ -2416,7 +2416,7 @@ void navigationInit(navConfig_t *initialnavConfig, rcControlsConfig_t *initialRcControlsConfig, rxConfig_t * initialRxConfig, flight3DConfig_t * initialFlight3DConfig, - escAndServoConfig_t * initialEscAndServoConfig) + motorConfig_t * initialmotorConfig) { /* Initial state */ posControl.navState = NAV_STATE_IDLE; @@ -2446,7 +2446,7 @@ void navigationInit(navConfig_t *initialnavConfig, navigationUsePIDs(initialPidProfile); navigationUseRcControlsConfig(initialRcControlsConfig); navigationUseRxConfig(initialRxConfig); - navigationUseEscAndServoConfig(initialEscAndServoConfig); + navigationUsemotorConfig(initialmotorConfig); navigationUseFlight3DConfig(initialFlight3DConfig); } diff --git a/src/main/flight/navigation_rewrite.h b/src/main/flight/navigation_rewrite.h index caf1acffb25..4c17dcb3d30 100755 --- a/src/main/flight/navigation_rewrite.h +++ b/src/main/flight/navigation_rewrite.h @@ -21,7 +21,7 @@ #include "common/filter.h" #include "fc/rc_controls.h" -#include "io/escservo.h" +#include "io/motors.h" #include "io/gps.h" #include "flight/pid.h" @@ -220,14 +220,14 @@ void navigationUsePIDs(pidProfile_t *pidProfile); void navigationUseConfig(navConfig_t *navConfigToUse); void navigationUseRcControlsConfig(rcControlsConfig_t *initialRcControlsConfig); void navigationUseRxConfig(rxConfig_t * initialRxConfig); -void navigationUseEscAndServoConfig(escAndServoConfig_t * initialEscAndServoConfig); +void navigationUsemotorConfig(motorConfig_t * initialmotorConfig); void navigationUseFlight3DConfig(flight3DConfig_t * initialFlight3DConfig); void navigationInit(navConfig_t *initialnavConfig, pidProfile_t *initialPidProfile, rcControlsConfig_t *initialRcControlsConfig, rxConfig_t * initialRxConfig, flight3DConfig_t * initialFlight3DConfig, - escAndServoConfig_t * initialEscAndServoConfig); + motorConfig_t * initialmotorConfig); /* Navigation system updates */ void updateWaypointsAndNavigationMode(void); diff --git a/src/main/flight/navigation_rewrite_fixedwing.c b/src/main/flight/navigation_rewrite_fixedwing.c index 8c0a5209209..50422e7ab21 100755 --- a/src/main/flight/navigation_rewrite_fixedwing.c +++ b/src/main/flight/navigation_rewrite_fixedwing.c @@ -439,7 +439,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) { uint16_t correctedThrottleValue = constrain(posControl.navConfig->fw_cruise_throttle + throttleCorrection, posControl.navConfig->fw_min_throttle, posControl.navConfig->fw_max_throttle); - rcCommand[THROTTLE] = constrain(correctedThrottleValue, posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle); + rcCommand[THROTTLE] = constrain(correctedThrottleValue, posControl.motorConfig->minthrottle, posControl.motorConfig->maxthrottle); } } diff --git a/src/main/flight/navigation_rewrite_multicopter.c b/src/main/flight/navigation_rewrite_multicopter.c index 6dee166eee7..03ded8e94d8 100755 --- a/src/main/flight/navigation_rewrite_multicopter.c +++ b/src/main/flight/navigation_rewrite_multicopter.c @@ -38,7 +38,7 @@ #include "sensors/acceleration.h" #include "sensors/boardalignment.h" -#include "io/escservo.h" +#include "io/motors.h" #include "fc/rc_controls.h" #include "fc/rc_curves.h" @@ -109,8 +109,8 @@ static void updateAltitudeVelocityController_MC(uint32_t deltaMicros) static void updateAltitudeThrottleController_MC(uint32_t deltaMicros) { // Calculate min and max throttle boundaries (to compensate for integral windup) - int16_t thrAdjustmentMin = (int16_t)posControl.escAndServoConfig->minthrottle - (int16_t)posControl.navConfig->mc_hover_throttle; - int16_t thrAdjustmentMax = (int16_t)posControl.escAndServoConfig->maxthrottle - (int16_t)posControl.navConfig->mc_hover_throttle; + int16_t thrAdjustmentMin = (int16_t)posControl.motorConfig->minthrottle - (int16_t)posControl.navConfig->mc_hover_throttle; + int16_t thrAdjustmentMax = (int16_t)posControl.motorConfig->maxthrottle - (int16_t)posControl.navConfig->mc_hover_throttle; posControl.rcAdjustment[THROTTLE] = navPidApply2(posControl.desiredState.vel.V.Z, posControl.actualState.vel.V.Z, US2S(deltaMicros), &posControl.pids.vel[Z], thrAdjustmentMin, thrAdjustmentMax, false); @@ -128,11 +128,11 @@ bool adjustMulticopterAltitudeFromRCInput(void) // Make sure we can satisfy max_manual_climb_rate in both up and down directions if (rcThrottleAdjustment > 0) { // Scaling from altHoldThrottleRCZero to maxthrottle - rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (posControl.escAndServoConfig->maxthrottle - altHoldThrottleRCZero); + rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (posControl.motorConfig->maxthrottle - altHoldThrottleRCZero); } else { // Scaling from minthrottle to altHoldThrottleRCZero - rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (altHoldThrottleRCZero - posControl.escAndServoConfig->minthrottle); + rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (altHoldThrottleRCZero - posControl.motorConfig->minthrottle); } updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_UPDATE_SURFACE_TARGET); @@ -168,8 +168,8 @@ void setupMulticopterAltitudeController(void) // Make sure we are able to satisfy the deadband altHoldThrottleRCZero = constrain(altHoldThrottleRCZero, - posControl.escAndServoConfig->minthrottle + posControl.rcControlsConfig->alt_hold_deadband + 10, - posControl.escAndServoConfig->maxthrottle - posControl.rcControlsConfig->alt_hold_deadband - 10); + posControl.motorConfig->minthrottle + posControl.rcControlsConfig->alt_hold_deadband + 10, + posControl.motorConfig->maxthrottle - posControl.rcControlsConfig->alt_hold_deadband - 10); /* Force AH controller to initialize althold integral for pending takeoff on reset */ if (throttleStatus == THROTTLE_LOW) { @@ -229,7 +229,7 @@ static void applyMulticopterAltitudeController(uint32_t currentTime) } // Update throttle controller - rcCommand[THROTTLE] = constrain((int16_t)posControl.navConfig->mc_hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle); + rcCommand[THROTTLE] = constrain((int16_t)posControl.navConfig->mc_hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.motorConfig->minthrottle, posControl.motorConfig->maxthrottle); // Save processed throttle for future use rcCommandAdjustedThrottle = rcCommand[THROTTLE]; @@ -586,7 +586,7 @@ static void applyMulticopterEmergencyLandingController(uint32_t currentTime) } // Update throttle controller - rcCommand[THROTTLE] = constrain((int16_t)posControl.navConfig->mc_hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle); + rcCommand[THROTTLE] = constrain((int16_t)posControl.navConfig->mc_hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.motorConfig->minthrottle, posControl.motorConfig->maxthrottle); } else { /* Sensors has gone haywire, attempt to land regardless */ @@ -596,7 +596,7 @@ static void applyMulticopterEmergencyLandingController(uint32_t currentTime) rcCommand[THROTTLE] = failsafeConfig->failsafe_throttle; } else { - rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle; + rcCommand[THROTTLE] = posControl.motorConfig->minthrottle; } } } diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h index 90e0c1ce459..62750323400 100755 --- a/src/main/flight/navigation_rewrite_private.h +++ b/src/main/flight/navigation_rewrite_private.h @@ -281,7 +281,7 @@ typedef struct { pidProfile_t * pidProfile; rxConfig_t * rxConfig; flight3DConfig_t * flight3DConfig; - escAndServoConfig_t * escAndServoConfig; + motorConfig_t * motorConfig; } navigationPosControl_t; extern navigationPosControl_t posControl; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9cbdc04fe6a..89281fb1d07 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -88,6 +88,7 @@ extern bool motorLimitReached; extern float dT; int16_t magHoldTargetHeading; +static pt1Filter_t magHoldRateFilter; // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied static float tpaFactor; @@ -333,6 +334,12 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p pidState->errorGyroIfLimit = ABS(pidState->errorGyroIf); } +#ifdef USE_SERVOS + if (STATE(FIXED_WING) && pidProfile->fixedWingItermThrowLimit != 0) { + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile->fixedWingItermThrowLimit, pidProfile->fixedWingItermThrowLimit); + } +#endif + axisPID[axis] = newOutputLimited; #ifdef BLACKBOX @@ -348,6 +355,12 @@ void updateMagHoldHeading(int16_t heading) magHoldTargetHeading = heading; } +void resetMagHoldHeading(int16_t heading) +{ + updateMagHoldHeading(heading); + pt1FilterReset(&magHoldRateFilter, 0.0f); +} + int16_t getMagHoldHeading() { return magHoldTargetHeading; } @@ -388,8 +401,6 @@ uint8_t getMagHoldState() */ float pidMagHold(const pidProfile_t *pidProfile) { - - static pt1Filter_t magHoldRateFilter; float magHoldRate; int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHoldTargetHeading; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 15c1c454f29..9c38e3202b7 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -27,6 +27,10 @@ #define MAG_HOLD_RATE_LIMIT_MAX 250 #define MAG_HOLD_RATE_LIMIT_DEFAULT 90 +#define FW_ITERM_THROW_LIMIT_DEFAULT 165 +#define FW_ITERM_THROW_LIMIT_MIN 0 +#define FW_ITERM_THROW_LIMIT_MAX 500 + #define AXIS_ACCEL_MIN_LIMIT 50 typedef enum { @@ -65,6 +69,10 @@ typedef struct pidProfile_s { int16_t max_angle_inclination[ANGLE_INDEX_COUNT]; // Max possible inclination (roll and pitch axis separately uint8_t mag_hold_rate_limit; //Maximum rotation rate MAG_HOLD mode can feed to yaw rate PID controller + +#ifdef USE_SERVOS + uint16_t fixedWingItermThrowLimit; +#endif } pidProfile_t; extern int16_t axisPID[]; @@ -87,4 +95,5 @@ enum { }; void updateMagHoldHeading(int16_t heading); +void resetMagHoldHeading(int16_t heading); int16_t getMagHoldHeading(); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c new file mode 100755 index 00000000000..df06f36a8cd --- /dev/null +++ b/src/main/flight/servos.c @@ -0,0 +1,467 @@ +/* + * 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 "platform.h" + +#ifdef USE_SERVOS + +#include "build/debug.h" +#include "build/build_config.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/system.h" +#include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/system.h" + +#include "rx/rx.h" + +#include "io/gimbal.h" +#include "io/servos.h" +#include "fc/rc_controls.h" +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/gyro.h" + +#include "flight/mixer.h" +#include "flight/servos.h" +#include "flight/failsafe.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation_rewrite.h" + +#include "fc/runtime_config.h" + +#include "config/config.h" +#include "config/config_profile.h" +#include "config/feature.h" + +extern mixerMode_e currentMixerMode; +extern const mixer_t mixers[]; +extern mixerConfig_t *mixerConfig; + +static rxConfig_t *rxConfig; + +servoMixerConfig_t *servoMixerConfig; + +static uint8_t servoRuleCount = 0; +static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; +static gimbalConfig_t *gimbalConfig; +int16_t servo[MAX_SUPPORTED_SERVOS]; +static int servoOutputEnabled; + +static uint8_t mixerUsesServos; +static uint8_t minServoIndex; +static uint8_t maxServoIndex; + +static servoParam_t *servoConf; +static biquadFilter_t servoFitlerState[MAX_SUPPORTED_SERVOS]; +static bool servoFilterIsSet; + +#define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t)) +// mixer rule format servo, input, rate, speed, min, max, box +static const servoMixer_t servoMixerAirplane[] = { + { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100 }, + { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, 100, 0, 0, 100 }, + { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100 }, + { SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100 }, + { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100 }, +}; + +static const servoMixer_t servoMixerFlyingWing[] = { + { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100 }, + { SERVO_FLAPPERON_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100 }, + { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, -100, 0, 0, 100 }, + { SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100 }, + { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100 }, +}; + +static const servoMixer_t servoMixerTri[] = { + { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100 }, +}; + +const mixerRules_t servoMixers[] = { + { 0, 0, 0, NULL }, // entry 0 + { COUNT_SERVO_RULES(servoMixerTri), SERVO_RUDDER, SERVO_RUDDER, servoMixerTri }, // MULTITYPE_TRI + { 0, 0, 0, NULL }, // MULTITYPE_QUADP + { 0, 0, 0, NULL }, // MULTITYPE_QUADX + { 0, 0, 0, NULL }, // MULTITYPE_BI + { 0, 0, 0, NULL }, // MULTITYPE_GIMBAL / MIXER_GIMBAL -> disabled + { 0, 0, 0, NULL }, // MULTITYPE_Y6 + { 0, 0, 0, NULL }, // MULTITYPE_HEX6 + { COUNT_SERVO_RULES(servoMixerFlyingWing), SERVO_FLAPPERONS_MIN, SERVO_FLAPPERONS_MAX, servoMixerFlyingWing },// * MULTITYPE_FLYING_WING + { 0, 0, 0, NULL }, // MULTITYPE_Y4 + { 0, 0, 0, NULL }, // MULTITYPE_HEX6X + { 0, 0, 0, NULL }, // MULTITYPE_OCTOX8 + { 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATP + { 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATX + { COUNT_SERVO_RULES(servoMixerAirplane), SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, servoMixerAirplane }, // * MULTITYPE_AIRPLANE + { 0, 0, 0, NULL }, // * MIXER_HELI_120_CCPM -> disabled, never fully implemented in CF + { 0, 0, 0, NULL }, // * MIXER_HELI_90_DEG -> disabled, never fully implemented in CF + { 0, 0, 0, NULL }, // MULTITYPE_VTAIL4 + { 0, 0, 0, NULL }, // MULTITYPE_HEX6H + { 0, 0, 0, NULL }, // * MULTITYPE_PPM_TO_SERVO + { 0, 0, 0, NULL }, // MULTITYPE_DUALCOPTER + { 0, 0, 0, NULL }, // MULTITYPE_SINGLECOPTER + { 0, 0, 0, NULL }, // MULTITYPE_ATAIL4 + { 0, 2, 5, NULL }, // MULTITYPE_CUSTOM + { 0, SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, NULL }, // MULTITYPE_CUSTOM_PLANE + { 0, SERVO_RUDDER, SERVO_RUDDER, NULL }, // MULTITYPE_CUSTOM_TRI +}; + +static servoMixer_t *customServoMixers; + +void servosUseConfigs(servoMixerConfig_t *servoMixerConfigToUse, servoParam_t *servoParamsToUse, gimbalConfig_t *gimbalConfigToUse, rxConfig_t *rxConfigToUse) +{ + servoMixerConfig = servoMixerConfigToUse; + servoConf = servoParamsToUse; + gimbalConfig = gimbalConfigToUse; + rxConfig = rxConfigToUse; +} + +int16_t getFlaperonDirection(uint8_t servoPin) { + if (servoPin == SERVO_FLAPPERON_2) { + return -1; + } else { + return 1; + } +} + +int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex) +{ + uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel; + + if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) { + return rcData[channelToForwardFrom]; + } + + return servoConf[servoIndex].middle; +} + + +int servoDirection(int servoIndex, int inputSource) +{ + // determine the direction (reversed or not) from the direction bitfield of the servo + if (servoConf[servoIndex].reversedSources & (1 << inputSource)) + return -1; + else + return 1; +} + +void servosInit(servoMixer_t *initialCustomServoMixers) +{ + int i; + + // set flag that we're on something with wings + if (currentMixerMode == MIXER_FLYING_WING || + currentMixerMode == MIXER_AIRPLANE || + currentMixerMode == MIXER_CUSTOM_AIRPLANE + ) { + ENABLE_STATE(FIXED_WING); + } else { + DISABLE_STATE(FIXED_WING); + } + + if (currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { + ENABLE_STATE(FLAPERON_AVAILABLE); + } else { + DISABLE_STATE(FLAPERON_AVAILABLE); + } + + customServoMixers = initialCustomServoMixers; + + minServoIndex = servoMixers[currentMixerMode].minServoIndex; + maxServoIndex = servoMixers[currentMixerMode].maxServoIndex; + + // enable servos for mixes that require them. note, this shifts motor counts. + mixerUsesServos = mixers[currentMixerMode].useServo || feature(FEATURE_SERVO_TILT); + + // if we want camstab/trig, that also enables servos, even if mixer doesn't + servoOutputEnabled = mixerUsesServos || feature(FEATURE_CHANNEL_FORWARDING); + + // give all servos a default command + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servo[i] = DEFAULT_SERVO_MIDDLE; + } + + /* + * If mixer has predefined servo mixes, load them + */ + if (mixerUsesServos) { + servoRuleCount = servoMixers[currentMixerMode].servoRuleCount; + if (servoMixers[currentMixerMode].rule) { + for (i = 0; i < servoRuleCount; i++) + currentServoMixer[i] = servoMixers[currentMixerMode].rule[i]; + } + } + + /* + * When we are dealing with CUSTOM mixers, load mixes defined with + * smix and update internal variables + */ + if (currentMixerMode == MIXER_CUSTOM_AIRPLANE || + currentMixerMode == MIXER_CUSTOM_TRI || + currentMixerMode == MIXER_CUSTOM) + { + loadCustomServoMixer(); + + // If there are servo rules after all, update variables + if (servoRuleCount > 0) { + servoOutputEnabled = 1; + mixerUsesServos = 1; + } + + } +} +void loadCustomServoMixer(void) +{ + int i; + + // reset settings + servoRuleCount = 0; + minServoIndex = 255; + maxServoIndex = 0; + memset(currentServoMixer, 0, sizeof(currentServoMixer)); + + // load custom mixer into currentServoMixer + for (i = 0; i < MAX_SERVO_RULES; i++) { + // check if done + if (customServoMixers[i].rate == 0) + break; + + if (customServoMixers[i].targetChannel < minServoIndex) { + minServoIndex = customServoMixers[i].targetChannel; + } + + if (customServoMixers[i].targetChannel > maxServoIndex) { + maxServoIndex = customServoMixers[i].targetChannel; + } + + currentServoMixer[i] = customServoMixers[i]; + servoRuleCount++; + } +} + +void servoMixerLoadMix(int index, servoMixer_t *customServoMixers) +{ + int i; + + // we're 1-based + index++; + // clear existing + for (i = 0; i < MAX_SERVO_RULES; i++) + customServoMixers[i].targetChannel = customServoMixers[i].inputSource = customServoMixers[i].rate = 0; + + for (i = 0; i < servoMixers[index].servoRuleCount; i++) { + customServoMixers[i] = servoMixers[index].rule[i]; + } +} + +STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex) +{ + // start forwarding from this channel + uint8_t channelOffset = AUX1; + + int servoOffset; + for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) { + pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]); + } +} + +void writeServos(void) +{ + int servoIndex = 0; + + bool zeroServoValue = false; + + /* + * in case of tricopters, there might me a need to zero servo output when unarmed + */ + if ((currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI) && !ARMING_FLAG(ARMED) && !servoMixerConfig->tri_unarmed_servo) { + zeroServoValue = true; + } + + // Write mixer servo outputs + // mixerUsesServos might indicate SERVO_TILT, servoRuleCount indicate if mixer alone uses servos + if (mixerUsesServos && servoRuleCount) { + for (int i = minServoIndex; i <= maxServoIndex; i++) { + if (zeroServoValue) { + pwmWriteServo(servoIndex++, 0); + } else { + pwmWriteServo(servoIndex++, servo[i]); + } + } + } + + if (feature(FEATURE_SERVO_TILT)) { + pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_PITCH]); + pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_ROLL]); + } + + // forward AUX to remaining servo outputs (not constrained) + if (feature(FEATURE_CHANNEL_FORWARDING)) { + forwardAuxChannelsToServos(servoIndex); + servoIndex += MAX_AUX_CHANNEL_COUNT; + } +} + +void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted) +{ + int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500] + static int16_t currentOutput[MAX_SERVO_RULES]; + int i; + + if (FLIGHT_MODE(PASSTHRU_MODE)) { + // Direct passthru from RX + input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL]; + input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH]; + input[INPUT_STABILIZED_YAW] = rcCommand[YAW]; + } else { + // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui + input[INPUT_STABILIZED_ROLL] = axisPID[ROLL]; + input[INPUT_STABILIZED_PITCH] = axisPID[PITCH]; + input[INPUT_STABILIZED_YAW] = axisPID[YAW]; + + // Reverse yaw servo when inverted in 3D mode + if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { + input[INPUT_STABILIZED_YAW] *= -1; + } + } + + input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500); + input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500); + + input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] + + // center the RC input value around the RC middle value + // by subtracting the RC middle value from the RC input value, we get: + // data - middle = input + // 2000 - 1500 = +500 + // 1500 - 1500 = 0 + // 1000 - 1500 = -500 + input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc; + input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc; + input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc; + input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc; + input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc; + input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc; + input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc; + input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc; + + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) + servo[i] = 0; + + // mix servos according to rules + for (i = 0; i < servoRuleCount; i++) { + uint8_t target = currentServoMixer[i].targetChannel; + uint8_t from = currentServoMixer[i].inputSource; + uint16_t servo_width = servoConf[target].max - servoConf[target].min; + int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2; + int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2; + + if (currentServoMixer[i].speed == 0) { + currentOutput[i] = input[from]; + } else { + if (currentOutput[i] < input[from]) { + currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]); + } else if (currentOutput[i] > input[from]) { + currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]); + } + } + + /* + Flaperon fligh mode + */ + if (FLIGHT_MODE(FLAPERON) && (target == SERVO_FLAPPERON_1 || target == SERVO_FLAPPERON_2)) { + int8_t multiplier = 1; + + if (flaperon_throw_inverted == 1) { + multiplier = -1; + } + currentOutput[i] += flaperon_throw_offset * getFlaperonDirection(target) * multiplier; + } + + servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max); + } + + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; + servo[i] += determineServoMiddleOrForwardFromChannel(i); + } +} + +void processServoTilt(void) { + // center at fixed position, or vary either pitch or roll by RC channel + servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); + servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); + + if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { + if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { + servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; + servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; + } else { + servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; + servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; + } + } +} + +bool isServoOutputEnabled(void) +{ + return servoOutputEnabled; +} + +bool isMixerUsingServos(void) { + return mixerUsesServos; +} + +void filterServos(void) +{ + int servoIdx; + + if (servoMixerConfig->servo_lowpass_enable) { + // Initialize servo lowpass filter (servos are calculated at looptime rate) + if (!servoFilterIsSet) { + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + biquadFilterInitLPF(&servoFitlerState[servoIdx], servoMixerConfig->servo_lowpass_freq, gyro.targetLooptime); + } + + servoFilterIsSet = true; + } + + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + // Apply servo lowpass filter and do sanity cheching + servo[servoIdx] = (int16_t) biquadFilterApply(&servoFitlerState[servoIdx], (float)servo[servoIdx]); + } + } + + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); + } +} + +#endif diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h new file mode 100644 index 00000000000..cbb1aa23a5f --- /dev/null +++ b/src/main/flight/servos.h @@ -0,0 +1,130 @@ +/* + * 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 + +// These must be consecutive, see 'reversedSources' +enum { + INPUT_STABILIZED_ROLL = 0, + INPUT_STABILIZED_PITCH, + INPUT_STABILIZED_YAW, + INPUT_STABILIZED_THROTTLE, + INPUT_RC_ROLL, + INPUT_RC_PITCH, + INPUT_RC_YAW, + INPUT_RC_THROTTLE, + INPUT_RC_AUX1, + INPUT_RC_AUX2, + INPUT_RC_AUX3, + INPUT_RC_AUX4, + INPUT_GIMBAL_PITCH, + INPUT_GIMBAL_ROLL, + + INPUT_SOURCE_COUNT +} inputSource_e; + +// target servo channels +typedef enum { + SERVO_GIMBAL_PITCH = 0, + SERVO_GIMBAL_ROLL = 1, + SERVO_ELEVATOR = 2, + SERVO_FLAPPERON_1 = 3, + SERVO_FLAPPERON_2 = 4, + SERVO_RUDDER = 5, + SERVO_THROTTLE = 6, // for internal combustion (IC) planes + SERVO_FLAPS = 7, + + SERVO_BICOPTER_LEFT = 4, + SERVO_BICOPTER_RIGHT = 5, + + SERVO_DUALCOPTER_LEFT = 4, + SERVO_DUALCOPTER_RIGHT = 5, + + SERVO_SINGLECOPTER_1 = 3, + SERVO_SINGLECOPTER_2 = 4, + SERVO_SINGLECOPTER_3 = 5, + SERVO_SINGLECOPTER_4 = 6, + +} servoIndex_e; // FIXME rename to servoChannel_e + +#define SERVO_PLANE_INDEX_MIN SERVO_ELEVATOR +#define SERVO_PLANE_INDEX_MAX SERVO_FLAPS + +#define SERVO_DUALCOPTER_INDEX_MIN SERVO_DUALCOPTER_LEFT +#define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT + +#define SERVO_SINGLECOPTER_INDEX_MIN SERVO_SINGLECOPTER_1 +#define SERVO_SINGLECOPTER_INDEX_MAX SERVO_SINGLECOPTER_4 + +#define SERVO_FLAPPERONS_MIN SERVO_FLAPPERON_1 +#define SERVO_FLAPPERONS_MAX SERVO_FLAPPERON_2 + +#define FLAPERON_THROW_DEFAULT 250 +#define FLAPERON_THROW_MIN 100 +#define FLAPERON_THROW_MAX 400 + +typedef struct servoMixerConfig_s { + uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed + int16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq + int8_t servo_lowpass_enable; // enable/disable lowpass filter +} servoMixerConfig_t; + +typedef struct servoMixer_s { + uint8_t targetChannel; // servo that receives the output of the rule + uint8_t inputSource; // input channel for this rule + int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction + uint8_t speed; // reduces the speed of the rule, 0=unlimited speed + int8_t min; // lower bound of rule range [0;100]% of servo max-min + int8_t max; // lower bound of rule range [0;100]% of servo max-min +} servoMixer_t; + +#define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS) +#define MAX_SERVO_SPEED UINT8_MAX +#define MAX_SERVO_BOXES 3 + +// Custom mixer configuration +typedef struct mixerRules_s { + uint8_t servoRuleCount; + uint8_t minServoIndex; + uint8_t maxServoIndex; + const servoMixer_t *rule; +} mixerRules_t; + +typedef struct servoParam_s { + int16_t min; // servo min + int16_t max; // servo max + int16_t middle; // servo middle + int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction + uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value. + uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value. + int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED + uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted +} __attribute__ ((__packed__)) servoParam_t; + +struct gimbalConfig_s; + +extern int16_t servo[MAX_SUPPORTED_SERVOS]; + +bool isServoOutputEnabled(void); +bool isMixerUsingServos(void); +void writeServos(void); +void filterServos(void); +void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); +void loadCustomServoMixer(void); +int servoDirection(int servoIndex, int fromChannel); +void servosUseConfigs(servoMixerConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, gimbalConfig_t *gimbalConfigToUse, struct rxConfig_s *rxConfigToUse); +void servosInit(servoMixer_t *customServoMixers); diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 639a92254c7..0418364e9b9 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -41,6 +41,7 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/feature.h" #include "io/beeper.h" diff --git a/src/main/io/display.c b/src/main/io/display.c index 9f7aed502cc..303d631f193 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -64,6 +64,7 @@ #include "config/config.h" +#include "config/feature.h" #include "display.h" diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 86bacf27de3..886e369fcd2 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -52,6 +52,8 @@ #include "flight/navigation_rewrite.h" #include "config/config.h" +#include "config/feature.h" + #include "fc/runtime_config.h" // GPS timeout for wrong baud rate/disconnection/etc in milliseconds (default 2000 ms) @@ -207,7 +209,7 @@ void gpsInit(serialConfig_t *initialSerialConfig, gpsConfig_t *initialGpsConfig) portMode_t mode = gpsProviders[gpsState.gpsConfig->provider].portMode; // no callback - buffer will be consumed in gpsThread() - gpsState.gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, gpsToSerialBaudRate[gpsState.baudrateIndex], mode, SERIAL_NOT_INVERTED); + gpsState.gpsPort = openSerialPort(gpsPortConfig->identifier, FUNCTION_GPS, NULL, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]], mode, SERIAL_NOT_INVERTED); if (!gpsState.gpsPort) { featureClear(FEATURE_GPS); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 8d9249fef89..e5a95119ddc 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -55,7 +55,8 @@ #include "io/ledstrip.h" #include "io/beeper.h" -#include "io/escservo.h" +#include "io/motors.h" +#include "io/servos.h" #include "io/gimbal.h" #include "io/serial.h" #include "io/gps.h" @@ -64,6 +65,7 @@ #include "flight/failsafe.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/navigation_rewrite.h" @@ -75,6 +77,7 @@ #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" /* PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0); diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 30450cf05d9..e281822102b 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -134,7 +134,8 @@ typedef struct ledCounts_s { ledConfig_t *ledConfigs; -hsvColor_t *colors; +struct hsvColor_s; +struct hsvColor_s *colors; modeColorIndexes_t *modeColors; specialColorIndexes_t specialColors; @@ -163,7 +164,7 @@ bool parseLedStripConfig(int ledIndex, const char *config); void generateLedConfig(ledConfig_t *ledConfig, char *ledConfigBuffer, size_t bufferSize); void reevaluateLedConfig(void); -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); +void ledStripInit(ledConfig_t *ledConfigsToUse, struct hsvColor_s *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); void ledStripEnable(void); void updateLedStrip(void); @@ -173,7 +174,7 @@ extern uint16_t rssi; // FIXME dependency on mw.c void applyDefaultLedStripConfig(ledConfig_t *ledConfig); -void applyDefaultColors(hsvColor_t *colors); +void applyDefaultColors(struct hsvColor_s *colors); void applyDefaultModeColors(modeColorIndexes_t *modeColors); void applyDefaultSpecialColors(specialColorIndexes_t *specialColors); diff --git a/src/main/io/escservo.h b/src/main/io/motors.h similarity index 86% rename from src/main/io/escservo.h rename to src/main/io/motors.h index 66cd7db59ea..feb2c76bd5c 100644 --- a/src/main/io/escservo.h +++ b/src/main/io/motors.h @@ -17,11 +17,11 @@ #pragma once -typedef struct escAndServoConfig_s { - - // PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms) +typedef struct motorConfig_s { + // PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms) uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs - uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500. -} escAndServoConfig_t; + uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) + uint8_t motorPwmProtocol; +} motorConfig_t; diff --git a/src/main/io/pwmdriver_i2c.c b/src/main/io/pwmdriver_i2c.c new file mode 100644 index 00000000000..4d4db9200b4 --- /dev/null +++ b/src/main/io/pwmdriver_i2c.c @@ -0,0 +1,57 @@ +#include +#include +#include "drivers/io_pca9685.h" + +#include "config/config.h" +#include "fc/runtime_config.h" +#include "config/feature.h" + +#define PWM_DRIVER_IMPLEMENTATION_COUNT 1 +#define PWM_DRIVER_MAX_CYCLE 4 + +static bool driverEnabled = false; +static uint8_t driverImplementationIndex = 0; + +typedef struct { + bool (*initFunction)(void); + void (*writeFunction)(uint8_t servoIndex, uint16_t off); + void (*setFrequencyFunction)(float freq); + void (*syncFunction)(uint8_t cycleIndex); +} pwmDriverDriver_t; + +pwmDriverDriver_t pwmDrivers[PWM_DRIVER_IMPLEMENTATION_COUNT] = { + [0] = { + .initFunction = pca9685Initialize, + .writeFunction = pca9685setServoPulse, + .setFrequencyFunction = pca9685setPWMFreq, + .syncFunction = pca9685sync + } +}; + +bool isPwmDriverEnabled() { + return driverEnabled; +} + +void pwmDriverSetPulse(uint8_t servoIndex, uint16_t length) { + (pwmDrivers[driverImplementationIndex].writeFunction)(servoIndex, length); +} + +void pwmDriverInitialize(void) { + driverEnabled = (pwmDrivers[driverImplementationIndex].initFunction)(); + + if (!driverEnabled) { + featureClear(FEATURE_PWM_SERVO_DRIVER); + } + +} + +void pwmDriverSync(void) { + static uint8_t cycle = 0; + + (pwmDrivers[driverImplementationIndex].syncFunction)(cycle); + + cycle++; + if (cycle == PWM_DRIVER_MAX_CYCLE) { + cycle = 0; + } +} diff --git a/src/main/io/pwmdriver_i2c.h b/src/main/io/pwmdriver_i2c.h new file mode 100644 index 00000000000..1170c87109c --- /dev/null +++ b/src/main/io/pwmdriver_i2c.h @@ -0,0 +1,4 @@ + +void pwmDriverInitialize(void); +void pwmDriverSync(void); +void pwmDriverSetPulse(uint8_t servoIndex, uint16_t length); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 70db726d08b..fb11d37e595 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -44,8 +44,8 @@ #include "io/serial.h" #include "serial_cli.h" -#include "serial_msp.h" +#include "msp/msp_serial.h" #ifdef TELEMETRY #include "telemetry/telemetry.h" @@ -192,7 +192,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction return NULL; } -#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK) +#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) #define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK) bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) @@ -262,7 +262,7 @@ bool doesConfigurationUsePort(serialPortIdentifier_e identifier) serialPort_t *openSerialPort( serialPortIdentifier_e identifier, serialPortFunction_e function, - serialReceiveCallbackPtr callback, + serialReceiveCallbackPtr rxCallback, uint32_t baudRate, portMode_t mode, portOptions_t options) @@ -290,43 +290,43 @@ serialPort_t *openSerialPort( #endif #ifdef USE_UART1 case SERIAL_PORT_USART1: - serialPort = uartOpen(USART1, callback, baudRate, mode, options); + serialPort = uartOpen(USART1, rxCallback, baudRate, mode, options); break; #endif #ifdef USE_UART2 case SERIAL_PORT_USART2: - serialPort = uartOpen(USART2, callback, baudRate, mode, options); + serialPort = uartOpen(USART2, rxCallback, baudRate, mode, options); break; #endif #ifdef USE_UART3 case SERIAL_PORT_USART3: - serialPort = uartOpen(USART3, callback, baudRate, mode, options); + serialPort = uartOpen(USART3, rxCallback, baudRate, mode, options); break; #endif #ifdef USE_UART4 case SERIAL_PORT_USART4: - serialPort = uartOpen(UART4, callback, baudRate, mode, options); + serialPort = uartOpen(UART4, rxCallback, baudRate, mode, options); break; #endif #ifdef USE_UART5 case SERIAL_PORT_USART5: - serialPort = uartOpen(UART5, callback, baudRate, mode, options); + serialPort = uartOpen(UART5, rxCallback, baudRate, mode, options); break; #endif #ifdef USE_UART6 case SERIAL_PORT_USART6: - serialPort = uartOpen(USART6, callback, baudRate, mode, options); + serialPort = uartOpen(USART6, rxCallback, baudRate, mode, options); break; #endif #ifdef USE_SOFTSERIAL1 case SERIAL_PORT_SOFTSERIAL1: - serialPort = openSoftSerial(SOFTSERIAL1, callback, baudRate, options); + serialPort = openSoftSerial(SOFTSERIAL1, rxCallback, baudRate, options); serialSetMode(serialPort, mode); break; #endif #ifdef USE_SOFTSERIAL2 case SERIAL_PORT_SOFTSERIAL2: - serialPort = openSoftSerial(SOFTSERIAL2, callback, baudRate, options); + serialPort = openSoftSerial(SOFTSERIAL2, rxCallback, baudRate, options); serialSetMode(serialPort, mode); break; #endif @@ -346,7 +346,8 @@ serialPort_t *openSerialPort( return serialPort; } -void closeSerialPort(serialPort_t *serialPort) { +void closeSerialPort(serialPort_t *serialPort) +{ serialPortUsage_t *serialPortUsage = findSerialPortUsageByPort(serialPort); if (!serialPortUsage) { // already closed @@ -355,7 +356,7 @@ void closeSerialPort(serialPort_t *serialPort) { // TODO wait until data has been transmitted. - serialPort->callback = NULL; + serialPort->rxCallback = NULL; serialPortUsage->function = FUNCTION_NONE; serialPortUsage->serialPort = NULL; @@ -420,18 +421,6 @@ bool serialIsPortAvailable(serialPortIdentifier_e identifier) return false; } -void handleSerial(void) -{ -#ifdef USE_CLI - // in cli mode, all serial stuff goes to here. enter cli mode by sending # - if (cliMode) { - cliProcess(); - return; - } -#endif - - mspProcess(); -} void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort) { @@ -440,9 +429,7 @@ void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort) }; } -void cliEnter(serialPort_t *serialPort); - -void evaluateOtherData(serialPort_t *serialPort, uint8_t receivedChar) +void serialEvaluateNonMspData(serialPort_t *serialPort, uint8_t receivedChar) { #ifndef USE_CLI UNUSED(serialPort); diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 46e36181159..a69617fcd98 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -34,6 +34,7 @@ typedef enum { FUNCTION_RX_SERIAL = (1 << 6), // 64 FUNCTION_BLACKBOX = (1 << 7), // 128 FUNCTION_TELEMETRY_MAVLINK = (1 << 8), // 256 + FUNCTION_TELEMETRY_IBUS = (1 << 9) // 512 } serialPortFunction_e; typedef enum { @@ -136,9 +137,5 @@ baudRate_e lookupBaudRateIndex(uint32_t baudRate); // // msp/cli/bootloader // -void evaluateOtherData(serialPort_t *serialPort, uint8_t receivedChar); -void handleSerial(void); - -void evaluateOtherData(serialPort_t *serialPort, uint8_t receivedChar); -void handleSerial(void); +void serialEvaluateNonMspData(serialPort_t *serialPort, uint8_t receivedChar); void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer *leftC, serialConsumer *rightC); diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 95bde6a5387..5d3978c8104 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -37,7 +37,6 @@ #include "drivers/system.h" #include "flight/mixer.h" #include "io/beeper.h" -#include "io/serial_msp.h" #include "io/serial_4way.h" #include "io/serial_4way_impl.h" diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 4e84746f219..f22ab042f34 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -33,7 +33,6 @@ #include "drivers/pwm_mapping.h" #include "drivers/gpio.h" #include "io/serial.h" -#include "io/serial_msp.h" #include "io/serial_4way.h" #include "io/serial_4way_impl.h" #include "io/serial_4way_avrootloader.h" diff --git a/src/main/io/serial_4way_stk500v2.c b/src/main/io/serial_4way_stk500v2.c index 82cce50441f..a53edef7225 100644 --- a/src/main/io/serial_4way_stk500v2.c +++ b/src/main/io/serial_4way_stk500v2.c @@ -32,7 +32,6 @@ #include "drivers/system.h" #include "config/config.h" #include "io/serial.h" -#include "io/serial_msp.h" #include "io/serial_4way.h" #include "io/serial_4way_impl.h" #include "io/serial_4way_stk500v2.h" diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9c7352b9401..3d04df54830 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -52,10 +52,12 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/sdcard.h" +#include "drivers/stack_check.h" #include "drivers/buf_writer.h" -#include "io/escservo.h" +#include "io/motors.h" +#include "io/servos.h" #include "io/gps.h" #include "io/gimbal.h" #include "fc/rc_controls.h" @@ -80,6 +82,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/navigation_rewrite.h" #include "flight/failsafe.h" @@ -89,8 +92,10 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" #include "common/printf.h" @@ -150,7 +155,7 @@ static void cliVersion(char *cmdline); static void cliRxRange(char *cmdline); static void cliPFlags(char *cmdline); -#ifndef SKIP_TASK_STATISTICS +#if !defined(SKIP_TASK_STATISTICS) && !defined(SKIP_CLI_RESOURCES) static void cliResource(char *cmdline); #endif #ifdef GPS @@ -205,12 +210,12 @@ static const char * const mixerNames[] = { // sync this with features_e static const char * const featureNames[] = { - "RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP", + "RX_PPM", "VBAT", "UNUSED_1", "RX_SERIAL", "MOTOR_STOP", "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", - "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", + "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "UNUSED_2", "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", - "SUPEREXPO", "VTX", "RX_NRF24", "SOFTSPI", NULL + "SUPEREXPO", "VTX", "RX_SPI", "SOFTSPI", "PWM_SERVO_DRIVER", NULL }; // sync this with rxFailsafeChannelMode_e @@ -320,7 +325,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), -#ifndef SKIP_TASK_STATISTICS +#if !defined(SKIP_TASK_STATISTICS) && !defined(SKIP_CLI_RESOURCES) CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), #endif CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), @@ -405,10 +410,6 @@ static const char * const lookupTableBlackboxDevice[] = { }; #endif -static const char * const lookupTablePidController[] = { - "UNUSED", "MWREWRITE", "LUX" -}; - #ifdef SERIAL_RX static const char * const lookupTableSerialRX[] = { "SPEK1024", @@ -422,9 +423,9 @@ static const char * const lookupTableSerialRX[] = { }; #endif -#ifdef USE_RX_NRF24 -// sync with nrf24_protocol_t -static const char * const lookupTableNRF24RX[] = { +#ifdef USE_RX_SPI +// sync with rx_spi_protocol_e +static const char * const lookupTableRxSpi[] = { "V202_250K", "V202_1M", "SYMA_X", @@ -463,6 +464,10 @@ static const char * const lookupTableAuxOperator[] = { "OR", "AND" }; +static const char * const lookupTablePwmProtocol[] = { + "STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED" +}; + typedef struct lookupTableEntry_s { const char * const *values; const uint8_t valueCount; @@ -484,12 +489,11 @@ typedef enum { #ifdef USE_SERVOS TABLE_GIMBAL_MODE, #endif - TABLE_PID_CONTROLLER, #ifdef SERIAL_RX TABLE_SERIAL_RX, #endif -#ifdef USE_RX_NRF24 - TABLE_NRF24_RX, +#ifdef USE_RX_SPI + TABLE_RX_SPI, #endif TABLE_GYRO_LPF, TABLE_FAILSAFE_PROCEDURE, @@ -498,6 +502,7 @@ typedef enum { TABLE_NAV_RTH_ALT_MODE, #endif TABLE_AUX_OPERATOR, + TABLE_MOTOR_PWM_PROTOCOL, } lookupTableIndex_e; static const lookupTableEntry_t lookupTables[] = { @@ -516,12 +521,11 @@ static const lookupTableEntry_t lookupTables[] = { #ifdef USE_SERVOS { lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) }, #endif - { lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) }, #ifdef SERIAL_RX { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }, #endif -#ifdef USE_RX_NRF24 - { lookupTableNRF24RX, sizeof(lookupTableNRF24RX) / sizeof(char *) }, +#ifdef USE_RX_SPI + { lookupTableRxSpi, sizeof(lookupTableRxSpi) / sizeof(char *) }, #endif { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) }, { lookupTableFailsafeProcedure, sizeof(lookupTableFailsafeProcedure) / sizeof(char *) }, @@ -530,6 +534,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableNavRthAltMode, sizeof(lookupTableNavRthAltMode) / sizeof(char *) }, #endif { lookupTableAuxOperator, sizeof(lookupTableAuxOperator) / sizeof(char *) }, + { lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) }, }; #define VALUE_TYPE_OFFSET 0 @@ -596,23 +601,19 @@ const clivalue_t valueTable[] = { { "rc_smoothing", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_OFF_ON }, 0 }, { "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON }, 0 }, - { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, - { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, - { "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, -#ifdef USE_SERVOS - { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, -#endif + { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, + { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, + { "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, // FIXME upper limit should match code in the mixer, 1500 currently { "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_high, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, // FIXME lower limit should match code in the mixer, 1500 currently, { "3d_neutral", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, - { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motor_pwm_rate, .config.minmax = { 50, 32000 }, 0 }, -#ifdef USE_SERVOS - { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servo_pwm_rate, .config.minmax = { 50, 498 }, 0 }, -#endif + { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.motorPwmRate, .config.minmax = { 50, 32000 }, 0 }, + { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.motorConfig.motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } }, + { "fixed_wing_auto_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.fixed_wing_auto_arm, .config.lookup = { TABLE_OFF_ON }, 0 }, { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON }, 0 }, { "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.auto_disarm_delay, .config.minmax = { 0, 60 }, 0 }, { "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.small_angle, .config.minmax = { 0, 180 }, 0 }, @@ -704,10 +705,10 @@ const clivalue_t valueTable[] = { #ifdef SERIAL_RX { "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.serialrx_provider, .config.lookup = { TABLE_SERIAL_RX }, 0 }, #endif -#ifdef USE_RX_NRF24 - { "nrf24rx_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.nrf24rx_protocol, .config.lookup = { TABLE_NRF24_RX }, 0 }, - { "nrf24rx_id", VAR_UINT32 | MASTER_VALUE, &masterConfig.rxConfig.nrf24rx_id, .config.minmax = { 0, 0 }, 0 }, - { "nrf24rx_rf_channel_count", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.nrf24rx_rf_channel_count, .config.minmax = { 0, 8 }, 0 }, +#ifdef USE_RX_SPI + { "rx_spi_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rx_spi_protocol, .config.lookup = { TABLE_RX_SPI }, 0 }, + { "rx_spi_id", VAR_UINT32 | MASTER_VALUE, &masterConfig.rxConfig.rx_spi_id, .config.minmax = { 0, 0 }, 0 }, + { "rx_spi_rf_channel_count", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rx_spi_rf_channel_count, .config.minmax = { 0, 8 }, 0 }, #endif #ifdef SPEKTRUM_BIND { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, 0 }, @@ -762,9 +763,13 @@ const clivalue_t valueTable[] = { #ifdef USE_SERVOS { "flaperon_throw_offset", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].flaperon_throw_offset, .config.minmax = { FLAPERON_THROW_MIN, FLAPERON_THROW_MAX}, 0 }, { "flaperon_throw_inverted", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].flaperon_throw_inverted, .config.lookup = { TABLE_OFF_ON }, 0 }, - { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON }, 0 }, - { "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400}, 0 }, - { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON }, 0 }, + { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoMixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON }, 0 }, + { "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.servoMixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400}, 0 }, + { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoMixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON }, 0 }, + { "gimbal_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE }, 0 }, + { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, + { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoPwmRate, .config.minmax = { 50, 498 }, 0 }, + { "fw_iterm_throw_limit", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.fixedWingItermThrowLimit, .config.minmax = { FW_ITERM_THROW_LIMIT_MIN, FW_ITERM_THROW_LIMIT_MAX}, 0 }, #endif { "mode_range_logic_operator", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].modeActivationOperator, .config.lookup = { TABLE_AUX_OPERATOR }, 0 }, @@ -795,10 +800,6 @@ const clivalue_t valueTable[] = { { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_min_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, 0 }, { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, 0 }, -#ifdef USE_SERVOS - { "gimbal_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE }, 0 }, -#endif - { "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, .config.minmax = { 0, ACC_MAX }, 0 }, #ifdef BARO @@ -1628,7 +1629,7 @@ static void cliServoMix(char *cmdline) masterConfig.customServoMixer[i].speed, masterConfig.customServoMixer[i].min, masterConfig.customServoMixer[i].max, - masterConfig.customServoMixer[i].box + 0 ); } cliPrintf("\r\n"); @@ -1727,7 +1728,6 @@ static void cliServoMix(char *cmdline) masterConfig.customServoMixer[i].speed = args[SPEED]; masterConfig.customServoMixer[i].min = args[MIN]; masterConfig.customServoMixer[i].max = args[MAX]; - masterConfig.customServoMixer[i].box = args[BOX]; cliServoMix(""); } else { cliShowParseError(); @@ -1998,7 +1998,7 @@ static void cliDump(char *cmdline) masterConfig.customServoMixer[i].speed, masterConfig.customServoMixer[i].min, masterConfig.customServoMixer[i].max, - masterConfig.customServoMixer[i].box + 0 ); } #endif // USE_SERVOS @@ -2499,8 +2499,11 @@ static void cliReboot(void) { cliPrint("\r\nRebooting"); bufWriterFlush(cliWriter); waitForSerialPortToFinishTransmitting(cliPort); + stopMotors(); - handleOneshotFeatureChangeOnRestart(); + stopPwmAllMotors(); + + delay(1000); systemReset(); } @@ -2827,6 +2830,10 @@ static void cliStatus(char *cmdline) const uint16_t i2cErrorCounter = 0; #endif +#ifdef STACK_CHECK + cliPrintf("Used stack: %d, Total stack: %d\r\n", getUsedStackSize(), getTotalStackSize()); +#endif + cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t)); } @@ -2988,7 +2995,7 @@ void cliProcess(void) } } -#ifndef SKIP_TASK_STATISTICS +#if !defined(SKIP_TASK_STATISTICS) && !defined(SKIP_CLI_RESOURCES) static void cliResource(char *cmdline) { UNUSED(cmdline); diff --git a/src/main/io/serial_cli.h b/src/main/io/serial_cli.h index 7c8754a3b9c..02748651bbd 100644 --- a/src/main/io/serial_cli.h +++ b/src/main/io/serial_cli.h @@ -23,4 +23,5 @@ struct serialConfig_s; void cliInit(struct serialConfig_s *serialConfig); void cliProcess(void); struct serialPort_s; +void cliEnter(struct serialPort_s *serialPort); bool cliIsActiveOnPort(struct serialPort_s *serialPort); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c deleted file mode 100644 index 6616a01c0d6..00000000000 --- a/src/main/io/serial_msp.c +++ /dev/null @@ -1,1856 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include -#include -#include -#include - -#include "platform.h" - -#include "build/build_config.h" -#include "build/debug.h" -#include "build/version.h" - -#include "scheduler/scheduler.h" - -#include "common/axis.h" -#include "common/color.h" -#include "common/maths.h" - -#include "drivers/system.h" - -#include "drivers/sensor.h" -#include "drivers/accgyro.h" -#include "drivers/compass.h" -#include "drivers/gpio.h" -#include "drivers/pwm_mapping.h" - -#include "drivers/serial.h" -#include "drivers/bus_i2c.h" -#include "drivers/gpio.h" -#include "drivers/timer.h" -#include "drivers/pwm_rx.h" -#include "drivers/sdcard.h" - -#include "drivers/buf_writer.h" -#include "rx/rx.h" -#include "rx/msp.h" -#include "blackbox/blackbox.h" - -#include "io/escservo.h" -#include "fc/rc_controls.h" - -#include "io/gps.h" -#include "io/gimbal.h" -#include "io/serial.h" -#include "io/ledstrip.h" -#include "io/flashfs.h" -#include "io/msp_protocol.h" -#include "io/asyncfatfs/asyncfatfs.h" - -#include "telemetry/telemetry.h" - -#include "sensors/boardalignment.h" -#include "sensors/sensors.h" -#include "sensors/battery.h" -#include "sensors/rangefinder.h" -#include "sensors/acceleration.h" -#include "sensors/barometer.h" -#include "sensors/compass.h" -#include "sensors/gyro.h" - -#include "flight/mixer.h" -#include "flight/pid.h" -#include "flight/imu.h" -#include "flight/hil.h" -#include "flight/failsafe.h" -#include "flight/navigation_rewrite.h" - -#include "fc/mw.h" -#include "fc/runtime_config.h" - -#include "config/config.h" -#include "config/config_profile.h" -#include "config/config_master.h" - -#include "blackbox/blackbox.h" - -#ifdef NAZE -#include "hardware_revision.h" -#endif - -#include "serial_msp.h" - -#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE -#include "io/serial_4way.h" -#endif -static serialPort_t *mspSerialPort; - -extern uint16_t cycleTime; // FIXME dependency on mw.c -extern uint16_t rssi; // FIXME dependency on mw.c -extern void resetPidProfile(pidProfile_t *pidProfile); - -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); - -static const char * const flightControllerIdentifier = INAV_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. -static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; - -typedef struct box_e { - const uint8_t boxId; // see boxId_e - const char *boxName; // GUI-readable box name - const uint8_t permanentId; // -} box_t; - -// FIXME remove ;'s -static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { - { BOXARM, "ARM;", 0 }, - { BOXANGLE, "ANGLE;", 1 }, - { BOXHORIZON, "HORIZON;", 2 }, - { BOXNAVALTHOLD, "NAV ALTHOLD;", 3 }, // old BARO - { BOXMAG, "MAG;", 5 }, - { BOXHEADFREE, "HEADFREE;", 6 }, - { BOXHEADADJ, "HEADADJ;", 7 }, - { BOXCAMSTAB, "CAMSTAB;", 8 }, - { BOXCAMTRIG, "CAMTRIG;", 9 }, - { BOXNAVRTH, "NAV RTH;", 10 }, // old GPS HOME - { BOXNAVPOSHOLD, "NAV POSHOLD;", 11 }, // old GPS HOLD - { BOXPASSTHRU, "PASSTHRU;", 12 }, - { BOXBEEPERON, "BEEPER;", 13 }, - { BOXLEDMAX, "LEDMAX;", 14 }, - { BOXLEDLOW, "LEDLOW;", 15 }, - { BOXLLIGHTS, "LLIGHTS;", 16 }, - { BOXGOV, "GOVERNOR;", 18 }, - { BOXOSD, "OSD SW;", 19 }, - { BOXTELEMETRY, "TELEMETRY;", 20 }, - //{ BOXGTUNE, "GTUNE;", 21 }, - { BOXSERVO1, "SERVO1;", 23 }, - { BOXSERVO2, "SERVO2;", 24 }, - { BOXSERVO3, "SERVO3;", 25 }, - { BOXBLACKBOX, "BLACKBOX;", 26 }, - { BOXFAILSAFE, "FAILSAFE;", 27 }, - { BOXNAVWP, "NAV WP;", 28 }, - { BOXAIRMODE, "AIR MODE;", 29 }, - { BOXHOMERESET, "HOME RESET;", 30 }, - { BOXGCSNAV, "GCS NAV;", 31 }, - { BOXHEADINGLOCK, "HEADING LOCK;", 32 }, - { BOXSURFACE, "SURFACE;", 33 }, - { BOXFLAPERON, "FLAPERON;", 34 }, - { BOXTURNASSIST, "TURN ASSIST;", 35 }, - { CHECKBOX_ITEM_COUNT, NULL, 0xFF } -}; - -// this is calculated at startup based on enabled features. -static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT]; -// this is the number of filled indexes in above array -static uint8_t activeBoxIdCount = 0; -// from mixer.c -extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; - -// cause reboot after MSP processing complete -static bool isRebootScheduled = false; - -static const char pidnames[] = - "ROLL;" - "PITCH;" - "YAW;" - "ALT;" - "Pos;" - "PosR;" - "NavR;" - "LEVEL;" - "MAG;" - "VEL;"; - -typedef enum { - MSP_SDCARD_STATE_NOT_PRESENT = 0, - MSP_SDCARD_STATE_FATAL = 1, - MSP_SDCARD_STATE_CARD_INIT = 2, - MSP_SDCARD_STATE_FS_INIT = 3, - MSP_SDCARD_STATE_READY = 4, -} mspSDCardState_e; - -typedef enum { - MSP_SDCARD_FLAG_SUPPORTTED = 1, -} mspSDCardFlags_e; - -typedef enum { - MSP_FLASHFS_BIT_READY = 1, - MSP_FLASHFS_BIT_SUPPORTED = 2, -} mspFlashfsFlags_e; - -static mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; - -static mspPort_t *currentPort; -static bufWriter_t *writer; - -static void serialize8(uint8_t a) -{ - bufWriterAppend(writer, a); - currentPort->checksum ^= a; -} - -static void serialize16(uint16_t a) -{ - serialize8((uint8_t)(a >> 0)); - serialize8((uint8_t)(a >> 8)); -} - -static void serialize32(uint32_t a) -{ - serialize16((uint16_t)(a >> 0)); - serialize16((uint16_t)(a >> 16)); -} - -static uint8_t read8(void) -{ - return currentPort->inBuf[currentPort->indRX++] & 0xff; -} - -static uint16_t read16(void) -{ - uint16_t t = read8(); - t += (uint16_t)read8() << 8; - return t; -} - -static uint32_t read32(void) -{ - uint32_t t = read16(); - t += (uint32_t)read16() << 16; - return t; -} - -static void headSerialResponse(uint8_t err, uint8_t responseBodySize) -{ - serialBeginWrite(mspSerialPort); - - serialize8('$'); - serialize8('M'); - serialize8(err ? '!' : '>'); - currentPort->checksum = 0; // start calculating a new checksum - serialize8(responseBodySize); - serialize8(currentPort->cmdMSP); -} - -static void headSerialReply(uint8_t responseBodySize) -{ - headSerialResponse(0, responseBodySize); -} - -static void headSerialError(uint8_t responseBodySize) -{ - headSerialResponse(1, responseBodySize); -} - -static void tailSerialReply(void) -{ - serialize8(currentPort->checksum); - serialEndWrite(mspSerialPort); -} - -#ifdef USE_SERVOS -static void s_struct(uint8_t *cb, uint8_t siz) -{ - headSerialReply(siz); - while (siz--) - serialize8(*cb++); -} -#endif - -static void serializeNames(const char *s) -{ - const char *c; - for (c = s; *c; c++) - serialize8(*c); -} - -static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId) -{ - uint8_t boxIndex; - const box_t *candidate; - for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) { - candidate = &boxes[boxIndex]; - if (candidate->boxId == activeBoxId) { - return candidate; - } - } - return NULL; -} - -static const box_t *findBoxByPermenantId(uint8_t permenantId) -{ - uint8_t boxIndex; - const box_t *candidate; - for (boxIndex = 0; boxIndex < sizeof(boxes) / sizeof(box_t); boxIndex++) { - candidate = &boxes[boxIndex]; - if (candidate->permanentId == permenantId) { - return candidate; - } - } - return NULL; -} - -static void serializeBoxNamesReply(void) -{ - int i, activeBoxId, j, flag = 1, count = 0, len; - const box_t *box; - -reset: - // in first run of the loop, we grab total size of junk to be sent - // then come back and actually send it - for (i = 0; i < activeBoxIdCount; i++) { - activeBoxId = activeBoxIds[i]; - - box = findBoxByActiveBoxId(activeBoxId); - if (!box) { - continue; - } - - len = strlen(box->boxName); - if (flag) { - count += len; - } else { - for (j = 0; j < len; j++) - serialize8(box->boxName[j]); - } - } - - if (flag) { - headSerialReply(count); - flag = 0; - goto reset; - } -} - -static void serializeSDCardSummaryReply(void) -{ - headSerialReply(3 + 2 * 4); - -#ifdef USE_SDCARD - uint8_t flags = MSP_SDCARD_FLAG_SUPPORTTED; - uint8_t state; - - serialize8(flags); - - // Merge the card and filesystem states together - if (!sdcard_isInserted()) { - state = MSP_SDCARD_STATE_NOT_PRESENT; - } else if (!sdcard_isFunctional()) { - state = MSP_SDCARD_STATE_FATAL; - } else { - switch (afatfs_getFilesystemState()) { - case AFATFS_FILESYSTEM_STATE_READY: - state = MSP_SDCARD_STATE_READY; - break; - case AFATFS_FILESYSTEM_STATE_INITIALIZATION: - if (sdcard_isInitialized()) { - state = MSP_SDCARD_STATE_FS_INIT; - } else { - state = MSP_SDCARD_STATE_CARD_INIT; - } - break; - case AFATFS_FILESYSTEM_STATE_FATAL: - case AFATFS_FILESYSTEM_STATE_UNKNOWN: - default: - state = MSP_SDCARD_STATE_FATAL; - break; - } - } - - serialize8(state); - serialize8(afatfs_getLastError()); - // Write free space and total space in kilobytes - serialize32(afatfs_getContiguousFreeSpace() / 1024); - serialize32(sdcard_getMetadata()->numBlocks / 2); // Block size is half a kilobyte -#else - serialize8(0); - serialize8(0); - serialize8(0); - serialize32(0); - serialize32(0); -#endif -} - -static void serializeDataflashSummaryReply(void) -{ - headSerialReply(1 + 3 * 4); -#ifdef USE_FLASHFS - const flashGeometry_t *geometry = flashfsGetGeometry(); - serialize8(flashfsIsReady() ? 1 : 0); - serialize32(geometry->sectors); - serialize32(geometry->totalSize); - serialize32(flashfsGetOffset()); // Effectively the current number of bytes stored on the volume -#else - serialize8(0); - serialize32(0); - serialize32(0); - serialize32(0); -#endif -} - -#ifdef USE_FLASHFS -static void serializeDataflashReadReply(uint32_t address, uint8_t size) -{ - uint8_t buffer[128]; - int bytesRead; - - if (size > sizeof(buffer)) { - size = sizeof(buffer); - } - - headSerialReply(4 + size); - - serialize32(address); - - // bytesRead will be lower than that requested if we reach end of volume - bytesRead = flashfsReadAbs(address, buffer, size); - - for (int i = 0; i < bytesRead; i++) { - serialize8(buffer[i]); - } -} -#endif - -static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort) -{ - memset(mspPortToReset, 0, sizeof(mspPort_t)); - - mspPortToReset->port = serialPort; -} - -void mspAllocateSerialPorts(void) -{ - serialPort_t *serialPort; - - uint8_t portIndex = 0; - - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP); - - while (portConfig && portIndex < MAX_MSP_PORT_COUNT) { - mspPort_t *mspPort = &mspPorts[portIndex]; - if (mspPort->port) { - portIndex++; - continue; - } - - serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); - if (serialPort) { - resetMspPort(mspPort, serialPort); - portIndex++; - } - - portConfig = findNextSerialPortConfig(FUNCTION_MSP); - } -} - -void mspReleasePortIfAllocated(serialPort_t *serialPort) -{ - uint8_t portIndex; - for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { - mspPort_t *candidateMspPort = &mspPorts[portIndex]; - if (candidateMspPort->port == serialPort) { - closeSerialPort(serialPort); - memset(candidateMspPort, 0, sizeof(mspPort_t)); - } - } -} - -void mspInit(void) -{ - // calculate used boxes based on features and fill availableBoxes[] array - memset(activeBoxIds, 0xFF, sizeof(activeBoxIds)); - - activeBoxIdCount = 0; - activeBoxIds[activeBoxIdCount++] = BOXARM; - - if (sensors(SENSOR_ACC)) { - activeBoxIds[activeBoxIdCount++] = BOXANGLE; - activeBoxIds[activeBoxIdCount++] = BOXHORIZON; - activeBoxIds[activeBoxIdCount++] = BOXTURNASSIST; - } - - activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; - activeBoxIds[activeBoxIdCount++] = BOXHEADINGLOCK; - - if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { - activeBoxIds[activeBoxIdCount++] = BOXMAG; - activeBoxIds[activeBoxIdCount++] = BOXHEADFREE; - activeBoxIds[activeBoxIdCount++] = BOXHEADADJ; - } - - if (feature(FEATURE_SERVO_TILT)) - activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; - - bool isFixedWing = masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE; - -#ifdef GPS - if (sensors(SENSOR_BARO) || (isFixedWing && feature(FEATURE_GPS))) { - activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD; - activeBoxIds[activeBoxIdCount++] = BOXSURFACE; - } - if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (isFixedWing && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) { - activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD; - activeBoxIds[activeBoxIdCount++] = BOXNAVRTH; - activeBoxIds[activeBoxIdCount++] = BOXNAVWP; - activeBoxIds[activeBoxIdCount++] = BOXHOMERESET; - activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; - } -#endif - - if (isFixedWing) { - activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU; - } - - /* - * FLAPERON mode active only in case of airplane and custom airplane. Activating on - * flying wing can cause bad thing - */ - if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) { - activeBoxIds[activeBoxIdCount++] = BOXFLAPERON; - } - - activeBoxIds[activeBoxIdCount++] = BOXBEEPERON; - -#ifdef LED_STRIP - if (feature(FEATURE_LED_STRIP)) { - activeBoxIds[activeBoxIdCount++] = BOXLEDLOW; - } -#endif - - activeBoxIds[activeBoxIdCount++] = BOXOSD; - -#ifdef TELEMETRY - if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch) - activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY; -#endif - -#ifdef USE_SERVOS - if (masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) { - activeBoxIds[activeBoxIdCount++] = BOXSERVO1; - activeBoxIds[activeBoxIdCount++] = BOXSERVO2; - activeBoxIds[activeBoxIdCount++] = BOXSERVO3; - } -#endif - -#ifdef BLACKBOX - if (feature(FEATURE_BLACKBOX)){ - activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX; - } -#endif - - if (feature(FEATURE_FAILSAFE)){ - activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE; - } - - memset(mspPorts, 0x00, sizeof(mspPorts)); - mspAllocateSerialPorts(); -} - -#define IS_ENABLED(mask) (mask == 0 ? 0 : 1) - -static uint32_t packFlightModeFlags(void) -{ - uint32_t i, junk, tmp; - - // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES - // Requires new Multiwii protocol version to fix - // It would be preferable to setting the enabled bits based on BOXINDEX. - junk = 0; - tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE | - IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON | - IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG | - IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHEADADJ)) << BOXHEADADJ | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMSTAB)) << BOXCAMSTAB | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCAMTRIG)) << BOXCAMTRIG | - IS_ENABLED(FLIGHT_MODE(PASSTHRU_MODE)) << BOXPASSTHRU | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBEEPERON)) << BOXBEEPERON | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDMAX)) << BOXLEDMAX | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLEDLOW)) << BOXLEDLOW | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLLIGHTS)) << BOXLLIGHTS | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | - IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | - IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | - IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)) << BOXNAVALTHOLD | - IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)) << BOXNAVPOSHOLD | - IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)) << BOXNAVRTH | - IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)) << BOXNAVWP | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)) << BOXGCSNAV | - IS_ENABLED(FLIGHT_MODE(HEADING_LOCK)) << BOXHEADINGLOCK | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSURFACE)) << BOXSURFACE | - IS_ENABLED(FLIGHT_MODE(FLAPERON)) << BOXFLAPERON | - IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)) << BOXTURNASSIST | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)) << BOXHOMERESET; - - for (i = 0; i < activeBoxIdCount; i++) { - int flag = (tmp & (1 << activeBoxIds[i])); - if (flag) - junk |= 1 << i; - } - - return junk; -} - -static bool processOutCommand(uint8_t cmdMSP) -{ - uint32_t i; - -#ifdef NAV - int8_t msp_wp_no; - navWaypoint_t msp_wp; -#endif - - switch (cmdMSP) { - case MSP_API_VERSION: - headSerialReply( - 1 + // protocol version length - API_VERSION_LENGTH - ); - serialize8(MSP_PROTOCOL_VERSION); - - serialize8(API_VERSION_MAJOR); - serialize8(API_VERSION_MINOR); - break; - - case MSP_FC_VARIANT: - headSerialReply(FLIGHT_CONTROLLER_IDENTIFIER_LENGTH); - - for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) { - serialize8(flightControllerIdentifier[i]); - } - break; - - case MSP_FC_VERSION: - headSerialReply(FLIGHT_CONTROLLER_VERSION_LENGTH); - - serialize8(FC_VERSION_MAJOR); - serialize8(FC_VERSION_MINOR); - serialize8(FC_VERSION_PATCH_LEVEL); - break; - - case MSP_BOARD_INFO: - headSerialReply( - BOARD_IDENTIFIER_LENGTH + - BOARD_HARDWARE_REVISION_LENGTH - ); - for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) { - serialize8(boardIdentifier[i]); - } -#ifdef NAZE - serialize16(hardwareRevision); -#else - serialize16(0); // No other build targets currently have hardware revision detection. -#endif - break; - - case MSP_BUILD_INFO: - headSerialReply( - BUILD_DATE_LENGTH + - BUILD_TIME_LENGTH + - GIT_SHORT_REVISION_LENGTH - ); - - for (i = 0; i < BUILD_DATE_LENGTH; i++) { - serialize8(buildDate[i]); - } - for (i = 0; i < BUILD_TIME_LENGTH; i++) { - serialize8(buildTime[i]); - } - - for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) { - serialize8(shortGitRevision[i]); - } - break; - - // DEPRECATED - Use MSP_API_VERSION - case MSP_IDENT: - headSerialReply(7); - serialize8(MW_VERSION); - serialize8(masterConfig.mixerMode); - serialize8(MSP_PROTOCOL_VERSION); - serialize32(CAP_PLATFORM_32BIT | CAP_DYNBALANCE | CAP_FLAPS | CAP_NAVCAP | CAP_EXTAUX); // "capability" - break; - -#ifdef HIL - case MSP_HIL_STATE: - headSerialReply(8); - serialize16(hilToSIM.pidCommand[ROLL]); - serialize16(hilToSIM.pidCommand[PITCH]); - serialize16(hilToSIM.pidCommand[YAW]); - serialize16(hilToSIM.pidCommand[THROTTLE]); - break; -#endif - - case MSP_STATUS_EX: - headSerialReply(13); - serialize16(cycleTime); -#ifdef USE_I2C - serialize16(i2cGetErrorCounter()); -#else - serialize16(0); -#endif - serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); - serialize32(packFlightModeFlags()); - serialize8(masterConfig.current_profile_index); - serialize16(averageSystemLoadPercent); - break; - - case MSP_STATUS: - headSerialReply(11); - serialize16(cycleTime); -#ifdef USE_I2C - serialize16(i2cGetErrorCounter()); -#else - serialize16(0); -#endif - serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); - serialize32(packFlightModeFlags()); - serialize8(masterConfig.current_profile_index); - break; - case MSP_RAW_IMU: - headSerialReply(18); - - // Hack scale due to choice of units for sensor data in multiwii - const uint8_t scale = (acc.acc_1G > 1024) ? 8 : 1; - - for (i = 0; i < 3; i++) - serialize16(accADC[i] / scale); - for (i = 0; i < 3; i++) - serialize16(gyroADC[i]); - for (i = 0; i < 3; i++) - serialize16(magADC[i]); - break; -#ifdef USE_SERVOS - case MSP_SERVO: - s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2); - break; - case MSP_SERVO_CONFIGURATIONS: - headSerialReply(MAX_SUPPORTED_SERVOS * sizeof(servoParam_t)); - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - serialize16(currentProfile->servoConf[i].min); - serialize16(currentProfile->servoConf[i].max); - serialize16(currentProfile->servoConf[i].middle); - serialize8(currentProfile->servoConf[i].rate); - serialize8(currentProfile->servoConf[i].angleAtMin); - serialize8(currentProfile->servoConf[i].angleAtMax); - serialize8(currentProfile->servoConf[i].forwardFromChannel); - serialize32(currentProfile->servoConf[i].reversedSources); - } - break; - case MSP_SERVO_MIX_RULES: - headSerialReply(MAX_SERVO_RULES * sizeof(servoMixer_t)); - for (i = 0; i < MAX_SERVO_RULES; i++) { - serialize8(masterConfig.customServoMixer[i].targetChannel); - serialize8(masterConfig.customServoMixer[i].inputSource); - serialize8(masterConfig.customServoMixer[i].rate); - serialize8(masterConfig.customServoMixer[i].speed); - serialize8(masterConfig.customServoMixer[i].min); - serialize8(masterConfig.customServoMixer[i].max); - serialize8(masterConfig.customServoMixer[i].box); - } - break; -#endif - case MSP_MOTOR: - headSerialReply(16); - for (unsigned i = 0; i < 8; i++) { - serialize16(i < MAX_SUPPORTED_MOTORS ? motor[i] : 0); - } - break; - case MSP_RC: - headSerialReply(2 * rxRuntimeConfig.channelCount); - for (i = 0; i < rxRuntimeConfig.channelCount; i++) - serialize16(rcData[i]); - break; - case MSP_ATTITUDE: - headSerialReply(6); - serialize16(attitude.values.roll); - serialize16(attitude.values.pitch); - serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); - break; - case MSP_ALTITUDE: - headSerialReply(6); -#if defined(NAV) - serialize32((uint32_t)lrintf(getEstimatedActualPosition(Z))); - serialize16((uint32_t)lrintf(getEstimatedActualVelocity(Z))); -#else - serialize32(0); - serialize16(0); -#endif - break; - case MSP_SONAR_ALTITUDE: - headSerialReply(4); -#if defined(SONAR) - serialize32(rangefinderGetLatestAltitude()); -#else - serialize32(0); -#endif - break; - case MSP_ANALOG: - headSerialReply(7); - serialize8((uint8_t)constrain(vbat, 0, 255)); - serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery - serialize16(rssi); - if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) { - serialize16((uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero - } else - serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A - break; - case MSP_ARMING_CONFIG: - headSerialReply(2); - serialize8(masterConfig.auto_disarm_delay); - serialize8(masterConfig.disarm_kill_switch); - break; - case MSP_LOOP_TIME: - headSerialReply(2); - serialize16(masterConfig.looptime); - break; - case MSP_RC_TUNING: - headSerialReply(11); - serialize8(100); //rcRate8 kept for compatibity reasons, this setting is no longer used - serialize8(currentControlRateProfile->rcExpo8); - for (i = 0 ; i < 3; i++) { - serialize8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t - } - serialize8(currentControlRateProfile->dynThrPID); - serialize8(currentControlRateProfile->thrMid8); - serialize8(currentControlRateProfile->thrExpo8); - serialize16(currentControlRateProfile->tpa_breakpoint); - serialize8(currentControlRateProfile->rcYawExpo8); - break; - case MSP_PID: - headSerialReply(3 * PID_ITEM_COUNT); - for (i = 0; i < PID_ITEM_COUNT; i++) { - serialize8(currentProfile->pidProfile.P8[i]); - serialize8(currentProfile->pidProfile.I8[i]); - serialize8(currentProfile->pidProfile.D8[i]); - } - break; - case MSP_PIDNAMES: - headSerialReply(sizeof(pidnames) - 1); - serializeNames(pidnames); - break; - case MSP_PID_CONTROLLER: - headSerialReply(1); - serialize8(2); // FIXME: Report as LuxFloat - break; - case MSP_MODE_RANGES: - headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT); - for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { - modeActivationCondition_t *mac = ¤tProfile->modeActivationConditions[i]; - const box_t *box = findBoxByActiveBoxId(mac->modeId); - serialize8(box ? box->permanentId : 0); - serialize8(mac->auxChannelIndex); - serialize8(mac->range.startStep); - serialize8(mac->range.endStep); - } - break; - case MSP_ADJUSTMENT_RANGES: - headSerialReply(MAX_ADJUSTMENT_RANGE_COUNT * ( - 1 + // adjustment index/slot - 1 + // aux channel index - 1 + // start step - 1 + // end step - 1 + // adjustment function - 1 // aux switch channel index - )); - for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { - adjustmentRange_t *adjRange = ¤tProfile->adjustmentRanges[i]; - serialize8(adjRange->adjustmentIndex); - serialize8(adjRange->auxChannelIndex); - serialize8(adjRange->range.startStep); - serialize8(adjRange->range.endStep); - serialize8(adjRange->adjustmentFunction); - serialize8(adjRange->auxSwitchChannelIndex); - } - break; - case MSP_BOXNAMES: - serializeBoxNamesReply(); - break; - case MSP_BOXIDS: - headSerialReply(activeBoxIdCount); - for (i = 0; i < activeBoxIdCount; i++) { - const box_t *box = findBoxByActiveBoxId(activeBoxIds[i]); - if (!box) { - continue; - } - serialize8(box->permanentId); - } - break; - case MSP_MISC: - headSerialReply(2 * 5 + 3 + 3 + 2 + 4); - serialize16(masterConfig.rxConfig.midrc); - - serialize16(masterConfig.escAndServoConfig.minthrottle); - serialize16(masterConfig.escAndServoConfig.maxthrottle); - serialize16(masterConfig.escAndServoConfig.mincommand); - - serialize16(masterConfig.failsafeConfig.failsafe_throttle); - -#ifdef GPS - serialize8(masterConfig.gpsConfig.provider); // gps_type - serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t - serialize8(masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas -#else - serialize8(0); // gps_type - serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t - serialize8(0); // gps_ubx_sbas -#endif - serialize8(masterConfig.batteryConfig.multiwiiCurrentMeterOutput); - serialize8(masterConfig.rxConfig.rssi_channel); - serialize8(0); - - serialize16(currentProfile->mag_declination / 10); - - serialize8(masterConfig.batteryConfig.vbatscale); - serialize8(masterConfig.batteryConfig.vbatmincellvoltage); - serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage); - serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage); - break; - - case MSP_MOTOR_PINS: - // FIXME This is hardcoded and should not be. - headSerialReply(8); - for (i = 0; i < 8; i++) - serialize8(i + 1); - break; -#ifdef GPS - case MSP_RAW_GPS: - headSerialReply(18); - serialize8(gpsSol.fixType); - serialize8(gpsSol.numSat); - serialize32(gpsSol.llh.lat); - serialize32(gpsSol.llh.lon); - serialize16(gpsSol.llh.alt/100); // meters - serialize16(gpsSol.groundSpeed); - serialize16(gpsSol.groundCourse); - serialize16(gpsSol.hdop); - break; - case MSP_COMP_GPS: - headSerialReply(5); - serialize16(GPS_distanceToHome); - serialize16(GPS_directionToHome); - serialize8(gpsSol.flags.gpsHeartbeat ? 1 : 0); - break; -#ifdef NAV - case MSP_NAV_STATUS: - headSerialReply(7); - serialize8(NAV_Status.mode); - serialize8(NAV_Status.state); - serialize8(NAV_Status.activeWpAction); - serialize8(NAV_Status.activeWpNumber); - serialize8(NAV_Status.error); - //serialize16( (int16_t)(target_bearing/100)); - serialize16(getMagHoldHeading()); - break; - case MSP_WP: - msp_wp_no = read8(); // get the wp number - getWaypoint(msp_wp_no, &msp_wp); - headSerialReply(21); - serialize8(msp_wp_no); // wp_no - serialize8(msp_wp.action); // action (WAYPOINT) - serialize32(msp_wp.lat); // lat - serialize32(msp_wp.lon); // lon - serialize32(msp_wp.alt); // altitude (cm) - serialize16(msp_wp.p1); // P1 - serialize16(msp_wp.p2); // P2 - serialize16(msp_wp.p3); // P3 - serialize8(msp_wp.flag); // flags - break; -#endif - case MSP_GPSSVINFO: - /* Compatibility stub - return zero SVs */ - headSerialReply(1 + (1 * 4)); - serialize8(1); - - // HDOP - serialize8(0); - serialize8(0); - serialize8(gpsSol.hdop / 100); - serialize8(gpsSol.hdop / 100); - break; - case MSP_GPSSTATISTICS: - headSerialReply(20); - serialize16(gpsStats.lastMessageDt); - serialize32(gpsStats.errors); - serialize32(gpsStats.timeouts); - serialize32(gpsStats.packetCount); - serialize16(gpsSol.hdop); - serialize16(gpsSol.eph); - serialize16(gpsSol.epv); - break; -#endif - case MSP_DEBUG: - headSerialReply(DEBUG16_VALUE_COUNT * sizeof(debug[0])); - - // output some useful QA statistics - // debug[x] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] - - for (i = 0; i < DEBUG16_VALUE_COUNT; i++) - serialize16(debug[i]); // 4 variables are here for general monitoring purpose - break; - - case MSP_UID: - headSerialReply(12); - serialize32(U_ID_0); - serialize32(U_ID_1); - serialize32(U_ID_2); - break; - - case MSP_FEATURE: - headSerialReply(4); - serialize32(featureMask()); - break; - - case MSP_BOARD_ALIGNMENT: - headSerialReply(6); - serialize16(masterConfig.boardAlignment.rollDeciDegrees); - serialize16(masterConfig.boardAlignment.pitchDeciDegrees); - serialize16(masterConfig.boardAlignment.yawDeciDegrees); - break; - - case MSP_VOLTAGE_METER_CONFIG: - headSerialReply(4); - serialize8(masterConfig.batteryConfig.vbatscale); - serialize8(masterConfig.batteryConfig.vbatmincellvoltage); - serialize8(masterConfig.batteryConfig.vbatmaxcellvoltage); - serialize8(masterConfig.batteryConfig.vbatwarningcellvoltage); - break; - - case MSP_CURRENT_METER_CONFIG: - headSerialReply(7); - serialize16(masterConfig.batteryConfig.currentMeterScale); - serialize16(masterConfig.batteryConfig.currentMeterOffset); - serialize8(masterConfig.batteryConfig.currentMeterType); - serialize16(masterConfig.batteryConfig.batteryCapacity); - break; - - case MSP_MIXER: - headSerialReply(1); - serialize8(masterConfig.mixerMode); - break; - - case MSP_RX_CONFIG: - headSerialReply(22); - serialize8(masterConfig.rxConfig.serialrx_provider); - serialize16(masterConfig.rxConfig.maxcheck); - serialize16(masterConfig.rxConfig.midrc); - serialize16(masterConfig.rxConfig.mincheck); - serialize8(masterConfig.rxConfig.spektrum_sat_bind); - serialize16(masterConfig.rxConfig.rx_min_usec); - serialize16(masterConfig.rxConfig.rx_max_usec); - serialize8(0); // for compatibility with betaflight - serialize8(0); // for compatibility with betaflight - serialize16(0); // for compatibility with betaflight - serialize8(masterConfig.rxConfig.nrf24rx_protocol); - serialize32(masterConfig.rxConfig.nrf24rx_id); - serialize8(masterConfig.rxConfig.nrf24rx_rf_channel_count); - break; - - case MSP_FAILSAFE_CONFIG: - headSerialReply(8); - serialize8(masterConfig.failsafeConfig.failsafe_delay); - serialize8(masterConfig.failsafeConfig.failsafe_off_delay); - serialize16(masterConfig.failsafeConfig.failsafe_throttle); - serialize8(masterConfig.failsafeConfig.failsafe_kill_switch); - serialize16(masterConfig.failsafeConfig.failsafe_throttle_low_delay); - serialize8(masterConfig.failsafeConfig.failsafe_procedure); - break; - - case MSP_RXFAIL_CONFIG: - headSerialReply(3 * (rxRuntimeConfig.channelCount)); - for (i = 0; i < rxRuntimeConfig.channelCount; i++) { - serialize8(masterConfig.rxConfig.failsafe_channel_configurations[i].mode); - serialize16(RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_channel_configurations[i].step)); - } - break; - - case MSP_RSSI_CONFIG: - headSerialReply(1); - serialize8(masterConfig.rxConfig.rssi_channel); - break; - - case MSP_RX_MAP: - headSerialReply(MAX_MAPPABLE_RX_INPUTS); - for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) - serialize8(masterConfig.rxConfig.rcmap[i]); - break; - - case MSP_BF_CONFIG: - headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2); - serialize8(masterConfig.mixerMode); - - serialize32(featureMask()); - - serialize8(masterConfig.rxConfig.serialrx_provider); - - serialize16(masterConfig.boardAlignment.rollDeciDegrees); - serialize16(masterConfig.boardAlignment.pitchDeciDegrees); - serialize16(masterConfig.boardAlignment.yawDeciDegrees); - - serialize16(masterConfig.batteryConfig.currentMeterScale); - serialize16(masterConfig.batteryConfig.currentMeterOffset); - break; - - case MSP_CF_SERIAL_CONFIG: - headSerialReply( - ((sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4)) * serialGetAvailablePortCount()) - ); - for (i = 0; i < SERIAL_PORT_COUNT; i++) { - if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) { - continue; - }; - serialize8(masterConfig.serialConfig.portConfigs[i].identifier); - serialize16(masterConfig.serialConfig.portConfigs[i].functionMask); - serialize8(masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex); - serialize8(masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex); - serialize8(masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex); - serialize8(masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex); - } - break; - -#ifdef LED_STRIP - case MSP_LED_COLORS: - headSerialReply(LED_CONFIGURABLE_COLOR_COUNT * 4); - for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &masterConfig.colors[i]; - serialize16(color->h); - serialize8(color->s); - serialize8(color->v); - } - break; - - case MSP_LED_STRIP_CONFIG: - headSerialReply(LED_MAX_STRIP_LENGTH * 4); - for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - serialize32(*ledConfig); - } - break; - - case MSP_LED_STRIP_MODECOLOR: - headSerialReply(((LED_MODE_COUNT * LED_DIRECTION_COUNT) + LED_SPECIAL_COLOR_COUNT) * 3); - for (int i = 0; i < LED_MODE_COUNT; i++) { - for (int j = 0; j < LED_DIRECTION_COUNT; j++) { - serialize8(i); - serialize8(j); - serialize8(masterConfig.modeColors[i].color[j]); - } - } - - for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { - serialize8(LED_MODE_COUNT); - serialize8(j); - serialize8(masterConfig.specialColors.color[j]); - } - break; -#endif - - case MSP_DATAFLASH_SUMMARY: - serializeDataflashSummaryReply(); - break; - -#ifdef USE_FLASHFS - case MSP_DATAFLASH_READ: - { - uint32_t readAddress = read32(); - - serializeDataflashReadReply(readAddress, 128); - } - break; -#endif - - case MSP_BLACKBOX_CONFIG: - headSerialReply(4); -#ifdef BLACKBOX - serialize8(1); //Blackbox supported - serialize8(masterConfig.blackbox_device); - serialize8(masterConfig.blackbox_rate_num); - serialize8(masterConfig.blackbox_rate_denom); -#else - serialize8(0); // Blackbox not supported - serialize8(0); - serialize8(0); - serialize8(0); -#endif - break; - - case MSP_SDCARD_SUMMARY: - serializeSDCardSummaryReply(); - break; - - case MSP_BF_BUILD_INFO: - headSerialReply(11 + 4 + 4); - for (i = 0; i < 11; i++) - serialize8(buildDate[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc - serialize32(0); // future exp - serialize32(0); // future exp - break; - - case MSP_3D: - headSerialReply(2 * 3); - serialize16(masterConfig.flight3DConfig.deadband3d_low); - serialize16(masterConfig.flight3DConfig.deadband3d_high); - serialize16(masterConfig.flight3DConfig.neutral3d); - break; - - case MSP_RC_DEADBAND: - headSerialReply(5); - serialize8(currentProfile->rcControlsConfig.deadband); - serialize8(currentProfile->rcControlsConfig.yaw_deadband); - serialize8(currentProfile->rcControlsConfig.alt_hold_deadband); - serialize16(masterConfig.flight3DConfig.deadband3d_throttle); - break; - case MSP_SENSOR_ALIGNMENT: - headSerialReply(3); - serialize8(masterConfig.sensorAlignmentConfig.gyro_align); - serialize8(masterConfig.sensorAlignmentConfig.acc_align); - serialize8(masterConfig.sensorAlignmentConfig.mag_align); - break; - - default: - return false; - } - return true; -} - -static bool processInCommand(void) -{ - uint32_t i; - uint16_t tmp; - uint8_t rate; - -#ifdef NAV - uint8_t msp_wp_no; - navWaypoint_t msp_wp; -#endif - - switch (currentPort->cmdMSP) { -#ifdef HIL - case MSP_SET_HIL_STATE: - hilToFC.rollAngle = read16(); - hilToFC.pitchAngle = read16(); - hilToFC.yawAngle = read16(); - hilToFC.baroAlt = read32(); - hilToFC.bodyAccel[0] = read16(); - hilToFC.bodyAccel[1] = read16(); - hilToFC.bodyAccel[2] = read16(); - hilActive = true; - break; -#endif - case MSP_SELECT_SETTING: - if (!ARMING_FLAG(ARMED)) { - masterConfig.current_profile_index = read8(); - if (masterConfig.current_profile_index > 2) { - masterConfig.current_profile_index = 0; - } - writeEEPROM(); - readEEPROM(); - } - break; - case MSP_SET_HEAD: - updateMagHoldHeading(read16()); - break; - case MSP_SET_RAW_RC: -#ifndef SKIP_RX_MSP - { - uint8_t channelCount = currentPort->dataSize / sizeof(uint16_t); - if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) { - headSerialError(0); - } else { - uint16_t frame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; - - for (i = 0; i < channelCount; i++) { - frame[i] = read16(); - } - - rxMspFrameReceive(frame, channelCount); - } - } -#endif - break; - case MSP_SET_ARMING_CONFIG: - masterConfig.auto_disarm_delay = read8(); - masterConfig.disarm_kill_switch = read8(); - break; - case MSP_SET_LOOP_TIME: - masterConfig.looptime = read16(); - break; - case MSP_SET_PID_CONTROLLER: - // FIXME: Do nothing - break; - case MSP_SET_PID: - for (i = 0; i < PID_ITEM_COUNT; i++) { - currentProfile->pidProfile.P8[i] = read8(); - currentProfile->pidProfile.I8[i] = read8(); - currentProfile->pidProfile.D8[i] = read8(); - } - break; - case MSP_SET_MODE_RANGE: - i = read8(); - if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { - modeActivationCondition_t *mac = ¤tProfile->modeActivationConditions[i]; - i = read8(); - const box_t *box = findBoxByPermenantId(i); - if (box) { - mac->modeId = box->boxId; - mac->auxChannelIndex = read8(); - mac->range.startStep = read8(); - mac->range.endStep = read8(); - - useRcControlsConfig(currentProfile->modeActivationConditions, &masterConfig.escAndServoConfig, ¤tProfile->pidProfile); - } else { - headSerialError(0); - } - } else { - headSerialError(0); - } - break; - case MSP_SET_ADJUSTMENT_RANGE: - i = read8(); - if (i < MAX_ADJUSTMENT_RANGE_COUNT) { - adjustmentRange_t *adjRange = ¤tProfile->adjustmentRanges[i]; - i = read8(); - if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { - adjRange->adjustmentIndex = i; - adjRange->auxChannelIndex = read8(); - adjRange->range.startStep = read8(); - adjRange->range.endStep = read8(); - adjRange->adjustmentFunction = read8(); - adjRange->auxSwitchChannelIndex = read8(); - } else { - headSerialError(0); - } - } else { - headSerialError(0); - } - break; - - case MSP_SET_RC_TUNING: - if (currentPort->dataSize >= 10) { - read8(); //Read rcRate8, kept for protocol compatibility reasons - currentControlRateProfile->rcExpo8 = read8(); - for (i = 0; i < 3; i++) { - rate = read8(); - if (i == FD_YAW) { - currentControlRateProfile->rates[i] = constrain(rate, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); - } - else { - currentControlRateProfile->rates[i] = constrain(rate, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); - } - } - rate = read8(); - currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); - currentControlRateProfile->thrMid8 = read8(); - currentControlRateProfile->thrExpo8 = read8(); - currentControlRateProfile->tpa_breakpoint = read16(); - if (currentPort->dataSize >= 11) { - currentControlRateProfile->rcYawExpo8 = read8(); - } - } else { - headSerialError(0); - } - break; - case MSP_SET_MISC: - tmp = read16(); - if (tmp < 1600 && tmp > 1400) - masterConfig.rxConfig.midrc = tmp; - - masterConfig.escAndServoConfig.minthrottle = read16(); - masterConfig.escAndServoConfig.maxthrottle = read16(); - masterConfig.escAndServoConfig.mincommand = read16(); - - masterConfig.failsafeConfig.failsafe_throttle = read16(); - -#ifdef GPS - masterConfig.gpsConfig.provider = read8(); // gps_type - read8(); // gps_baudrate - masterConfig.gpsConfig.sbasMode = read8(); // gps_ubx_sbas -#else - read8(); // gps_type - read8(); // gps_baudrate - read8(); // gps_ubx_sbas -#endif - masterConfig.batteryConfig.multiwiiCurrentMeterOutput = read8(); - masterConfig.rxConfig.rssi_channel = read8(); - read8(); - - currentProfile->mag_declination = read16() * 10; - - masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended - masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI - masterConfig.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI - masterConfig.batteryConfig.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert - break; - case MSP_SET_MOTOR: - for (i = 0; i < 8; i++) { - const int16_t disarmed = read16(); - if (i < MAX_SUPPORTED_MOTORS) { - motor_disarmed[i] = disarmed; - } - } - break; - case MSP_SET_SERVO_CONFIGURATION: -#ifdef USE_SERVOS - if (currentPort->dataSize != 1 + sizeof(servoParam_t)) { - headSerialError(0); - break; - } - i = read8(); - if (i >= MAX_SUPPORTED_SERVOS) { - headSerialError(0); - } else { - currentProfile->servoConf[i].min = read16(); - currentProfile->servoConf[i].max = read16(); - currentProfile->servoConf[i].middle = read16(); - currentProfile->servoConf[i].rate = read8(); - currentProfile->servoConf[i].angleAtMin = read8(); - currentProfile->servoConf[i].angleAtMax = read8(); - currentProfile->servoConf[i].forwardFromChannel = read8(); - currentProfile->servoConf[i].reversedSources = read32(); - } -#endif - break; - - case MSP_SET_SERVO_MIX_RULE: -#ifdef USE_SERVOS - i = read8(); - if (i >= MAX_SERVO_RULES) { - headSerialError(0); - } else { - masterConfig.customServoMixer[i].targetChannel = read8(); - masterConfig.customServoMixer[i].inputSource = read8(); - masterConfig.customServoMixer[i].rate = read8(); - masterConfig.customServoMixer[i].speed = read8(); - masterConfig.customServoMixer[i].min = read8(); - masterConfig.customServoMixer[i].max = read8(); - masterConfig.customServoMixer[i].box = read8(); - loadCustomServoMixer(); - } -#endif - break; - - case MSP_SET_3D: - masterConfig.flight3DConfig.deadband3d_low = read16(); - masterConfig.flight3DConfig.deadband3d_high = read16(); - masterConfig.flight3DConfig.neutral3d = read16(); - masterConfig.flight3DConfig.deadband3d_throttle = read16(); - break; - - case MSP_SET_RC_DEADBAND: - currentProfile->rcControlsConfig.deadband = read8(); - currentProfile->rcControlsConfig.yaw_deadband = read8(); - currentProfile->rcControlsConfig.alt_hold_deadband = read8(); - break; - - case MSP_SET_RESET_CURR_PID: - resetPidProfile(¤tProfile->pidProfile); - break; - - case MSP_SET_SENSOR_ALIGNMENT: - masterConfig.sensorAlignmentConfig.gyro_align = read8(); - masterConfig.sensorAlignmentConfig.acc_align = read8(); - masterConfig.sensorAlignmentConfig.mag_align = read8(); - break; - - case MSP_RESET_CONF: - if (!ARMING_FLAG(ARMED)) { - resetEEPROM(); - readEEPROM(); - } - break; - case MSP_ACC_CALIBRATION: - if (!ARMING_FLAG(ARMED)) - accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); - break; - case MSP_MAG_CALIBRATION: - if (!ARMING_FLAG(ARMED)) - ENABLE_STATE(CALIBRATE_MAG); - break; - case MSP_EEPROM_WRITE: - if (ARMING_FLAG(ARMED)) { - headSerialError(0); - return true; - } - writeEEPROM(); - readEEPROM(); - break; - -#ifdef BLACKBOX - case MSP_SET_BLACKBOX_CONFIG: - // Don't allow config to be updated while Blackbox is logging - if (!blackboxMayEditConfig()) - return false; - masterConfig.blackbox_device = read8(); - masterConfig.blackbox_rate_num = read8(); - masterConfig.blackbox_rate_denom = read8(); - break; -#endif - -#ifdef USE_FLASHFS - case MSP_DATAFLASH_ERASE: - flashfsEraseCompletely(); - break; -#endif - -#ifdef GPS - case MSP_SET_RAW_GPS: - if (read8()) { - ENABLE_STATE(GPS_FIX); - } else { - DISABLE_STATE(GPS_FIX); - } - gpsSol.flags.validVelNE = 0; - gpsSol.flags.validVelD = 0; - gpsSol.flags.validEPE = 0; - gpsSol.numSat = read8(); - gpsSol.llh.lat = read32(); - gpsSol.llh.lon = read32(); - gpsSol.llh.alt = read16(); - gpsSol.groundSpeed = read16(); - gpsSol.velNED[X] = 0; - gpsSol.velNED[Y] = 0; - gpsSol.velNED[Z] = 0; - gpsSol.eph = 100; - gpsSol.epv = 100; - // Feed data to navigation - sensorsSet(SENSOR_GPS); - onNewGPSData(); - break; -#endif -#ifdef NAV - case MSP_SET_WP: - msp_wp_no = read8(); // get the wp number - msp_wp.action = read8(); // action - msp_wp.lat = read32(); // lat - msp_wp.lon = read32(); // lon - msp_wp.alt = read32(); // to set altitude (cm) - msp_wp.p1 = read16(); // P1 - msp_wp.p2 = read16(); // P2 - msp_wp.p3 = read16(); // P3 - msp_wp.flag = read8(); // future: to set nav flag - setWaypoint(msp_wp_no, &msp_wp); - break; -#endif - case MSP_SET_FEATURE: - featureClearAll(); - featureSet(read32()); // features bitmap - break; - - case MSP_SET_BOARD_ALIGNMENT: - masterConfig.boardAlignment.rollDeciDegrees = read16(); - masterConfig.boardAlignment.pitchDeciDegrees = read16(); - masterConfig.boardAlignment.yawDeciDegrees = read16(); - break; - - case MSP_SET_VOLTAGE_METER_CONFIG: - masterConfig.batteryConfig.vbatscale = read8(); // actual vbatscale as intended - masterConfig.batteryConfig.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI - masterConfig.batteryConfig.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI - masterConfig.batteryConfig.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert - break; - - case MSP_SET_CURRENT_METER_CONFIG: - masterConfig.batteryConfig.currentMeterScale = read16(); - masterConfig.batteryConfig.currentMeterOffset = read16(); - masterConfig.batteryConfig.currentMeterType = read8(); - masterConfig.batteryConfig.batteryCapacity = read16(); - break; - -#ifndef USE_QUAD_MIXER_ONLY - case MSP_SET_MIXER: - masterConfig.mixerMode = read8(); - break; -#endif - - case MSP_SET_RX_CONFIG: - masterConfig.rxConfig.serialrx_provider = read8(); - masterConfig.rxConfig.maxcheck = read16(); - masterConfig.rxConfig.midrc = read16(); - masterConfig.rxConfig.mincheck = read16(); - masterConfig.rxConfig.spektrum_sat_bind = read8(); - if (currentPort->dataSize > 8) { - masterConfig.rxConfig.rx_min_usec = read16(); - masterConfig.rxConfig.rx_max_usec = read16(); - } - if (currentPort->dataSize > 12) { - // for compatibility with betaflight - read8(); - read8(); - read16(); - } - if (currentPort->dataSize > 16) { - masterConfig.rxConfig.nrf24rx_protocol = read8(); - } - if (currentPort->dataSize > 17) { - masterConfig.rxConfig.nrf24rx_id = read32(); - } - if (currentPort->dataSize > 21) { - masterConfig.rxConfig.nrf24rx_rf_channel_count = read8(); - } - break; - - case MSP_SET_FAILSAFE_CONFIG: - masterConfig.failsafeConfig.failsafe_delay = read8(); - masterConfig.failsafeConfig.failsafe_off_delay = read8(); - masterConfig.failsafeConfig.failsafe_throttle = read16(); - masterConfig.failsafeConfig.failsafe_kill_switch = read8(); - masterConfig.failsafeConfig.failsafe_throttle_low_delay = read16(); - masterConfig.failsafeConfig.failsafe_procedure = read8(); - break; - - case MSP_SET_RXFAIL_CONFIG: - i = read8(); - if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) { - masterConfig.rxConfig.failsafe_channel_configurations[i].mode = read8(); - masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(read16()); - } else { - headSerialError(0); - } - break; - - case MSP_SET_RSSI_CONFIG: - masterConfig.rxConfig.rssi_channel = read8(); - break; - - case MSP_SET_RX_MAP: - for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { - masterConfig.rxConfig.rcmap[i] = read8(); - } - break; - - case MSP_SET_BF_CONFIG: - -#ifdef USE_QUAD_MIXER_ONLY - read8(); // mixerMode ignored -#else - masterConfig.mixerMode = read8(); // mixerMode -#endif - - featureClearAll(); - featureSet(read32()); // features bitmap - - masterConfig.rxConfig.serialrx_provider = read8(); // serialrx_type - - masterConfig.boardAlignment.rollDeciDegrees = read16(); // board_align_roll - masterConfig.boardAlignment.pitchDeciDegrees = read16(); // board_align_pitch - masterConfig.boardAlignment.yawDeciDegrees = read16(); // board_align_yaw - - masterConfig.batteryConfig.currentMeterScale = read16(); - masterConfig.batteryConfig.currentMeterOffset = read16(); - break; - - case MSP_SET_CF_SERIAL_CONFIG: - { - uint8_t portConfigSize = sizeof(uint8_t) + sizeof(uint16_t) + (sizeof(uint8_t) * 4); - - if (currentPort->dataSize % portConfigSize != 0) { - headSerialError(0); - break; - } - - uint8_t remainingPortsInPacket = currentPort->dataSize / portConfigSize; - - while (remainingPortsInPacket--) { - uint8_t identifier = read8(); - - serialPortConfig_t *portConfig = serialFindPortConfiguration(identifier); - if (!portConfig) { - headSerialError(0); - break; - } - - portConfig->identifier = identifier; - portConfig->functionMask = read16(); - portConfig->msp_baudrateIndex = read8(); - portConfig->gps_baudrateIndex = read8(); - portConfig->telemetry_baudrateIndex = read8(); - portConfig->blackbox_baudrateIndex = read8(); - } - } - break; - -#ifdef LED_STRIP - case MSP_SET_LED_COLORS: - for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - hsvColor_t *color = &masterConfig.colors[i]; - color->h = read16(); - color->s = read8(); - color->v = read8(); - } - break; - - case MSP_SET_LED_STRIP_CONFIG: - { - i = read8(); - if (i >= LED_MAX_STRIP_LENGTH || currentPort->dataSize != (1 + 4)) { - headSerialError(0); - break; - } - ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - *ledConfig = read32(); - reevaluateLedConfig(); - } - break; - - case MSP_SET_LED_STRIP_MODECOLOR: - { - ledModeIndex_e modeIdx = read8(); - int funIdx = read8(); - int color = read8(); - - if (!setModeColor(modeIdx, funIdx, color)) - return false; - } - break; -#endif - case MSP_REBOOT: - isRebootScheduled = true; - break; - -#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE - case MSP_SET_4WAY_IF: - // get channel number - // switch all motor lines HI - // reply the count of ESC found - headSerialReply(1); - serialize8(esc4wayInit()); - // because we do not come back after calling Process4WayInterface - // proceed with a success reply first - tailSerialReply(); - // flush the transmit buffer - bufWriterFlush(writer); - // wait for all data to send - waitForSerialPortToFinishTransmitting(currentPort->port); - // rem: App: Wait at least appx. 500 ms for BLHeli to jump into - // bootloader mode before try to connect any ESC - // Start to activate here - esc4wayProcess(currentPort->port); - // former used MSP uart is still active - // proceed as usual with MSP commands - break; -#endif - default: - // we do not know how to handle the (valid) message, indicate error MSP $M! - return false; - } - headSerialReply(0); - return true; -} - -static void mspProcessReceivedCommand() { - if (!(processOutCommand(currentPort->cmdMSP) || processInCommand())) { - headSerialError(0); - } - tailSerialReply(); - currentPort->c_state = IDLE; -} - -static bool mspProcessReceivedData(uint8_t c) -{ - if (currentPort->c_state == IDLE) { - if (c == '$') { - currentPort->c_state = HEADER_START; - } else { - return false; - } - } else if (currentPort->c_state == HEADER_START) { - currentPort->c_state = (c == 'M') ? HEADER_M : IDLE; - } else if (currentPort->c_state == HEADER_M) { - currentPort->c_state = (c == '<') ? HEADER_ARROW : IDLE; - } else if (currentPort->c_state == HEADER_ARROW) { - if (c > MSP_PORT_INBUF_SIZE) { - currentPort->c_state = IDLE; - - } else { - currentPort->dataSize = c; - currentPort->offset = 0; - currentPort->checksum = 0; - currentPort->indRX = 0; - currentPort->checksum ^= c; - currentPort->c_state = HEADER_SIZE; - } - } else if (currentPort->c_state == HEADER_SIZE) { - currentPort->cmdMSP = c; - currentPort->checksum ^= c; - currentPort->c_state = HEADER_CMD; - } else if (currentPort->c_state == HEADER_CMD && currentPort->offset < currentPort->dataSize) { - currentPort->checksum ^= c; - currentPort->inBuf[currentPort->offset++] = c; - } else if (currentPort->c_state == HEADER_CMD && currentPort->offset >= currentPort->dataSize) { - if (currentPort->checksum == c) { - currentPort->c_state = COMMAND_RECEIVED; - } else { - currentPort->c_state = IDLE; - } - } - return true; -} - -static void setCurrentPort(mspPort_t *port) -{ - currentPort = port; - mspSerialPort = currentPort->port; -} - -void mspProcess(void) -{ - uint8_t portIndex; - mspPort_t *candidatePort; - - for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { - candidatePort = &mspPorts[portIndex]; - if (!candidatePort->port) { - continue; - } - - setCurrentPort(candidatePort); - // Big enough to fit a MSP_STATUS in one write. - uint8_t buf[sizeof(bufWriter_t) + 20]; - writer = bufWriterInit(buf, sizeof(buf), - (bufWrite_t)serialWriteBufShim, currentPort->port); - - while (serialRxBytesWaiting(mspSerialPort)) { - - uint8_t c = serialRead(mspSerialPort); - bool consumed = mspProcessReceivedData(c); - - if (!consumed && !ARMING_FLAG(ARMED)) { - evaluateOtherData(mspSerialPort, c); - } - - if (currentPort->c_state == COMMAND_RECEIVED) { - mspProcessReceivedCommand(); - break; // process one command at a time so as not to block. - } - } - - bufWriterFlush(writer); - - if (isRebootScheduled) { - waitForSerialPortToFinishTransmitting(candidatePort->port); - stopMotors(); - handleOneshotFeatureChangeOnRestart(); - systemReset(); - } - } -} diff --git a/src/main/io/servos.h b/src/main/io/servos.h new file mode 100644 index 00000000000..927b067e285 --- /dev/null +++ b/src/main/io/servos.h @@ -0,0 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +typedef struct servoConfig_s { + // PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms) + uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500. + uint16_t servoPwmRate; // The update rate of servo outputs (50-498Hz) +} servoConfig_t; diff --git a/src/main/main.c b/src/main/main.c index 47e7d179e7e..c63e2bff5ad 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -50,6 +50,7 @@ #include "drivers/accgyro.h" #include "drivers/compass.h" #include "drivers/pwm_mapping.h" +#include "drivers/pwm_output.h" #include "drivers/pwm_rx.h" #include "drivers/pwm_output.h" #include "drivers/adc.h" @@ -62,6 +63,10 @@ #include "drivers/gyro_sync.h" #include "drivers/io.h" #include "drivers/exti.h" +#include "drivers/io_pca9685.h" + +#include "fc/fc_tasks.h" +#include "fc/msp_fc.h" #include "rx/rx.h" #include "rx/spektrum.h" @@ -70,16 +75,19 @@ #include "io/serial.h" #include "io/flashfs.h" #include "io/gps.h" -#include "io/escservo.h" +#include "io/motors.h" +#include "io/servos.h" #include "fc/rc_controls.h" #include "io/gimbal.h" #include "io/ledstrip.h" #include "io/display.h" #include "io/asyncfatfs/asyncfatfs.h" -#include "io/serial_msp.h" +#include "io/pwmdriver_i2c.h" #include "io/serial_cli.h" +#include "msp/msp_serial.h" + #include "scheduler/scheduler.h" #include "sensors/sensors.h" @@ -98,14 +106,17 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/failsafe.h" #include "flight/navigation_rewrite.h" #include "fc/runtime_config.h" #include "config/config.h" +#include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -215,10 +226,9 @@ void init(void) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif -#ifdef USE_SERVOS - mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer); -#else mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); +#ifdef USE_SERVOS + servosInit(masterConfig.customServoMixer); #endif drv_pwm_config_t pwm_params; @@ -265,28 +275,46 @@ void init(void) #ifdef USE_SERVOS pwm_params.useServos = isMixerUsingServos(); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); - pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; - pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; + pwm_params.servoCenterPulse = masterConfig.servoConfig.servoCenterPulse; + pwm_params.servoPwmRate = masterConfig.servoConfig.servoPwmRate; #endif - pwm_params.useOneshot = feature(FEATURE_ONESHOT125); - pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; - pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; - if (feature(FEATURE_3D)) + pwm_params.pwmProtocolType = masterConfig.motorConfig.motorPwmProtocol; + pwm_params.useFastPwm = (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_ONESHOT125) || + (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_ONESHOT42) || + (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_MULTISHOT); + pwm_params.motorPwmRate = masterConfig.motorConfig.motorPwmRate; + pwm_params.idlePulse = masterConfig.motorConfig.mincommand; + if (feature(FEATURE_3D)) { pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; - if (pwm_params.motorPwmRate > 500) + } + + if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) { + pwm_params.useFastPwm = false; + featureClear(FEATURE_3D); pwm_params.idlePulse = 0; // brushed motors + } #ifndef SKIP_RX_PWM_PPM pwmRxInit(masterConfig.inputFilteringMode); #endif +#ifdef USE_PMW_SERVO_DRIVER + /* + If external PWM driver is enabled, for example PCA9685, disable internal + servo handling mechanism, since external device will do that + */ + if (feature(FEATURE_PWM_SERVO_DRIVER)) { + pwm_params.useServos = false; + } +#endif + // pwmInit() needs to be called as soon as possible for ESC compatibility reasons pwmInit(&pwm_params); mixerUsePWMIOConfiguration(); - if (!feature(FEATURE_ONESHOT125)) + if (!pwm_params.useFastPwm) motorControlEnable = true; addBootlogEvent2(BOOT_EVENT_PWM_INIT_DONE, BOOT_EVENT_FLAGS_NONE); @@ -453,7 +481,7 @@ void init(void) imuInit(); - mspInit(); + mspSerialInit(mspFcInit()); #ifdef USE_CLI cliInit(&masterConfig.serialConfig); @@ -481,7 +509,7 @@ void init(void) ¤tProfile->rcControlsConfig, &masterConfig.rxConfig, &masterConfig.flight3DConfig, - &masterConfig.escAndServoConfig + &masterConfig.motorConfig ); #endif @@ -572,10 +600,16 @@ void init(void) LED2_ON; #endif +#ifdef USE_PMW_SERVO_DRIVER + pwmDriverInitialize(); +#endif + // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; + fcTasksInit(); + addBootlogEvent2(BOOT_EVENT_SYSTEM_READY, BOOT_EVENT_FLAGS_NONE); systemState |= SYSTEM_STATE_READY; } @@ -597,45 +631,6 @@ void processLoopback(void) { int main(void) { init(); - - /* Setup scheduler */ - schedulerInit(); - - rescheduleTask(TASK_GYROPID, gyro.targetLooptime); - setTaskEnabled(TASK_GYROPID, true); - - setTaskEnabled(TASK_SERIAL, true); -#ifdef BEEPER - setTaskEnabled(TASK_BEEPER, true); -#endif - setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); - setTaskEnabled(TASK_RX, true); -#ifdef GPS - setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); -#endif -#ifdef MAG - setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); -#if (defined(MPU6500_SPI_INSTANCE) || defined(MPU9250_SPI_INSTANCE)) && defined(USE_MAG_AK8963) - // fixme temporary solution for AK6983 via slave I2C on MPU9250 - rescheduleTask(TASK_COMPASS, 1000000 / 40); -#endif -#endif -#ifdef BARO - setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); -#endif -#ifdef SONAR - setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); -#endif -#ifdef DISPLAY - setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); -#endif -#ifdef TELEMETRY - setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); -#endif -#ifdef LED_STRIP - setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); -#endif - while (true) { scheduler(); processLoopback(); diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h new file mode 100644 index 00000000000..b322ee7df6a --- /dev/null +++ b/src/main/msp/msp.h @@ -0,0 +1,37 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include "common/streambuf.h" + +// return positive for ACK, negative on error, zero for no reply +typedef enum { + MSP_RESULT_ACK = 1, + MSP_RESULT_ERROR = -1, + MSP_RESULT_NO_REPLY = 0 +} mspResult_e; + +typedef struct mspPacket_s { + sbuf_t buf; + int16_t cmd; + int16_t result; +} mspPacket_t; + +struct serialPort_s; +typedef void (*mspPostProcessFnPtr)(struct serialPort_s *port); // msp post process function, used for gracefully handling reboots, etc. +typedef mspResult_e (*mspProcessCommandFnPtr)(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn); diff --git a/src/main/io/msp_protocol.h b/src/main/msp/msp_protocol.h similarity index 99% rename from src/main/io/msp_protocol.h rename to src/main/msp/msp_protocol.h index 40f99575465..3bc4545d808 100644 --- a/src/main/io/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -199,8 +199,8 @@ #define MSP_SET_VTX_CONFIG 89 //in message Set vtx settings - betaflight // Betaflight Additional Commands -#define MSP_PID_ADVANCED_CONFIG 90 -#define MSP_SET_PID_ADVANCED_CONFIG 91 +#define MSP_ADVANCED_CONFIG 90 +#define MSP_SET_ADVANCED_CONFIG 91 #define MSP_FILTER_CONFIG 92 #define MSP_SET_FILTER_CONFIG 93 diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c new file mode 100644 index 00000000000..a7ec6289500 --- /dev/null +++ b/src/main/msp/msp_serial.c @@ -0,0 +1,203 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "common/streambuf.h" +#include "common/utils.h" + +#include "drivers/serial.h" + +#include "io/serial.h" + +#include "msp/msp.h" +#include "msp/msp_serial.h" + +static mspProcessCommandFnPtr mspProcessCommandFn; +static mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; + + +static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort) +{ + memset(mspPortToReset, 0, sizeof(mspPort_t)); + + mspPortToReset->port = serialPort; +} + +void mspSerialAllocatePorts(void) +{ + uint8_t portIndex = 0; + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP); + while (portConfig && portIndex < MAX_MSP_PORT_COUNT) { + mspPort_t *mspPort = &mspPorts[portIndex]; + if (mspPort->port) { + portIndex++; + continue; + } + + serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); + if (serialPort) { + resetMspPort(mspPort, serialPort); + portIndex++; + } + + portConfig = findNextSerialPortConfig(FUNCTION_MSP); + } +} + +void mspSerialReleasePortIfAllocated(serialPort_t *serialPort) +{ + for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + mspPort_t *candidateMspPort = &mspPorts[portIndex]; + if (candidateMspPort->port == serialPort) { + closeSerialPort(serialPort); + memset(candidateMspPort, 0, sizeof(mspPort_t)); + } + } +} + +static bool mspSerialProcessReceivedData(mspPort_t * mspPort, uint8_t c) +{ + if (mspPort->c_state == MSP_IDLE) { + if (c == '$') { + mspPort->c_state = MSP_HEADER_START; + } else { + return false; + } + } else if (mspPort->c_state == MSP_HEADER_START) { + mspPort->c_state = (c == 'M') ? MSP_HEADER_M : MSP_IDLE; + } else if (mspPort->c_state == MSP_HEADER_M) { + mspPort->c_state = (c == '<') ? MSP_HEADER_ARROW : MSP_IDLE; + } else if (mspPort->c_state == MSP_HEADER_ARROW) { + if (c > MSP_PORT_INBUF_SIZE) { + mspPort->c_state = MSP_IDLE; + } else { + mspPort->dataSize = c; + mspPort->offset = 0; + mspPort->checksum = 0; + mspPort->checksum ^= c; + mspPort->c_state = MSP_HEADER_SIZE; + } + } else if (mspPort->c_state == MSP_HEADER_SIZE) { + mspPort->cmdMSP = c; + mspPort->checksum ^= c; + mspPort->c_state = MSP_HEADER_CMD; + } else if (mspPort->c_state == MSP_HEADER_CMD && mspPort->offset < mspPort->dataSize) { + mspPort->checksum ^= c; + mspPort->inBuf[mspPort->offset++] = c; + } else if (mspPort->c_state == MSP_HEADER_CMD && mspPort->offset >= mspPort->dataSize) { + if (mspPort->checksum == c) { + mspPort->c_state = MSP_COMMAND_RECEIVED; + } else { + mspPort->c_state = MSP_IDLE; + } + } + return true; +} + +static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int len) +{ + while (len-- > 0) { + checksum ^= *data++; + } + return checksum; +} + +static void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet) +{ + serialBeginWrite(msp->port); + const int len = sbufBytesRemaining(&packet->buf); + const uint8_t hdr[5] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', len, packet->cmd}; + serialWriteBuf(msp->port, hdr, sizeof(hdr)); + uint8_t checksum = mspSerialChecksumBuf(0, hdr + 3, 2); // checksum starts from len field + if (len > 0) { + serialWriteBuf(msp->port, sbufPtr(&packet->buf), len); + checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), len); + } + serialWrite(msp->port, checksum); + serialEndWrite(msp->port); +} + +static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp) +{ + static uint8_t outBuf[MSP_PORT_OUTBUF_SIZE]; + + mspPacket_t reply = { + .buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), }, + .cmd = -1, + .result = 0, + }; + uint8_t *outBufHead = reply.buf.ptr; + + mspPacket_t command = { + .buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, }, + .cmd = msp->cmdMSP, + .result = 0, + }; + + mspPostProcessFnPtr mspPostProcessFn = NULL; + const mspResult_e status = mspProcessCommandFn(&command, &reply, &mspPostProcessFn); + + if (status != MSP_RESULT_NO_REPLY) { + sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction + mspSerialEncode(msp, &reply); + } + + msp->c_state = MSP_IDLE; + return mspPostProcessFn; +} + +void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData) +{ + for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + mspPort_t * const mspPort = &mspPorts[portIndex]; + if (!mspPort->port) { + continue; + } + mspPostProcessFnPtr mspPostProcessFn = NULL; + while (serialRxBytesWaiting(mspPort->port)) { + + const uint8_t c = serialRead(mspPort->port); + const bool consumed = mspSerialProcessReceivedData(mspPort, c); + + if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) { + serialEvaluateNonMspData(mspPort->port, c); + } + + if (mspPort->c_state == MSP_COMMAND_RECEIVED) { + mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort); + break; // process one command at a time so as not to block. + } + } + if (mspPostProcessFn) { + waitForSerialPortToFinishTransmitting(mspPort->port); + mspPostProcessFn(mspPort->port); + } + } +} + +void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse) +{ + mspProcessCommandFn = mspProcessCommandFnToUse; + memset(mspPorts, 0, sizeof(mspPorts)); + mspSerialAllocatePorts(); +} + diff --git a/src/main/io/serial_msp.h b/src/main/msp/msp_serial.h similarity index 64% rename from src/main/io/serial_msp.h rename to src/main/msp/msp_serial.h index 42af16c9148..03a9d938570 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/msp/msp_serial.h @@ -17,36 +17,42 @@ #pragma once -#include "io/serial.h" -#include "drivers/serial.h" +#include "msp/msp.h" // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports. #define MAX_MSP_PORT_COUNT 2 typedef enum { - IDLE, - HEADER_START, - HEADER_M, - HEADER_ARROW, - HEADER_SIZE, - HEADER_CMD, - COMMAND_RECEIVED + MSP_IDLE, + MSP_HEADER_START, + MSP_HEADER_M, + MSP_HEADER_ARROW, + MSP_HEADER_SIZE, + MSP_HEADER_CMD, + MSP_COMMAND_RECEIVED } mspState_e; +typedef enum { + MSP_EVALUATE_NON_MSP_DATA, + MSP_SKIP_NON_MSP_DATA +} mspEvaluateNonMspData_e; + #define MSP_PORT_INBUF_SIZE 64 +#define MSP_PORT_OUTBUF_SIZE 256 +struct serialPort_s; typedef struct mspPort_s { - serialPort_t *port; // null when port unused. + struct serialPort_s *port; // null when port unused. uint8_t offset; uint8_t dataSize; uint8_t checksum; - uint8_t indRX; - uint8_t inBuf[MSP_PORT_INBUF_SIZE]; - mspState_e c_state; uint8_t cmdMSP; + mspState_e c_state; + uint8_t inBuf[MSP_PORT_INBUF_SIZE]; } mspPort_t; -void mspInit(void); -void mspProcess(void); -void mspAllocateSerialPorts(void); -void mspReleasePortIfAllocated(serialPort_t *serialPort); \ No newline at end of file + +void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFn); +void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData); +void mspSerialAllocatePorts(void); +void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index fab05bce6c2..49c4e64ea1a 100755 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -49,7 +49,7 @@ static bool ibusFrameDone = false; static uint32_t ibusChannelData[IBUS_MAX_CHANNEL]; static void ibusDataReceive(uint16_t c); -static uint16_t ibusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); bool ibusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { @@ -134,7 +134,7 @@ uint8_t ibusFrameStatus(void) return frameStatus; } -static uint16_t ibusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { UNUSED(rxRuntimeConfig); return ibusChannelData[chan]; diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index b5b59814c37..d56636866cb 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -208,7 +208,7 @@ static uint8_t jetiExBusRequestFrame[EXBUS_MAX_REQUEST_FRAME_SIZE]; static uint16_t jetiExBusChannelData[JETIEXBUS_CHANNEL_COUNT]; static void jetiExBusDataReceive(uint16_t c); -static uint16_t jetiExBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t jetiExBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); static void jetiExBusFrameReset(); @@ -416,7 +416,7 @@ uint8_t jetiExBusFrameStatus() } -static uint16_t jetiExBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t jetiExBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { if (chan >= rxRuntimeConfig->channelCount) return 0; diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index 66ad3f85f44..94b64049d78 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -35,7 +35,7 @@ static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static bool rxMspFrameDone = false; -static uint16_t rxMspReadRawRC(rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) +static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) { UNUSED(rxRuntimeConfigPtr); return mspFrame[chan]; diff --git a/src/main/rx/nrf24_cx10.c b/src/main/rx/nrf24_cx10.c index e37609a9764..2434d1259c2 100644 --- a/src/main/rx/nrf24_cx10.c +++ b/src/main/rx/nrf24_cx10.c @@ -32,7 +32,7 @@ #include "drivers/rx_xn297.h" #include "drivers/system.h" -#include "rx/nrf24.h" +#include "rx/rx_spi.h" #include "rx/nrf24_cx10.h" /* @@ -71,7 +71,7 @@ enum { #define FLAG_VIDEO 0x02 #define FLAG_PICTURE 0x04 -static nrf24_protocol_t cx10Protocol; +static rx_spi_protocol_e cx10Protocol; typedef enum { STATE_BIND = 0, @@ -129,10 +129,10 @@ STATIC_UNIT_TESTED uint16_t cx10ConvertToPwmUnsigned(const uint8_t *pVal) void cx10Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload) { const uint8_t offset = (cx10Protocol == NRF24RX_CX10) ? 0 : 4; - rcData[NRF24_ROLL] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - cx10ConvertToPwmUnsigned(&payload[5 + offset]); // aileron - rcData[NRF24_PITCH] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - cx10ConvertToPwmUnsigned(&payload[7 + offset]); // elevator - rcData[NRF24_THROTTLE] = cx10ConvertToPwmUnsigned(&payload[9 + offset]); // throttle - rcData[NRF24_YAW] = cx10ConvertToPwmUnsigned(&payload[11 + offset]); // rudder + rcData[RC_SPI_ROLL] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - cx10ConvertToPwmUnsigned(&payload[5 + offset]); // aileron + rcData[RC_SPI_PITCH] = (PWM_RANGE_MAX + PWM_RANGE_MIN) - cx10ConvertToPwmUnsigned(&payload[7 + offset]); // elevator + rcData[RC_SPI_THROTTLE] = cx10ConvertToPwmUnsigned(&payload[9 + offset]); // throttle + rcData[RC_SPI_YAW] = cx10ConvertToPwmUnsigned(&payload[11 + offset]); // rudder const uint8_t flags1 = payload[13 + offset]; const uint8_t rate = flags1 & FLAG_MODE_MASK; // takes values 0, 1, 2 if (rate == RATE_LOW) { @@ -193,12 +193,12 @@ static bool cx10ReadPayloadIfAvailable(uint8_t *payload) /* * This is called periodically by the scheduler. - * Returns NRF24_RECEIVED_DATA if a data packet was received. + * Returns RX_SPI_RECEIVED_DATA if a data packet was received. */ -nrf24_received_t cx10Nrf24DataReceived(uint8_t *payload) +rx_spi_received_e cx10Nrf24DataReceived(uint8_t *payload) { static uint8_t ackCount; - nrf24_received_t ret = NRF24_RECEIVED_NONE; + rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; int totalDelayUs; uint32_t timeNowUs; @@ -209,7 +209,7 @@ nrf24_received_t cx10Nrf24DataReceived(uint8_t *payload) if (bindPacket) { // set the hopping channels as determined by the txId received in the bind packet cx10SetHoppingChannels(txId); - ret = NRF24_RECEIVED_BIND; + ret = RX_SPI_RECEIVED_BIND; protocolState = STATE_ACK; ackCount = 0; } @@ -259,7 +259,7 @@ nrf24_received_t cx10Nrf24DataReceived(uint8_t *payload) if (cx10ReadPayloadIfAvailable(payload)) { cx10HopToNextChannel(); timeOfLastHop = timeNowUs; - ret = NRF24_RECEIVED_DATA; + ret = RX_SPI_RECEIVED_DATA; } if (timeNowUs > timeOfLastHop + hopTimeout) { cx10HopToNextChannel(); @@ -269,7 +269,7 @@ nrf24_received_t cx10Nrf24DataReceived(uint8_t *payload) return ret; } -static void cx10Nrf24Setup(nrf24_protocol_t protocol) +static void cx10Nrf24Setup(rx_spi_protocol_e protocol) { cx10Protocol = protocol; protocolState = STATE_BIND; @@ -295,7 +295,7 @@ static void cx10Nrf24Setup(nrf24_protocol_t protocol) void cx10Nrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT; - cx10Nrf24Setup((nrf24_protocol_t)rxConfig->nrf24rx_protocol); + cx10Nrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol); } #endif diff --git a/src/main/rx/nrf24_cx10.h b/src/main/rx/nrf24_cx10.h index 0115326e856..8af15473333 100644 --- a/src/main/rx/nrf24_cx10.h +++ b/src/main/rx/nrf24_cx10.h @@ -24,5 +24,5 @@ struct rxConfig_s; struct rxRuntimeConfig_s; void cx10Nrf24Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); void cx10Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload); -nrf24_received_t cx10Nrf24DataReceived(uint8_t *payload); +rx_spi_received_e cx10Nrf24DataReceived(uint8_t *payload); diff --git a/src/main/rx/nrf24_h8_3d.c b/src/main/rx/nrf24_h8_3d.c index 80dab7efa5d..9ffa1dcf47f 100644 --- a/src/main/rx/nrf24_h8_3d.c +++ b/src/main/rx/nrf24_h8_3d.c @@ -33,7 +33,7 @@ #include "drivers/rx_xn297.h" #include "drivers/system.h" -#include "rx/nrf24.h" +#include "rx/rx_spi.h" #include "rx/nrf24_h8_3d.h" @@ -85,7 +85,7 @@ STATIC_UNIT_TESTED uint8_t payloadSize; STATIC_UNIT_TESTED uint8_t rxTxAddrXN297[RX_TX_ADDR_LEN] = {0x41, 0xbd, 0x42, 0xd4, 0xc2}; // converted XN297 address #define TX_ID_LEN 4 STATIC_UNIT_TESTED uint8_t txId[TX_ID_LEN]; -uint32_t *nrf24rxIdPtr; +uint32_t *rxSpiIdPtr; // radio channels for frequency hopping #define H8_3D_RF_CHANNEL_COUNT 4 @@ -112,9 +112,9 @@ STATIC_UNIT_TESTED bool h8_3dCheckBindPacket(const uint8_t *payload) txId[1] = payload[2]; txId[2] = payload[3]; txId[3] = payload[4]; - if (nrf24rxIdPtr != NULL && *nrf24rxIdPtr == 0) { + if (rxSpiIdPtr != NULL && *rxSpiIdPtr == 0) { // copy the txId so it can be saved - memcpy(nrf24rxIdPtr, txId, sizeof(uint32_t)); + memcpy(rxSpiIdPtr, txId, sizeof(uint32_t)); } } } @@ -133,11 +133,11 @@ STATIC_UNIT_TESTED uint16_t h8_3dConvertToPwm(uint8_t val, int16_t _min, int16_t void h8_3dNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload) { - rcData[NRF24_ROLL] = h8_3dConvertToPwm(payload[12], 0xbb, 0x43); // aileron - rcData[NRF24_PITCH] = h8_3dConvertToPwm(payload[11], 0x43, 0xbb); // elevator - rcData[NRF24_THROTTLE] = h8_3dConvertToPwm(payload[9], 0, 0xff); // throttle + rcData[RC_SPI_ROLL] = h8_3dConvertToPwm(payload[12], 0xbb, 0x43); // aileron + rcData[RC_SPI_PITCH] = h8_3dConvertToPwm(payload[11], 0x43, 0xbb); // elevator + rcData[RC_SPI_THROTTLE] = h8_3dConvertToPwm(payload[9], 0, 0xff); // throttle const int8_t yawByte = payload[10]; // rudder - rcData[NRF24_YAW] = yawByte >= 0 ? h8_3dConvertToPwm(yawByte, -0x3c, 0x3c) : h8_3dConvertToPwm(yawByte, 0xbc, 0x44); + rcData[RC_SPI_YAW] = yawByte >= 0 ? h8_3dConvertToPwm(yawByte, -0x3c, 0x3c) : h8_3dConvertToPwm(yawByte, 0xbc, 0x44); const uint8_t flags = payload[17]; const uint8_t flags2 = payload[18]; @@ -156,15 +156,15 @@ void h8_3dNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload) rcData[RC_CHANNEL_RTH] = flags & FLAG_RTH ? PWM_RANGE_MAX : PWM_RANGE_MIN; if (flags2 & FLAG_CAMERA_UP) { - rcData[NRF24_AUX7] = PWM_RANGE_MAX; + rcData[RC_SPI_AUX7] = PWM_RANGE_MAX; } else if (flags2 & FLAG_CAMERA_DOWN) { - rcData[NRF24_AUX7] = PWM_RANGE_MIN; + rcData[RC_SPI_AUX7] = PWM_RANGE_MIN; } else { - rcData[NRF24_AUX7] = PWM_RANGE_MIDDLE; + rcData[RC_SPI_AUX7] = PWM_RANGE_MIDDLE; } - rcData[NRF24_AUX8] = h8_3dConvertToPwm(payload[14], 0x10, 0x30); - rcData[NRF24_AUX9] = h8_3dConvertToPwm(payload[15], 0x30, 0x10); - rcData[NRF24_AUX10] = h8_3dConvertToPwm(payload[16], 0x10, 0x30); + rcData[RC_SPI_AUX8] = h8_3dConvertToPwm(payload[14], 0x10, 0x30); + rcData[RC_SPI_AUX9] = h8_3dConvertToPwm(payload[15], 0x30, 0x10); + rcData[RC_SPI_AUX10] = h8_3dConvertToPwm(payload[16], 0x10, 0x30); } static void h8_3dHopToNextChannel(void) @@ -216,9 +216,9 @@ static bool h8_3dCrcOK(uint16_t crc, const uint8_t *payload) * This is called periodically by the scheduler. * Returns NRF24L01_RECEIVED_DATA if a data packet was received. */ -nrf24_received_t h8_3dNrf24DataReceived(uint8_t *payload) +rx_spi_received_e h8_3dNrf24DataReceived(uint8_t *payload) { - nrf24_received_t ret = NRF24_RECEIVED_NONE; + rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; bool payloadReceived = false; if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize + CRC_LEN)) { const uint16_t crc = XN297_UnscramblePayload(payload, payloadSize, rxTxAddrXN297); @@ -231,26 +231,26 @@ nrf24_received_t h8_3dNrf24DataReceived(uint8_t *payload) if (payloadReceived) { const bool bindPacket = h8_3dCheckBindPacket(payload); if (bindPacket) { - ret = NRF24_RECEIVED_BIND; + ret = RX_SPI_RECEIVED_BIND; h8_3dSetBound(txId); } } break; case STATE_DATA: if (payloadReceived) { - ret = NRF24_RECEIVED_DATA; + ret = RX_SPI_RECEIVED_DATA; } break; } const uint32_t timeNowUs = micros(); - if ((ret == NRF24_RECEIVED_DATA) || (timeNowUs > timeOfLastHop + hopTimeout)) { + if ((ret == RX_SPI_RECEIVED_DATA) || (timeNowUs > timeOfLastHop + hopTimeout)) { h8_3dHopToNextChannel(); timeOfLastHop = timeNowUs; } return ret; } -static void h8_3dNrf24Setup(nrf24_protocol_t protocol, const uint32_t *nrf24rx_id) +static void h8_3dNrf24Setup(rx_spi_protocol_e protocol, const uint32_t *rxSpiId) { UNUSED(protocol); protocolState = STATE_BIND; @@ -261,12 +261,12 @@ static void h8_3dNrf24Setup(nrf24_protocol_t protocol, const uint32_t *nrf24rx_i NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, NRF24L01_06_RF_SETUP_RF_DR_1Mbps | NRF24L01_06_RF_SETUP_RF_PWR_n12dbm); // RX_ADDR for pipes P1-P5 are left at default values NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rxTxAddrXN297, RX_TX_ADDR_LEN); - nrf24rxIdPtr = (uint32_t*)nrf24rx_id; - if (nrf24rx_id == NULL || *nrf24rx_id == 0) { + rxSpiIdPtr = (uint32_t*)rxSpiId; + if (rxSpiId == NULL || *rxSpiId == 0) { h8_3dRfChannelIndex = H8_3D_RF_BIND_CHANNEL_START; NRF24L01_SetChannel(H8_3D_RF_BIND_CHANNEL_START); } else { - h8_3dSetBound((uint8_t*)nrf24rx_id); + h8_3dSetBound((uint8_t*)rxSpiId); } payloadSize = H8_3D_PROTOCOL_PAYLOAD_SIZE; @@ -278,6 +278,6 @@ static void h8_3dNrf24Setup(nrf24_protocol_t protocol, const uint32_t *nrf24rx_i void h8_3dNrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT; - h8_3dNrf24Setup((nrf24_protocol_t)rxConfig->nrf24rx_protocol, &rxConfig->nrf24rx_id); + h8_3dNrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol, &rxConfig->rx_spi_id); } #endif diff --git a/src/main/rx/nrf24_h8_3d.h b/src/main/rx/nrf24_h8_3d.h index 2f0462d3616..306259f9aa7 100644 --- a/src/main/rx/nrf24_h8_3d.h +++ b/src/main/rx/nrf24_h8_3d.h @@ -24,4 +24,4 @@ struct rxConfig_s; struct rxRuntimeConfig_s; void h8_3dNrf24Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); void h8_3dNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload); -nrf24_received_t h8_3dNrf24DataReceived(uint8_t *payload); +rx_spi_received_e h8_3dNrf24DataReceived(uint8_t *payload); diff --git a/src/main/rx/nrf24_inav.c b/src/main/rx/nrf24_inav.c index 3f9df15c3d7..5b1dd267f5b 100644 --- a/src/main/rx/nrf24_inav.c +++ b/src/main/rx/nrf24_inav.c @@ -29,7 +29,7 @@ #include "drivers/rx_nrf24l01.h" #include "drivers/system.h" -#include "rx/nrf24.h" +#include "rx/rx_spi.h" #include "rx/nrf24_inav.h" #include "telemetry/ltm.h" @@ -41,6 +41,7 @@ #define USE_AUTO_ACKKNOWLEDGEMENT +#define USE_WHITENING /* * iNav Protocol @@ -78,6 +79,12 @@ * Channels AUX11 and AUX12 are analog channels, values 1000 to 2000 with resolution of 4 (8-bit channels) * c) For 18 byte payload there are 18 channels, the first 16 channelsar are as for 16 byte payload, and then there are two * additional channels: AUX13 and AUX14 both with resolution of 4 (8-bit channels) + * + * Intercepting packets + * + * Packets are designed to be intercepted by a second receiver. So a second receiver could, for example intercept the + * ACK packets and use the GPS telemetry to display the position of the aircraft on a map, or to control a camera gimbal + * to point at the aircraft. */ #define RC_CHANNEL_COUNT 16 // standard variant of the protocol has 16 RC channels @@ -105,6 +112,15 @@ typedef enum { STATIC_UNIT_TESTED protocol_state_t protocolState; STATIC_UNIT_TESTED uint8_t ackPayload[NRF24L01_MAX_PAYLOAD_SIZE]; +#define BIND_PAYLOAD_SIZE 16 +#define BIND_PAYLOAD0 0xad // 10101101 +#define BIND_PAYLOAD1 0xc9 // 11001001 +#define BIND_ACK_PAYLOAD0 0x95 // 10010101 +#define BIND_ACK_PAYLOAD1 0xa9 // 10101001 +#define TELEMETRY_ACK_PAYLOAD0 0x5a // 01011010 +// TELEMETRY_ACK_PAYLOAD1 is sequence count +#define DATA_PAYLOAD0 0x00 +#define DATA_PAYLOAD1 0x00 #define INAV_PROTOCOL_PAYLOAD_SIZE_MIN 8 #define INAV_PROTOCOL_PAYLOAD_SIZE_DEFAULT 16 @@ -115,7 +131,7 @@ uint8_t receivedPowerSnapshot; #define RX_TX_ADDR_LEN 5 // set rxTxAddr to the bind address STATIC_UNIT_TESTED uint8_t rxTxAddr[RX_TX_ADDR_LEN] = {0x4b,0x5c,0x6d,0x7e,0x8f}; -uint32_t *nrf24rxIdPtr; +uint32_t *rxSpiIdPtr; #define RX_TX_ADDR_4 0xD2 // rxTxAddr[4] always set to this value // radio channels for frequency hopping @@ -130,10 +146,30 @@ STATIC_UNIT_TESTED uint8_t inavRfChannels[INAV_RF_CHANNEL_COUNT_MAX]; static uint32_t timeOfLastHop; static const uint32_t hopTimeout = 5000; // 5ms +static void whitenPayload(uint8_t *payload, uint8_t len) +{ +#ifdef USE_WHITENING + uint8_t whitenCoeff = 0x6b; // 01101011 + while (len--) { + for (uint8_t m = 1; m; m <<= 1) { + if (whitenCoeff & 0x80) { + whitenCoeff ^= 0x11; + (*payload) ^= m; + } + whitenCoeff <<= 1; + } + payload++; + } +#else + UNUSED(payload); + UNUSED(len); +#endif +} + STATIC_UNIT_TESTED bool inavCheckBindPacket(const uint8_t *payload) { bool bindPacket = false; - if (payload[0] == 0xae && payload[1] == 0xc9) { + if (payload[0] == BIND_PAYLOAD0 && payload[1] == BIND_PAYLOAD1) { bindPacket = true; if (protocolState ==STATE_BIND) { rxTxAddr[0] = payload[2]; @@ -145,9 +181,9 @@ STATIC_UNIT_TESTED bool inavCheckBindPacket(const uint8_t *payload) if (inavRfChannelHoppingCount > INAV_RF_CHANNEL_COUNT_MAX) { inavRfChannelHoppingCount = INAV_RF_CHANNEL_COUNT_MAX; }*/ - if (nrf24rxIdPtr != NULL && *nrf24rxIdPtr == 0) { + if (rxSpiIdPtr != NULL && *rxSpiIdPtr == 0) { // copy the rxTxAddr so it can be saved - memcpy(nrf24rxIdPtr, rxTxAddr, sizeof(uint32_t)); + memcpy(rxSpiIdPtr, rxTxAddr, sizeof(uint32_t)); } } } @@ -160,18 +196,18 @@ void inavNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload) // payload[0] and payload[1] are zero in DATA state // the AETR channels have 10 bit resolution uint8_t lowBits = payload[6]; // least significant bits for AETR - rcData[NRF24_ROLL] = PWM_RANGE_MIN + ((payload[2] << 2) | (lowBits & 0x03)); // Aileron + rcData[RC_SPI_ROLL] = PWM_RANGE_MIN + ((payload[2] << 2) | (lowBits & 0x03)); // Aileron lowBits >>= 2; - rcData[NRF24_PITCH] = PWM_RANGE_MIN + ((payload[3] << 2) | (lowBits & 0x03)); // Elevator + rcData[RC_SPI_PITCH] = PWM_RANGE_MIN + ((payload[3] << 2) | (lowBits & 0x03)); // Elevator lowBits >>= 2; - rcData[NRF24_THROTTLE] = PWM_RANGE_MIN + ((payload[4] << 2) | (lowBits & 0x03)); // Throttle + rcData[RC_SPI_THROTTLE] = PWM_RANGE_MIN + ((payload[4] << 2) | (lowBits & 0x03)); // Throttle lowBits >>= 2; - rcData[NRF24_YAW] = PWM_RANGE_MIN + ((payload[5] << 2) | (lowBits & 0x03)); // Rudder + rcData[RC_SPI_YAW] = PWM_RANGE_MIN + ((payload[5] << 2) | (lowBits & 0x03)); // Rudder if (payloadSize == INAV_PROTOCOL_PAYLOAD_SIZE_MIN) { // small payload variant of protocol, supports 6 channels - rcData[NRF24_AUX1] = PWM_RANGE_MIN + (payload[7] << 2); - rcData[NRF24_AUX2] = PWM_RANGE_MIN + (payload[1] << 2); + rcData[RC_SPI_AUX1] = PWM_RANGE_MIN + (payload[7] << 2); + rcData[RC_SPI_AUX2] = PWM_RANGE_MIN + (payload[1] << 2); } else { // channel AUX1 is used for rate, as per the deviation convention const uint8_t rate = payload[7]; @@ -194,24 +230,24 @@ void inavNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload) // channels AUX7 to AUX10 have 10 bit resolution lowBits = payload[13]; // least significant bits for AUX7 to AUX10 - rcData[NRF24_AUX7] = PWM_RANGE_MIN + ((payload[9] << 2) | (lowBits & 0x03)); + rcData[RC_SPI_AUX7] = PWM_RANGE_MIN + ((payload[9] << 2) | (lowBits & 0x03)); lowBits >>= 2; - rcData[NRF24_AUX8] = PWM_RANGE_MIN + ((payload[10] << 2) | (lowBits & 0x03)); + rcData[RC_SPI_AUX8] = PWM_RANGE_MIN + ((payload[10] << 2) | (lowBits & 0x03)); lowBits >>= 2; - rcData[NRF24_AUX9] = PWM_RANGE_MIN + ((payload[11] << 2) | (lowBits & 0x03)); + rcData[RC_SPI_AUX9] = PWM_RANGE_MIN + ((payload[11] << 2) | (lowBits & 0x03)); lowBits >>= 2; - rcData[NRF24_AUX10] = PWM_RANGE_MIN + ((payload[12] << 2) | (lowBits & 0x03)); + rcData[RC_SPI_AUX10] = PWM_RANGE_MIN + ((payload[12] << 2) | (lowBits & 0x03)); lowBits >>= 2; // channels AUX11 and AUX12 have 8 bit resolution - rcData[NRF24_AUX11] = PWM_RANGE_MIN + (payload[14] << 2); - rcData[NRF24_AUX12] = PWM_RANGE_MIN + (payload[15] << 2); + rcData[RC_SPI_AUX11] = PWM_RANGE_MIN + (payload[14] << 2); + rcData[RC_SPI_AUX12] = PWM_RANGE_MIN + (payload[15] << 2); } if (payloadSize == INAV_PROTOCOL_PAYLOAD_SIZE_MAX) { // large payload variant of protocol // channels AUX13 to AUX16 have 8 bit resolution - rcData[NRF24_AUX13] = PWM_RANGE_MIN + (payload[16] << 2); - rcData[NRF24_AUX14] = PWM_RANGE_MIN + (payload[17] << 2); + rcData[RC_SPI_AUX13] = PWM_RANGE_MIN + (payload[16] << 2); + rcData[RC_SPI_AUX14] = PWM_RANGE_MIN + (payload[17] << 2); } } @@ -260,10 +296,10 @@ static void inavSetBound(void) #endif } -static void writeAckPayload(const uint8_t *data, uint8_t length) +static void writeAckPayload(uint8_t *data, uint8_t length) { - NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_STATUS_TX_DS) | BV(NRF24L01_07_STATUS_MAX_RT)); - NRF24L01_FlushTx(); + whitenPayload(data, length); + NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_STATUS_MAX_RT)); NRF24L01_WriteAckPayload(data, length, NRF24L01_PIPE0); } @@ -274,17 +310,19 @@ static void writeTelemetryAckPayload(void) static uint8_t sequenceNumber = 0; static ltm_frame_e ltmFrameType = LTM_FRAME_START; - ackPayload[0] = sequenceNumber++; - const int ackPayloadSize = getLtmFrame(&ackPayload[1], ltmFrameType) + 1; + ackPayload[0] = TELEMETRY_ACK_PAYLOAD0; + ackPayload[1] = sequenceNumber++; + const int ackPayloadSize = getLtmFrame(&ackPayload[2], ltmFrameType) + 2; + ++ltmFrameType; if (ltmFrameType > LTM_FRAME_COUNT) { ltmFrameType = LTM_FRAME_START; } writeAckPayload(ackPayload, ackPayloadSize); #ifdef DEBUG_NRF24_INAV - debug[1] = ackPayload[0]; // sequenceNumber - debug[2] = ackPayload[1]; // frame type, 'A', 'S' etc - debug[3] = ackPayload[2]; // pitch for AFrame + debug[1] = ackPayload[1]; // sequenceNumber + debug[2] = ackPayload[2]; // frame type, 'A', 'S' etc + debug[3] = ackPayload[3]; // pitch for AFrame #endif #endif } @@ -292,23 +330,24 @@ static void writeTelemetryAckPayload(void) static void writeBindAckPayload(uint8_t *payload) { #ifdef USE_AUTO_ACKKNOWLEDGEMENT + memcpy(ackPayload, payload, BIND_PAYLOAD_SIZE); // send back the payload with the first two bytes set to zero as the ack - payload[0] = 0; - payload[1] = 0; + ackPayload[0] = BIND_ACK_PAYLOAD0; + ackPayload[1] = BIND_ACK_PAYLOAD1; // respond to request for rfChannelCount; - payload[7] = inavRfChannelHoppingCount; + ackPayload[7] = inavRfChannelHoppingCount; // respond to request for payloadSize switch (payloadSize) { case INAV_PROTOCOL_PAYLOAD_SIZE_MIN: case INAV_PROTOCOL_PAYLOAD_SIZE_DEFAULT: case INAV_PROTOCOL_PAYLOAD_SIZE_MAX: - payload[8] = payloadSize; + ackPayload[8] = payloadSize; break; default: - payload[8] = INAV_PROTOCOL_PAYLOAD_SIZE_DEFAULT; + ackPayload[8] = INAV_PROTOCOL_PAYLOAD_SIZE_DEFAULT; break; } - writeAckPayload(payload, payloadSize); + writeAckPayload(ackPayload, BIND_PAYLOAD_SIZE); #else UNUSED(payload); #endif @@ -316,18 +355,19 @@ static void writeBindAckPayload(uint8_t *payload) /* * This is called periodically by the scheduler. - * Returns NRF24L01_RECEIVED_DATA if a data packet was received. + * Returns RX_SPI_RECEIVED_DATA if a data packet was received. */ -nrf24_received_t inavNrf24DataReceived(uint8_t *payload) +rx_spi_received_e inavNrf24DataReceived(uint8_t *payload) { - nrf24_received_t ret = NRF24_RECEIVED_NONE; + rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; uint32_t timeNowUs; switch (protocolState) { case STATE_BIND: if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) { + whitenPayload(payload, payloadSize); const bool bindPacket = inavCheckBindPacket(payload); if (bindPacket) { - ret = NRF24_RECEIVED_BIND; + ret = RX_SPI_RECEIVED_BIND; writeBindAckPayload(payload); // got a bind packet, so set the hopping channels and the rxTxAddr and start listening for data inavSetBound(); @@ -338,18 +378,19 @@ nrf24_received_t inavNrf24DataReceived(uint8_t *payload) timeNowUs = micros(); // read the payload, processing of payload is deferred if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) { + whitenPayload(payload, payloadSize); receivedPowerSnapshot = NRF24L01_ReadReg(NRF24L01_09_RPD); // set to 1 if received power > -64dBm const bool bindPacket = inavCheckBindPacket(payload); if (bindPacket) { // transmitter may still continue to transmit bind packets after we have switched to data mode - ret = NRF24_RECEIVED_BIND; + ret = RX_SPI_RECEIVED_BIND; writeBindAckPayload(payload); } else { - ret = NRF24_RECEIVED_DATA; + ret = RX_SPI_RECEIVED_DATA; writeTelemetryAckPayload(); } } - if ((ret == NRF24_RECEIVED_DATA) || (timeNowUs > timeOfLastHop + hopTimeout)) { + if ((ret == RX_SPI_RECEIVED_DATA) || (timeNowUs > timeOfLastHop + hopTimeout)) { inavHopToNextChannel(); timeOfLastHop = timeNowUs; } @@ -358,20 +399,23 @@ nrf24_received_t inavNrf24DataReceived(uint8_t *payload) return ret; } -static void inavNrf24Setup(nrf24_protocol_t protocol, const uint32_t *nrf24rx_id, int rfChannelHoppingCount) +static void inavNrf24Setup(rx_spi_protocol_e protocol, const uint32_t *rxSpiId, int rfChannelHoppingCount) { UNUSED(protocol); UNUSED(rfChannelHoppingCount); - NRF24L01_Initialize(BV(NRF24L01_00_CONFIG_EN_CRC) | BV(NRF24L01_00_CONFIG_CRCO)); // sets PWR_UP, EN_CRC, CRCO - 2 byte CRC + // sets PWR_UP, EN_CRC, CRCO - 2 byte CRC, only get IRQ pin interrupt on RX_DR + NRF24L01_Initialize(BV(NRF24L01_00_CONFIG_EN_CRC) | BV(NRF24L01_00_CONFIG_CRCO) | BV(NRF24L01_00_CONFIG_MASK_MAX_RT) | BV(NRF24L01_00_CONFIG_MASK_TX_DS)); #ifdef USE_AUTO_ACKKNOWLEDGEMENT NRF24L01_WriteReg(NRF24L01_01_EN_AA, BV(NRF24L01_01_EN_AA_ENAA_P0)); // auto acknowledgment on P0 NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, BV(NRF24L01_02_EN_RXADDR_ERX_P0)); NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, NRF24L01_03_SETUP_AW_5BYTES); // 5-byte RX/TX address NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0); + NRF24L01_Activate(0x73); // activate R_RX_PL_WID, W_ACK_PAYLOAD, and W_TX_PAYLOAD_NOACK registers NRF24L01_WriteReg(NRF24L01_1D_FEATURE, BV(NRF24L01_1D_FEATURE_EN_ACK_PAY) | BV(NRF24L01_1D_FEATURE_EN_DPL)); NRF24L01_WriteReg(NRF24L01_1C_DYNPD, BV(NRF24L01_1C_DYNPD_DPL_P0)); // enable dynamic payload length on P0 + //NRF24L01_Activate(0x73); // deactivate R_RX_PL_WID, W_ACK_PAYLOAD, and W_TX_PAYLOAD_NOACK registers NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rxTxAddr, RX_TX_ADDR_LEN); #else @@ -385,19 +429,19 @@ static void inavNrf24Setup(nrf24_protocol_t protocol, const uint32_t *nrf24rx_id #ifdef USE_BIND_ADDRESS_FOR_DATA_STATE inavSetBound(); - UNUSED(nrf24rx_id); + UNUSED(rxSpiId); #else - nrf24rx_id = NULL; // !!TODO remove this once configurator supports setting rx_id - if (nrf24rx_id == NULL || *nrf24rx_id == 0) { - nrf24rxIdPtr = NULL; + rxSpiId = NULL; // !!TODO remove this once configurator supports setting rx_id + if (rxSpiId == NULL || *rxSpiId == 0) { + rxSpiIdPtr = NULL; protocolState = STATE_BIND; inavRfChannelCount = 1; inavRfChannelIndex = 0; NRF24L01_SetChannel(INAV_RF_BIND_CHANNEL); } else { - nrf24rxIdPtr = (uint32_t*)nrf24rx_id; + rxSpiIdPtr = (uint32_t*)rxSpiId; // use the rxTxAddr provided and go straight into DATA_STATE - memcpy(rxTxAddr, nrf24rx_id, sizeof(uint32_t)); + memcpy(rxTxAddr, rxSpiId, sizeof(uint32_t)); rxTxAddr[4] = RX_TX_ADDR_4; inavSetBound(); } @@ -411,7 +455,7 @@ static void inavNrf24Setup(nrf24_protocol_t protocol, const uint32_t *nrf24rx_id void inavNrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT_MAX; - inavNrf24Setup((nrf24_protocol_t)rxConfig->nrf24rx_protocol, &rxConfig->nrf24rx_id, rxConfig->nrf24rx_rf_channel_count); + inavNrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol, &rxConfig->rx_spi_id, rxConfig->rx_spi_rf_channel_count); } #endif diff --git a/src/main/rx/nrf24_inav.h b/src/main/rx/nrf24_inav.h index e79b39a2a76..f499f12a3ad 100644 --- a/src/main/rx/nrf24_inav.h +++ b/src/main/rx/nrf24_inav.h @@ -24,5 +24,5 @@ struct rxConfig_s; struct rxRuntimeConfig_s; void inavNrf24Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); void inavNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload); -nrf24_received_t inavNrf24DataReceived(uint8_t *payload); +rx_spi_received_e inavNrf24DataReceived(uint8_t *payload); diff --git a/src/main/rx/nrf24_syma.c b/src/main/rx/nrf24_syma.c index eb20cc22b27..b08cfaa2753 100644 --- a/src/main/rx/nrf24_syma.c +++ b/src/main/rx/nrf24_syma.c @@ -31,7 +31,7 @@ #include "drivers/rx_nrf24l01.h" #include "drivers/system.h" -#include "rx/nrf24.h" +#include "rx/rx_spi.h" #include "rx/nrf24_syma.h" /* @@ -85,7 +85,7 @@ enum { #define FLAG_VIDEO_X5C 0x10 #define FLAG_RATE_X5C 0x04 -STATIC_UNIT_TESTED nrf24_protocol_t symaProtocol; +STATIC_UNIT_TESTED rx_spi_protocol_e symaProtocol; typedef enum { STATE_BIND = 0, @@ -159,11 +159,11 @@ STATIC_UNIT_TESTED uint16_t symaConvertToPwmSigned(uint8_t val) void symaNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet) { - rcData[NRF24_THROTTLE] = symaConvertToPwmUnsigned(packet[0]); // throttle - rcData[NRF24_ROLL] = symaConvertToPwmSigned(packet[3]); // aileron + rcData[RC_SPI_THROTTLE] = symaConvertToPwmUnsigned(packet[0]); // throttle + rcData[RC_SPI_ROLL] = symaConvertToPwmSigned(packet[3]); // aileron if (symaProtocol == NRF24RX_SYMA_X) { - rcData[NRF24_PITCH] = symaConvertToPwmSigned(packet[1]); // elevator - rcData[NRF24_YAW] = symaConvertToPwmSigned(packet[2]); // rudder + rcData[RC_SPI_PITCH] = symaConvertToPwmSigned(packet[1]); // elevator + rcData[RC_SPI_YAW] = symaConvertToPwmSigned(packet[2]); // rudder const uint8_t rate = (packet[5] & 0xc0) >> 6; if (rate == RATE_LOW) { rcData[RC_CHANNEL_RATE] = PWM_RANGE_MIN; @@ -177,8 +177,8 @@ void symaNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet) rcData[RC_CHANNEL_VIDEO] = packet[4] & FLAG_VIDEO ? PWM_RANGE_MAX : PWM_RANGE_MIN; rcData[RC_CHANNEL_HEADLESS] = packet[14] & FLAG_HEADLESS ? PWM_RANGE_MAX : PWM_RANGE_MIN; } else { - rcData[NRF24_PITCH] = symaConvertToPwmSigned(packet[2]); // elevator - rcData[NRF24_YAW] = symaConvertToPwmSigned(packet[1]); // rudder + rcData[RC_SPI_PITCH] = symaConvertToPwmSigned(packet[2]); // elevator + rcData[RC_SPI_YAW] = symaConvertToPwmSigned(packet[1]); // rudder const uint8_t flags = packet[14]; rcData[RC_CHANNEL_RATE] = flags & FLAG_RATE_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN; rcData[RC_CHANNEL_FLIP] = flags & FLAG_FLIP_X5C ? PWM_RANGE_MAX : PWM_RANGE_MIN; @@ -224,18 +224,18 @@ static void setSymaXHoppingChannels(uint32_t addr) /* * This is called periodically by the scheduler. - * Returns NRF24_RECEIVED_DATA if a data packet was received. + * Returns RX_SPI_RECEIVED_DATA if a data packet was received. */ -nrf24_received_t symaNrf24DataReceived(uint8_t *payload) +rx_spi_received_e symaNrf24DataReceived(uint8_t *payload) { - nrf24_received_t ret = NRF24_RECEIVED_NONE; + rx_spi_received_e ret = RX_SPI_RECEIVED_NONE; switch (protocolState) { case STATE_BIND: if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) { const bool bindPacket = symaCheckBindPacket(payload); if (bindPacket) { - ret = NRF24_RECEIVED_BIND; + ret = RX_SPI_RECEIVED_BIND; protocolState = STATE_DATA; // using protocol NRF24L01_SYMA_X, since NRF24L01_SYMA_X5C went straight into data mode // set the hopping channels as determined by the rxTxAddr received in the bind packet @@ -253,7 +253,7 @@ nrf24_received_t symaNrf24DataReceived(uint8_t *payload) if (NRF24L01_ReadPayloadIfAvailable(payload, payloadSize)) { symaHopToNextChannel(); timeOfLastHop = micros(); - ret = NRF24_RECEIVED_DATA; + ret = RX_SPI_RECEIVED_DATA; } if (micros() > timeOfLastHop + hopTimeout) { symaHopToNextChannel(); @@ -264,7 +264,7 @@ nrf24_received_t symaNrf24DataReceived(uint8_t *payload) return ret; } -static void symaNrf24Setup(nrf24_protocol_t protocol) +static void symaNrf24Setup(rx_spi_protocol_e protocol) { symaProtocol = protocol; NRF24L01_Initialize(BV(NRF24L01_00_CONFIG_EN_CRC) | BV( NRF24L01_00_CONFIG_CRCO)); // sets PWR_UP, EN_CRC, CRCO - 2 byte CRC @@ -295,7 +295,7 @@ static void symaNrf24Setup(nrf24_protocol_t protocol) void symaNrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { rxRuntimeConfig->channelCount = RC_CHANNEL_COUNT; - symaNrf24Setup((nrf24_protocol_t)rxConfig->nrf24rx_protocol); + symaNrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol); } #endif diff --git a/src/main/rx/nrf24_syma.h b/src/main/rx/nrf24_syma.h index 86b1dd90e53..688bbb99eec 100644 --- a/src/main/rx/nrf24_syma.h +++ b/src/main/rx/nrf24_syma.h @@ -24,5 +24,5 @@ struct rxConfig_s; struct rxRuntimeConfig_s; void symaNrf24Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); void symaNrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload); -nrf24_received_t symaNrf24DataReceived(uint8_t *payload); +rx_spi_received_e symaNrf24DataReceived(uint8_t *payload); diff --git a/src/main/rx/nrf24_v202.c b/src/main/rx/nrf24_v202.c index 01ccd0619af..be724469f34 100644 --- a/src/main/rx/nrf24_v202.c +++ b/src/main/rx/nrf24_v202.c @@ -31,7 +31,7 @@ #include "drivers/rx_nrf24l01.h" #include "drivers/system.h" -#include "rx/nrf24.h" +#include "rx/rx_spi.h" #include "rx/nrf24_v202.h" /* @@ -97,10 +97,10 @@ STATIC_UNIT_TESTED uint8_t bind_phase; static uint32_t packet_timer; STATIC_UNIT_TESTED uint8_t txid[TXIDSIZE]; static uint32_t rx_timeout; -extern uint16_t nrf24RcData[]; +extern uint16_t rxSpiRcData[]; -static const unsigned char v2x2_channelindex[] = {NRF24_THROTTLE,NRF24_YAW,NRF24_PITCH,NRF24_ROLL, - NRF24_AUX1,NRF24_AUX2,NRF24_AUX3,NRF24_AUX4,NRF24_AUX5,NRF24_AUX6,NRF24_AUX7}; +static const unsigned char v2x2_channelindex[] = {RC_SPI_THROTTLE,RC_SPI_YAW,RC_SPI_PITCH,RC_SPI_ROLL, + RC_SPI_AUX1,RC_SPI_AUX2,RC_SPI_AUX3,RC_SPI_AUX4,RC_SPI_AUX5,RC_SPI_AUX6,RC_SPI_AUX7}; static void prepare_to_bind(void) { @@ -147,41 +147,41 @@ static void decode_bind_packet(uint8_t *packet) } // Returns whether the data was successfully decoded -static nrf24_received_t decode_packet(uint8_t *packet) +static rx_spi_received_e decode_packet(uint8_t *packet) { if(bind_phase != PHASE_BOUND) { decode_bind_packet(packet); - return NRF24_RECEIVED_BIND; + return RX_SPI_RECEIVED_BIND; } // Decode packet if ((packet[14] & V2X2_FLAG_BIND) == V2X2_FLAG_BIND) { - return NRF24_RECEIVED_BIND; + return RX_SPI_RECEIVED_BIND; } if (packet[7] != txid[0] || packet[8] != txid[1] || packet[9] != txid[2]) { - return NRF24_RECEIVED_NONE; + return RX_SPI_RECEIVED_NONE; } // Restore regular interval rx_timeout = 10000L; // 4ms interval, duplicate packets, (8ms unique) + 25% // TREA order in packet to MultiWii order is handled by // correct assignment to channelindex // Throttle 0..255 to 1000..2000 - nrf24RcData[v2x2_channelindex[0]] = ((uint16_t)packet[0]) * 1000 / 255 + 1000; + rxSpiRcData[v2x2_channelindex[0]] = ((uint16_t)packet[0]) * 1000 / 255 + 1000; for (int i = 1; i < 4; ++i) { uint8_t a = packet[i]; - nrf24RcData[v2x2_channelindex[i]] = ((uint16_t)(a < 0x80 ? 0x7f - a : a)) * 1000 / 255 + 1000; + rxSpiRcData[v2x2_channelindex[i]] = ((uint16_t)(a < 0x80 ? 0x7f - a : a)) * 1000 / 255 + 1000; } const uint8_t flags[] = {V2X2_FLAG_LED, V2X2_FLAG_FLIP, V2X2_FLAG_CAMERA, V2X2_FLAG_VIDEO}; // two more unknown bits for (int i = 4; i < 8; ++i) { - nrf24RcData[v2x2_channelindex[i]] = (packet[14] & flags[i-4]) ? PWM_RANGE_MAX : PWM_RANGE_MIN; + rxSpiRcData[v2x2_channelindex[i]] = (packet[14] & flags[i-4]) ? PWM_RANGE_MAX : PWM_RANGE_MIN; } const uint8_t flags10[] = {V2X2_FLAG_HEADLESS, V2X2_FLAG_MAG_CAL_X, V2X2_FLAG_MAG_CAL_Y}; for (int i = 8; i < 11; ++i) { - nrf24RcData[v2x2_channelindex[i]] = (packet[10] & flags10[i-8]) ? PWM_RANGE_MAX : PWM_RANGE_MIN; + rxSpiRcData[v2x2_channelindex[i]] = (packet[10] & flags10[i-8]) ? PWM_RANGE_MAX : PWM_RANGE_MIN; } packet_timer = micros(); - return NRF24_RECEIVED_DATA; + return RX_SPI_RECEIVED_DATA; } void v202Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet) @@ -191,7 +191,7 @@ void v202Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *packet) // Ideally the decoding of the packet should be moved into here, to reduce the overhead of v202DataReceived function. } -static nrf24_received_t readrx(uint8_t *packet) +static rx_spi_received_e readrx(uint8_t *packet) { if (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & BV(NRF24L01_07_STATUS_RX_DR))) { uint32_t t = micros() - packet_timer; @@ -199,7 +199,7 @@ static nrf24_received_t readrx(uint8_t *packet) switch_channel(); packet_timer = micros(); } - return NRF24_RECEIVED_NONE; + return RX_SPI_RECEIVED_NONE; } packet_timer = micros(); NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_STATUS_RX_DR)); // clear the RX_DR flag @@ -212,14 +212,14 @@ static nrf24_received_t readrx(uint8_t *packet) /* * This is called periodically by the scheduler. - * Returns NRF24_RECEIVED_DATA if a data packet was received. + * Returns RX_SPI_RECEIVED_DATA if a data packet was received. */ -nrf24_received_t v202Nrf24DataReceived(uint8_t *packet) +rx_spi_received_e v202Nrf24DataReceived(uint8_t *packet) { return readrx(packet); } -static void v202Nrf24Setup(nrf24_protocol_t protocol) +static void v202Nrf24Setup(rx_spi_protocol_e protocol) { NRF24L01_Initialize(BV(NRF24L01_00_CONFIG_EN_CRC) | BV(NRF24L01_00_CONFIG_CRCO)); // 2-bytes CRC @@ -253,7 +253,6 @@ static void v202Nrf24Setup(nrf24_protocol_t protocol) void v202Nrf24Init(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { rxRuntimeConfig->channelCount = V2X2_RC_CHANNEL_COUNT; - v202Nrf24Setup((nrf24_protocol_t)rxConfig->nrf24rx_protocol); + v202Nrf24Setup((rx_spi_protocol_e)rxConfig->rx_spi_protocol); } - #endif diff --git a/src/main/rx/nrf24_v202.h b/src/main/rx/nrf24_v202.h index fa47a934ba9..f3600ad7d08 100644 --- a/src/main/rx/nrf24_v202.h +++ b/src/main/rx/nrf24_v202.h @@ -23,5 +23,5 @@ struct rxConfig_s; struct rxRuntimeConfig_s; void v202Nrf24Init(const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); -nrf24_received_t v202Nrf24DataReceived(uint8_t *payload); void v202Nrf24SetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload); +rx_spi_received_e v202Nrf24DataReceived(uint8_t *payload); diff --git a/src/main/rx/pwm.c b/src/main/rx/pwm.c index 900d0173724..ab92a044ca0 100644 --- a/src/main/rx/pwm.c +++ b/src/main/rx/pwm.c @@ -33,17 +33,18 @@ #include "drivers/pwm_rx.h" #include "config/config.h" +#include "config/feature.h" #include "rx/rx.h" #include "rx/pwm.h" -static uint16_t pwmReadRawRC(rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t channel) +static uint16_t pwmReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t channel) { UNUSED(rxRuntimeConfigPtr); return pwmRead(channel); } -static uint16_t ppmReadRawRC(rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t channel) +static uint16_t ppmReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t channel) { UNUSED(rxRuntimeConfigPtr); return ppmRead(channel); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 9a0ef4cd22f..38e32bb7e64 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -22,31 +22,30 @@ #include #include "platform.h" -#include "build/build_config.h" +#include "build/build_config.h" #include "build/debug.h" - - #include "common/maths.h" #include "config/config.h" +#include "config/feature.h" #include "drivers/serial.h" #include "drivers/adc.h" +#include "drivers/gpio.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/rx_spi.h" +#include "drivers/system.h" -#include "io/serial.h" #include "fc/rc_controls.h" - #include "flight/failsafe.h" -#include "drivers/gpio.h" -#include "drivers/timer.h" -#include "drivers/pwm_rx.h" -#include "drivers/rx_nrf24l01.h" -#include "drivers/system.h" +#include "io/serial.h" +#include "rx/rx.h" #include "rx/pwm.h" #include "rx/sbus.h" #include "rx/spektrum.h" @@ -56,9 +55,7 @@ #include "rx/xbus.h" #include "rx/ibus.h" #include "rx/jetiexbus.h" -#include "rx/nrf24.h" - -#include "rx/rx.h" +#include "rx/rx_spi.h" //#define DEBUG_RX_SIGNAL_LOSS @@ -96,7 +93,7 @@ rxRuntimeConfig_t rxRuntimeConfig; static rxConfig_t *rxConfig; static uint8_t rcSampleIndex = 0; -static uint16_t nullReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { +static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { UNUSED(rxRuntimeConfig); UNUSED(channel); @@ -192,13 +189,13 @@ void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationCondi } #endif -#ifdef USE_RX_NRF24 - if (feature(FEATURE_RX_NRF24)) { +#ifdef USE_RX_SPI + if (feature(FEATURE_RX_SPI)) { rxRefreshRate = 10000; - const nfr24l01_spi_type_e spiType = feature(FEATURE_SOFTSPI) ? NFR24L01_SOFTSPI : NFR24L01_SPI; - const bool enabled = rxNrf24Init(spiType, rxConfig, &rxRuntimeConfig, &rcReadRawFunc); + const rx_spi_type_e spiType = feature(FEATURE_SOFTSPI) ? RX_SPI_SOFTSPI : RX_SPI_HARDSPI; + const bool enabled = rxSpiInit(spiType, rxConfig, &rxRuntimeConfig, &rcReadRawFunc); if (!enabled) { - featureClear(FEATURE_RX_NRF24); + featureClear(FEATURE_RX_SPI); rcReadRawFunc = nullReadRawRC; } } @@ -362,9 +359,9 @@ void updateRx(uint32_t currentTime) } #endif -#ifdef USE_RX_NRF24 - if (feature(FEATURE_RX_NRF24)) { - rxDataReceived = rxNrf24DataReceived(); +#ifdef USE_RX_SPI + if (feature(FEATURE_RX_SPI)) { + rxDataReceived = rxSpiDataReceived(); if (rxDataReceived) { rxSignalReceived = true; rxIsInFailsafeMode = false; @@ -667,4 +664,3 @@ void updateRSSI(uint32_t currentTime) void initRxRefreshRate(uint16_t *rxRefreshRatePtr) { *rxRefreshRatePtr = rxRefreshRate; } - diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 6b1443376a8..1e92411126c 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -113,18 +113,18 @@ typedef struct rxConfig_s { uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. uint8_t sbus_inversion; // default sbus (Futaba, FrSKY) is inverted. Support for uninverted OpenLRS (and modified FrSKY) receivers. - uint8_t nrf24rx_protocol; // type of nrf24 protocol (0 = v202 250kbps). Must be enabled by FEATURE_RX_NRF24 first. - uint32_t nrf24rx_id; - uint8_t nrf24rx_rf_channel_count; + uint8_t rx_spi_protocol; // type of nrf24 protocol (0 = v202 250kbps). Must be enabled by FEATURE_RX_NRF24 first. + uint32_t rx_spi_id; + uint8_t rx_spi_rf_channel_count; uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers uint8_t spektrum_sat_bind_autoreset; // whenever we will reset (exit) binding mode after hard reboot uint8_t rssi_channel; uint8_t rssi_scale; uint8_t rssi_ppm_invert; - uint8_t rcSmoothing; // Enable/Disable RC filtering uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here uint16_t mincheck; // minimum rc end uint16_t maxcheck; // maximum rc end + uint8_t rcSmoothing; // Enable/Disable RC filtering uint16_t rx_min_usec; uint16_t rx_max_usec; @@ -142,12 +142,11 @@ typedef struct rxRuntimeConfig_s { extern rxRuntimeConfig_t rxRuntimeConfig; +typedef uint16_t (*rcReadRawDataPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data + struct modeActivationCondition_s; void rxInit(rxConfig_t *rxConfig, struct modeActivationCondition_s *modeActivationConditions); void useRxConfig(rxConfig_t *rxConfigToUse); - -typedef uint16_t (*rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data - void updateRx(uint32_t currentTime); bool rxIsReceivingSignal(void); bool rxAreFlightChannelsValid(void); diff --git a/src/main/rx/nrf24.c b/src/main/rx/rx_spi.c similarity index 71% rename from src/main/rx/nrf24.c rename to src/main/rx/rx_spi.c index b6a23ebabcb..c7397fbfd9d 100644 --- a/src/main/rx/nrf24.c +++ b/src/main/rx/rx_spi.c @@ -20,13 +20,13 @@ #include -#ifdef USE_RX_NRF24 +#ifdef USE_RX_SPI #include "build/build_config.h" #include "drivers/rx_nrf24l01.h" #include "rx/rx.h" -#include "rx/nrf24.h" +#include "rx/rx_spi.h" #include "rx/nrf24_cx10.h" #include "rx/nrf24_syma.h" #include "rx/nrf24_v202.h" @@ -34,31 +34,32 @@ #include "rx/nrf24_inav.h" -uint16_t nrf24RcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; -STATIC_UNIT_TESTED uint8_t nrf24Payload[NRF24L01_MAX_PAYLOAD_SIZE]; -STATIC_UNIT_TESTED uint8_t nrf24NewPacketAvailable; // set true when a new packet is received +uint16_t rxSpiRcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +STATIC_UNIT_TESTED uint8_t rxSpiPayload[RX_SPI_MAX_PAYLOAD_SIZE]; +STATIC_UNIT_TESTED uint8_t rxSpiNewPacketAvailable; // set true when a new packet is received typedef void (*protocolInitPtr)(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); -typedef nrf24_received_t (*protocolDataReceivedPtr)(uint8_t *payload); +typedef rx_spi_received_e (*protocolDataReceivedPtr)(uint8_t *payload); typedef void (*protocolSetRcDataFromPayloadPtr)(uint16_t *rcData, const uint8_t *payload); static protocolInitPtr protocolInit; static protocolDataReceivedPtr protocolDataReceived; static protocolSetRcDataFromPayloadPtr protocolSetRcDataFromPayload; -STATIC_UNIT_TESTED uint16_t rxNrf24ReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) +STATIC_UNIT_TESTED uint16_t rxSpiReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { + BUILD_BUG_ON(NRF24L01_MAX_PAYLOAD_SIZE > RX_SPI_MAX_PAYLOAD_SIZE); if (channel >= rxRuntimeConfig->channelCount) { return 0; } - if (nrf24NewPacketAvailable) { - protocolSetRcDataFromPayload(nrf24RcData, nrf24Payload); - nrf24NewPacketAvailable = false; + if (rxSpiNewPacketAvailable) { + protocolSetRcDataFromPayload(rxSpiRcData, rxSpiPayload); + rxSpiNewPacketAvailable = false; } - return nrf24RcData[channel]; + return rxSpiRcData[channel]; } -STATIC_UNIT_TESTED bool rxNrf24SetProtocol(nrf24_protocol_t protocol) +STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol) { switch (protocol) { default: @@ -105,33 +106,33 @@ STATIC_UNIT_TESTED bool rxNrf24SetProtocol(nrf24_protocol_t protocol) } /* - * Returns true if the NRF24L01 has received new data. + * Returns true if the RX has received new data. * Called from updateRx in rx.c, updateRx called from taskUpdateRxCheck. * If taskUpdateRxCheck returns true, then taskUpdateRxMain will shortly be called. */ -bool rxNrf24DataReceived(void) +bool rxSpiDataReceived(void) { - if (protocolDataReceived(nrf24Payload) == NRF24_RECEIVED_DATA) { - nrf24NewPacketAvailable = true; + if (protocolDataReceived(rxSpiPayload) == RX_SPI_RECEIVED_DATA) { + rxSpiNewPacketAvailable = true; return true; } return false; } /* - * Set and initialize the NRF24 protocol + * Set and initialize the RX protocol */ -bool rxNrf24Init(nfr24l01_spi_type_e spiType, const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) +bool rxSpiInit(rx_spi_type_e spiType, const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { bool ret = false; - NRF24L01_SpiInit(spiType); - if (rxNrf24SetProtocol(rxConfig->nrf24rx_protocol)) { + rxSpiDeviceInit(spiType); + if (rxSpiSetProtocol(rxConfig->rx_spi_protocol)) { protocolInit(rxConfig, rxRuntimeConfig); ret = true; } - nrf24NewPacketAvailable = false; + rxSpiNewPacketAvailable = false; if (callback) { - *callback = rxNrf24ReadRawRC; + *callback = rxSpiReadRawRC; } return ret; } diff --git a/src/main/rx/nrf24.h b/src/main/rx/rx_spi.h similarity index 55% rename from src/main/rx/nrf24.h rename to src/main/rx/rx_spi.h index a0190f5e6db..48b7f0a54f0 100644 --- a/src/main/rx/nrf24.h +++ b/src/main/rx/rx_spi.h @@ -32,45 +32,45 @@ typedef enum { NRF24RX_H8_3D, NRF24RX_INAV, NRF24RX_PROTOCOL_COUNT -} nrf24_protocol_t; +} rx_spi_protocol_e; typedef enum { - NRF24_RECEIVED_NONE = 0, - NRF24_RECEIVED_BIND, - NRF24_RECEIVED_DATA -} nrf24_received_t; + RX_SPI_RECEIVED_NONE = 0, + RX_SPI_RECEIVED_BIND, + RX_SPI_RECEIVED_DATA +} rx_spi_received_e; // RC channels in AETR order typedef enum { - NRF24_ROLL = 0, - NRF24_PITCH, - NRF24_THROTTLE, - NRF24_YAW, - NRF24_AUX1, - NRF24_AUX2, - NRF24_AUX3, - NRF24_AUX4, - NRF24_AUX5, - NRF24_AUX6, - NRF24_AUX7, - NRF24_AUX8, - NRF24_AUX9, - NRF24_AUX10, - NRF24_AUX11, - NRF24_AUX12, - NRF24_AUX13, - NRF24_AUX14 -} nrf24_AETR_t; + RC_SPI_ROLL = 0, + RC_SPI_PITCH, + RC_SPI_THROTTLE, + RC_SPI_YAW, + RC_SPI_AUX1, + RC_SPI_AUX2, + RC_SPI_AUX3, + RC_SPI_AUX4, + RC_SPI_AUX5, + RC_SPI_AUX6, + RC_SPI_AUX7, + RC_SPI_AUX8, + RC_SPI_AUX9, + RC_SPI_AUX10, + RC_SPI_AUX11, + RC_SPI_AUX12, + RC_SPI_AUX13, + RC_SPI_AUX14 +} rc_spi_aetr_e; // RC channels as used by deviation -#define RC_CHANNEL_RATE NRF24_AUX1 -#define RC_CHANNEL_FLIP NRF24_AUX2 -#define RC_CHANNEL_PICTURE NRF24_AUX3 -#define RC_CHANNEL_VIDEO NRF24_AUX4 -#define RC_CHANNEL_HEADLESS NRF24_AUX5 -#define RC_CHANNEL_RTH NRF24_AUX6 // return to home +#define RC_CHANNEL_RATE RC_SPI_AUX1 +#define RC_CHANNEL_FLIP RC_SPI_AUX2 +#define RC_CHANNEL_PICTURE RC_SPI_AUX3 +#define RC_CHANNEL_VIDEO RC_SPI_AUX4 +#define RC_CHANNEL_HEADLESS RC_SPI_AUX5 +#define RC_CHANNEL_RTH RC_SPI_AUX6 // return to home -bool rxNrf24DataReceived(void); +bool rxSpiDataReceived(void); struct rxConfig_s; struct rxRuntimeConfig_s; -bool rxNrf24Init(nfr24l01_spi_type_e spiType, const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig, rcReadRawDataPtr *callback); +bool rxSpiInit(rx_spi_type_e spiType, const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig, rcReadRawDataPtr *callback); diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index c86c81d09fa..75098b106d4 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -75,7 +75,7 @@ static uint16_t sbusStateFlags = 0; static bool sbusFrameDone = false; static void sbusDataReceive(uint16_t c); -static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t sbusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); static uint32_t sbusChannelData[SBUS_MAX_CHANNEL]; @@ -237,7 +237,7 @@ uint8_t sbusFrameStatus(void) return SERIAL_RX_FRAME_COMPLETE; } -static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t sbusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { UNUSED(rxRuntimeConfig); // Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index b8152c28f59..e26d94e65c2 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -61,7 +61,7 @@ static bool spekHiRes = false; static volatile uint8_t spekFrame[SPEK_FRAME_SIZE]; static void spektrumDataReceive(uint16_t c); -static uint16_t spektrumReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); static rxRuntimeConfig_t *rxRuntimeConfigPtr; @@ -165,7 +165,7 @@ uint8_t spektrumFrameStatus(void) return SERIAL_RX_FRAME_COMPLETE; } -static uint16_t spektrumReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { uint16_t data; diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 82a8da55f90..9da6e5bfe51 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -48,7 +48,7 @@ static uint16_t sumdChannels[SUMD_MAX_CHANNEL]; static uint16_t crc; static void sumdDataReceive(uint16_t c); -static uint16_t sumdReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t sumdReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { @@ -171,7 +171,7 @@ uint8_t sumdFrameStatus(void) return frameStatus; } -static uint16_t sumdReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t sumdReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { UNUSED(rxRuntimeConfig); return sumdChannels[chan] / 8; diff --git a/src/main/rx/sumh.c b/src/main/rx/sumh.c index 9701597d356..cf7719aad74 100644 --- a/src/main/rx/sumh.c +++ b/src/main/rx/sumh.c @@ -51,15 +51,11 @@ static bool sumhFrameDone = false; static uint8_t sumhFrame[SUMH_FRAME_SIZE]; static uint32_t sumhChannels[SUMH_MAX_CHANNEL_COUNT]; -static void sumhDataReceive(uint16_t c); -static uint16_t sumhReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); - static serialPort_t *sumhPort; static void sumhDataReceive(uint16_t c); -static uint16_t sumhReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); - +static uint16_t sumhReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) @@ -125,7 +121,7 @@ uint8_t sumhFrameStatus(void) return SERIAL_RX_FRAME_COMPLETE; } -static uint16_t sumhReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t sumhReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { UNUSED(rxRuntimeConfig); diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index dbd28e7c481..4c1f0513ae9 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -82,7 +82,7 @@ static volatile uint8_t xBusFrame[XBUS_RJ01_FRAME_SIZE]; static uint16_t xBusChannelData[XBUS_RJ01_CHANNEL_COUNT]; static void xBusDataReceive(uint16_t c); -static uint16_t xBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t xBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { @@ -303,7 +303,7 @@ uint8_t xBusFrameStatus(void) return SERIAL_RX_FRAME_COMPLETE; } -static uint16_t xBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t xBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { uint16_t data; diff --git a/src/main/rx/xbus.h b/src/main/rx/xbus.h index bc76f567868..19dc6d5735a 100644 --- a/src/main/rx/xbus.h +++ b/src/main/rx/xbus.h @@ -17,7 +17,5 @@ #pragma once -#include "rx/rx.h" - bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); uint8_t xBusFrameStatus(void); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 2bd7b47e2e7..8183f311945 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -68,6 +68,12 @@ typedef enum { #ifdef LED_STRIP TASK_LEDSTRIP, #endif +#ifdef USE_PMW_SERVO_DRIVER + TASK_PWMDRIVER, +#endif +#ifdef STACK_CHECK + TASK_STACK_CHECK, +#endif /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/scheduler/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c deleted file mode 100755 index d58235a11b8..00000000000 --- a/src/main/scheduler/scheduler_tasks.c +++ /dev/null @@ -1,132 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include -#include - -#include "platform.h" -#include "scheduler.h" -#include "scheduler_tasks.h" - -cfTask_t cfTasks[TASK_COUNT] = { - [TASK_SYSTEM] = { - .taskName = "SYSTEM", - .taskFunc = taskSystem, - .desiredPeriod = 1000000 / 10, // run every 100 ms - .staticPriority = TASK_PRIORITY_HIGH, - }, - - [TASK_GYROPID] = { - .taskName = "GYRO/PID", - .taskFunc = taskMainPidLoopChecker, - .desiredPeriod = 1000, - .staticPriority = TASK_PRIORITY_REALTIME, - }, - - [TASK_SERIAL] = { - .taskName = "SERIAL", - .taskFunc = taskHandleSerial, - .desiredPeriod = 1000000 / 100, // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud - .staticPriority = TASK_PRIORITY_LOW, - }, - - [TASK_BEEPER] = { - .taskName = "BEEPER", - .taskFunc = taskUpdateBeeper, - .desiredPeriod = 1000000 / 100, // 100 Hz - .staticPriority = TASK_PRIORITY_MEDIUM, - }, - - [TASK_BATTERY] = { - .taskName = "BATTERY", - .taskFunc = taskUpdateBattery, - .desiredPeriod = 1000000 / 50, // 50 Hz - .staticPriority = TASK_PRIORITY_MEDIUM, - }, - - [TASK_RX] = { - .taskName = "RX", - .checkFunc = taskUpdateRxCheck, - .taskFunc = taskUpdateRxMain, - .desiredPeriod = 1000000 / 50, // If event-based scheduling doesn't work, fallback to periodic scheduling - .staticPriority = TASK_PRIORITY_HIGH, - }, - -#ifdef GPS - [TASK_GPS] = { - .taskName = "GPS", - .taskFunc = taskProcessGPS, - .desiredPeriod = 1000000 / 25, // GPS usually don't go raster than 10Hz - .staticPriority = TASK_PRIORITY_MEDIUM, - }, -#endif - -#ifdef MAG - [TASK_COMPASS] = { - .taskName = "COMPASS", - .taskFunc = taskUpdateCompass, - .desiredPeriod = 1000000 / 10, // Compass is updated at 10 Hz - .staticPriority = TASK_PRIORITY_MEDIUM, - }, -#endif - -#ifdef BARO - [TASK_BARO] = { - .taskName = "BARO", - .taskFunc = taskUpdateBaro, - .desiredPeriod = 1000000 / 20, - .staticPriority = TASK_PRIORITY_MEDIUM, - }, -#endif - -#ifdef SONAR - [TASK_SONAR] = { - .taskName = "SONAR", - .taskFunc = taskUpdateSonar, - .desiredPeriod = 70000, // every 70 ms, approximately 14 Hz - .staticPriority = TASK_PRIORITY_MEDIUM, - }, -#endif - -#ifdef DISPLAY - [TASK_DISPLAY] = { - .taskName = "DISPLAY", - .taskFunc = taskUpdateDisplay, - .desiredPeriod = 1000000 / 10, - .staticPriority = TASK_PRIORITY_LOW, - }, -#endif - -#ifdef TELEMETRY - [TASK_TELEMETRY] = { - .taskName = "TELEMETRY", - .taskFunc = taskTelemetry, - .desiredPeriod = 1000000 / 250, // 250 Hz - .staticPriority = TASK_PRIORITY_IDLE, - }, -#endif - -#ifdef LED_STRIP - [TASK_LEDSTRIP] = { - .taskName = "LEDSTRIP", - .taskFunc = taskLedStrip, - .desiredPeriod = 1000000 / 100, // 100 Hz - .staticPriority = TASK_PRIORITY_IDLE, - }, -#endif -}; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index b0a4e7bcbba..956e2ebb342 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -58,7 +58,7 @@ void accInit(uint32_t targetLooptime) accTargetLooptime = targetLooptime; if (accLpfCutHz) { for (int axis = 0; axis < 3; axis++) { - biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime); + biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime); } } } @@ -223,7 +223,7 @@ void setAccelerationFilter(uint8_t initialAccLpfCutHz) accLpfCutHz = initialAccLpfCutHz; if (accTargetLooptime) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime); + biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime); } } } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 7a0950ca423..a637a8ccbb4 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -29,6 +29,7 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/feature.h" #include "sensors/battery.h" diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index a6a20b219ae..f7f46edff1e 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -20,6 +20,7 @@ #include #include "platform.h" +#include "build/debug.h" #include "common/axis.h" #include "common/maths.h" diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 9806d846e6f..e2dadee1642 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -59,7 +59,7 @@ void gyroInit(void) { if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { - biquadFilterInit(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); + biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); } } } diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 6eced42ae37..ed0dff5c996 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -67,6 +67,7 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/feature.h" #include "io/gps.h" @@ -523,7 +524,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse) #endif ; // fallthough case BARO_BMP280: -#ifdef USE_BARO_BMP280 +#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280) if (bmp280Detect(&baro)) { baroHardware = BARO_BMP280; break; @@ -786,4 +787,3 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, return true; } - diff --git a/src/main/startup/startup_stm32f10x_hd_gcc.S b/src/main/startup/startup_stm32f10x_hd_gcc.S index a828691b64d..c2422c20f53 100644 --- a/src/main/startup/startup_stm32f10x_hd_gcc.S +++ b/src/main/startup/startup_stm32f10x_hd_gcc.S @@ -98,6 +98,19 @@ LoopFillZerobss: cmp r2, r3 bcc FillZerobss +/* Mark the heap and stack */ + ldr r2, =_heap_stack_begin + b LoopMarkHeapStack + +MarkHeapStack: + movs r3, 0xa5a5a5a5 + str r3, [r2], #4 + +LoopMarkHeapStack: + ldr r3, = _heap_stack_end + cmp r2, r3 + bcc MarkHeapStack + /* Call the clock system intitialization function.*/ bl SystemInit /* Call the application's entry point.*/ @@ -105,7 +118,7 @@ LoopFillZerobss: /* Atollic update, branch LoopForever */ LoopForever: b LoopForever - + .equ RCC_APB2ENR, 0x40021018 // HJI - TC bootloader entry on reset mod .equ GPIO_AFIO_MASK, 0x00000009 // HJI - TC bootloader entry on reset mod .equ GPIOB_CRL, 0x40010C00 // HJI - TC bootloader entry on reset mod @@ -117,27 +130,27 @@ Reboot_Loader: // HJI - TC bootloader entry on reset mod ldr r6, =RCC_APB2ENR // HJI - TC bootloader entry on reset mod ldr r0, =GPIO_AFIO_MASK // HJI - TC bootloader entry on reset mod str R0, [r6]; // HJI - TC bootloader entry on reset mod - + // MAPR pt1 // HJI - TC bootloader entry on reset mod ldr r0, =AFIO_MAPR // HJI - TC bootloader entry on reset mod ldr r1, [r0] // HJI - TC bootloader entry on reset mod bic r1, r1, #0x0F000000 // HJI - TC bootloader entry on reset mod str r1, [r0] // HJI - TC bootloader entry on reset mod - + // MAPR pt2 // HJI - TC bootloader entry on reset mod lsls r1, r0, #9 // HJI - TC bootloader entry on reset mod str r1, [r0] // HJI - TC bootloader entry on reset mod - + // BRR // HJI - TC bootloader entry on reset mod ldr r4, =GPIOB_BRR // HJI - TC bootloader entry on reset mod movs r0, #0x18 // HJI - TC bootloader entry on reset mod str r0, [r4] // HJI - TC bootloader entry on reset mod - + // CRL // HJI - TC bootloader entry on reset mod ldr r1, =GPIOB_CRL // HJI - TC bootloader entry on reset mod ldr r0, =0x44433444 // HJI - TC bootloader entry on reset mod str r0, [r1] // HJI - TC bootloader entry on reset mod - + // Reboot to ROM // HJI - TC bootloader entry on reset mod ldr r0, =0x1FFFF000 // HJI - TC bootloader entry on reset mod ldr sp,[r0, #0] // HJI - TC bootloader entry on reset mod diff --git a/src/main/startup/startup_stm32f10x_md_gcc.S b/src/main/startup/startup_stm32f10x_md_gcc.S index 4bd8b2b7313..946bf5bfdac 100644 --- a/src/main/startup/startup_stm32f10x_md_gcc.S +++ b/src/main/startup/startup_stm32f10x_md_gcc.S @@ -97,6 +97,19 @@ LoopFillZerobss: cmp r2, r3 bcc FillZerobss +/* Mark the heap and stack */ + ldr r2, =_heap_stack_begin + b LoopMarkHeapStack + +MarkHeapStack: + movs r3, 0xa5a5a5a5 + str r3, [r2], #4 + +LoopMarkHeapStack: + ldr r3, = _heap_stack_end + cmp r2, r3 + bcc MarkHeapStack + /* Call the clock system intitialization function.*/ bl SystemInit /* Call the application's entry point.*/ @@ -104,7 +117,7 @@ LoopFillZerobss: /* Atollic update, branch LoopForever */ LoopForever: b LoopForever - + .equ RCC_APB2ENR, 0x40021018 // HJI - TC bootloader entry on reset mod .equ GPIO_AFIO_MASK, 0x00000009 // HJI - TC bootloader entry on reset mod .equ GPIOB_CRL, 0x40010C00 // HJI - TC bootloader entry on reset mod @@ -116,27 +129,27 @@ Reboot_Loader: // HJI - TC bootloader entry on reset mod ldr r6, =RCC_APB2ENR // HJI - TC bootloader entry on reset mod ldr r0, =GPIO_AFIO_MASK // HJI - TC bootloader entry on reset mod str R0, [r6]; // HJI - TC bootloader entry on reset mod - + // MAPR pt1 // HJI - TC bootloader entry on reset mod ldr r0, =AFIO_MAPR // HJI - TC bootloader entry on reset mod ldr r1, [r0] // HJI - TC bootloader entry on reset mod bic r1, r1, #0x0F000000 // HJI - TC bootloader entry on reset mod str r1, [r0] // HJI - TC bootloader entry on reset mod - + // MAPR pt2 // HJI - TC bootloader entry on reset mod lsls r1, r0, #9 // HJI - TC bootloader entry on reset mod str r1, [r0] // HJI - TC bootloader entry on reset mod - + // BRR // HJI - TC bootloader entry on reset mod ldr r4, =GPIOB_BRR // HJI - TC bootloader entry on reset mod movs r0, #0x18 // HJI - TC bootloader entry on reset mod str r0, [r4] // HJI - TC bootloader entry on reset mod - + // CRL // HJI - TC bootloader entry on reset mod ldr r1, =GPIOB_CRL // HJI - TC bootloader entry on reset mod ldr r0, =0x44433444 // HJI - TC bootloader entry on reset mod str r0, [r1] // HJI - TC bootloader entry on reset mod - + // Reboot to ROM // HJI - TC bootloader entry on reset mod ldr r0, =0x1FFFF000 // HJI - TC bootloader entry on reset mod ldr sp,[r0, #0] // HJI - TC bootloader entry on reset mod diff --git a/src/main/startup/startup_stm32f30x_md_gcc.S b/src/main/startup/startup_stm32f30x_md_gcc.S index e1387cbc0e6..b0dc20a653f 100644 --- a/src/main/startup/startup_stm32f30x_md_gcc.S +++ b/src/main/startup/startup_stm32f30x_md_gcc.S @@ -105,6 +105,19 @@ LoopFillZerobss: cmp r2, r3 bcc FillZerobss +/* Mark the heap and stack */ + ldr r2, =_heap_stack_begin + b LoopMarkHeapStack + +MarkHeapStack: + movs r3, 0xa5a5a5a5 + str r3, [r2], #4 + +LoopMarkHeapStack: + ldr r3, = _heap_stack_end + cmp r2, r3 + bcc MarkHeapStack + /* Call the clock system intitialization function.*/ bl SystemInit /* Call the application's entry point.*/ diff --git a/src/main/startup/startup_stm32f3_debug_hardfault_handler.S b/src/main/startup/startup_stm32f3_debug_hardfault_handler.S index 65ee39a6709..13a8487d29c 100644 --- a/src/main/startup/startup_stm32f3_debug_hardfault_handler.S +++ b/src/main/startup/startup_stm32f3_debug_hardfault_handler.S @@ -108,6 +108,19 @@ LoopFillZerobss: cmp r2, r3 bcc FillZerobss +/* Mark the heap and stack */ + ldr r2, =_heap_stack_begin + b LoopMarkHeapStack + +MarkHeapStack: + movs r3, 0xa5a5a5a5 + str r3, [r2], #4 + +LoopMarkHeapStack: + ldr r3, = _heap_stack_end + cmp r2, r3 + bcc MarkHeapStack + /* Call the clock system intitialization function.*/ bl SystemInit /* Call the application's entry point.*/ diff --git a/src/main/startup/startup_stm32f40xx.s b/src/main/startup/startup_stm32f40xx.s index fa98fff962a..680ec28dd04 100644 --- a/src/main/startup/startup_stm32f40xx.s +++ b/src/main/startup/startup_stm32f40xx.s @@ -4,14 +4,14 @@ * @author MCD Application Team * @version V1.6.1 * @date 21-October-2015 - * @brief STM32F40xxx/41xxx Devices vector table for Atollic TrueSTUDIO toolchain. - * Same as startup_stm32f40_41xxx.s and maintained for legacy purpose + * @brief STM32F40xxx/41xxx Devices vector table for Atollic TrueSTUDIO toolchain. + * Same as startup_stm32f40_41xxx.s and maintained for legacy purpose * This module performs: * - Set the initial SP * - Set the initial PC == Reset_Handler, * - Set the vector table entries with the exceptions ISR address - * - Configure the clock system and the external SRAM mounted on - * STM324xG-EVAL board to be used as data memory (optional, + * - Configure the clock system and the external SRAM mounted on + * STM324xG-EVAL board to be used as data memory (optional, * to be enabled by user) * - Branches to main in the C library (which eventually * calls main()). @@ -28,15 +28,15 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * ****************************************************************************** */ - + .syntax unified .cpu cortex-m4 .fpu softvfp @@ -46,10 +46,10 @@ .global Default_Handler .global irq_stack -/* start address for the initialization values of the .data section. +/* start address for the initialization values of the .data section. defined in linker script */ .word _sidata -/* start address for the .data section. defined in linker script */ +/* start address for the .data section. defined in linker script */ .word _sdata /* end address for the .data section. defined in linker script */ .word _edata @@ -63,7 +63,7 @@ defined in linker script */ * @brief This is the code that gets called when the processor first * starts execution following a reset event. Only the absolutely * necessary set is performed, after which the application - * supplied main() routine is called. + * supplied main() routine is called. * @param None * @retval : None */ @@ -71,7 +71,7 @@ defined in linker script */ .section .text.Reset_Handler .weak Reset_Handler .type Reset_Handler, %function -Reset_Handler: +Reset_Handler: ldr r0, =0x2001FFFC // mj666 ldr r1, =0xDEADBEEF // mj666 ldr r2, [r0, #0] // mj666 @@ -79,7 +79,7 @@ Reset_Handler: cmp r2, r1 // mj666 beq Reboot_Loader // mj666 -/* Copy the data segment initializers from flash to SRAM */ +/* Copy the data segment initializers from flash to SRAM */ movs r1, #0 b LoopCopyDataInit @@ -88,7 +88,7 @@ CopyDataInit: ldr r3, [r3, r1] str r3, [r0, r1] adds r1, r1, #4 - + LoopCopyDataInit: ldr r0, =_sdata ldr r3, =_edata @@ -97,16 +97,29 @@ LoopCopyDataInit: bcc CopyDataInit ldr r2, =_sbss b LoopFillZerobss -/* Zero fill the bss segment. */ +/* Zero fill the bss segment. */ FillZerobss: movs r3, #0 str r3, [r2], #4 - + LoopFillZerobss: ldr r3, = _ebss cmp r2, r3 bcc FillZerobss +/* Mark the heap and stack */ + ldr r2, =_heap_stack_begin + b LoopMarkHeapStack + +MarkHeapStack: + movs r3, 0xa5a5a5a5 + str r3, [r2], #4 + +LoopMarkHeapStack: + ldr r3, = _heap_stack_end + cmp r2, r3 + bcc MarkHeapStack + /*FPU settings*/ ldr r0, =0xE000ED88 /* Enable CP10,CP11 */ ldr r1,[r0] @@ -114,11 +127,11 @@ LoopFillZerobss: str r1,[r0] /* Call the clock system intitialization function.*/ - bl SystemInit + bl SystemInit /* Call the application's entry point.*/ bl main - bx lr + bx lr LoopForever: b LoopForever @@ -134,11 +147,11 @@ Reboot_Loader: // mj666 .size Reset_Handler, .-Reset_Handler /** - * @brief This is the code that gets called when the processor receives an + * @brief This is the code that gets called when the processor receives an * unexpected interrupt. This simply enters an infinite loop, preserving * the system state for examination by a debugger. - * @param None - * @retval None + * @param None + * @retval None */ .section .text.Default_Handler,"ax",%progbits Default_Handler: @@ -150,7 +163,7 @@ Infinite_Loop: * The minimal vector table for a Cortex M3. Note that the proper constructs * must be placed on this to ensure that it ends up at physical address * 0x0000.0000. -* +* *******************************************************************************/ .section .irqstack,"aw",%progbits irq_stack: @@ -159,8 +172,8 @@ Infinite_Loop: .section .isr_vector,"a",%progbits .type g_pfnVectors, %object .size g_pfnVectors, .-g_pfnVectors - - + + g_pfnVectors: .word irq_stack+1024 .word Reset_Handler @@ -178,107 +191,107 @@ g_pfnVectors: .word 0 .word PendSV_Handler .word SysTick_Handler - + /* External Interrupts */ - .word WWDG_IRQHandler /* Window WatchDog */ - .word PVD_IRQHandler /* PVD through EXTI Line detection */ - .word TAMP_STAMP_IRQHandler /* Tamper and TimeStamps through the EXTI line */ - .word RTC_WKUP_IRQHandler /* RTC Wakeup through the EXTI line */ - .word FLASH_IRQHandler /* FLASH */ - .word RCC_IRQHandler /* RCC */ - .word EXTI0_IRQHandler /* EXTI Line0 */ - .word EXTI1_IRQHandler /* EXTI Line1 */ - .word EXTI2_IRQHandler /* EXTI Line2 */ - .word EXTI3_IRQHandler /* EXTI Line3 */ - .word EXTI4_IRQHandler /* EXTI Line4 */ - .word DMA1_Stream0_IRQHandler /* DMA1 Stream 0 */ - .word DMA1_Stream1_IRQHandler /* DMA1 Stream 1 */ - .word DMA1_Stream2_IRQHandler /* DMA1 Stream 2 */ - .word DMA1_Stream3_IRQHandler /* DMA1 Stream 3 */ - .word DMA1_Stream4_IRQHandler /* DMA1 Stream 4 */ - .word DMA1_Stream5_IRQHandler /* DMA1 Stream 5 */ - .word DMA1_Stream6_IRQHandler /* DMA1 Stream 6 */ - .word ADC_IRQHandler /* ADC1, ADC2 and ADC3s */ - .word CAN1_TX_IRQHandler /* CAN1 TX */ - .word CAN1_RX0_IRQHandler /* CAN1 RX0 */ - .word CAN1_RX1_IRQHandler /* CAN1 RX1 */ - .word CAN1_SCE_IRQHandler /* CAN1 SCE */ - .word EXTI9_5_IRQHandler /* External Line[9:5]s */ - .word TIM1_BRK_TIM9_IRQHandler /* TIM1 Break and TIM9 */ - .word TIM1_UP_TIM10_IRQHandler /* TIM1 Update and TIM10 */ + .word WWDG_IRQHandler /* Window WatchDog */ + .word PVD_IRQHandler /* PVD through EXTI Line detection */ + .word TAMP_STAMP_IRQHandler /* Tamper and TimeStamps through the EXTI line */ + .word RTC_WKUP_IRQHandler /* RTC Wakeup through the EXTI line */ + .word FLASH_IRQHandler /* FLASH */ + .word RCC_IRQHandler /* RCC */ + .word EXTI0_IRQHandler /* EXTI Line0 */ + .word EXTI1_IRQHandler /* EXTI Line1 */ + .word EXTI2_IRQHandler /* EXTI Line2 */ + .word EXTI3_IRQHandler /* EXTI Line3 */ + .word EXTI4_IRQHandler /* EXTI Line4 */ + .word DMA1_Stream0_IRQHandler /* DMA1 Stream 0 */ + .word DMA1_Stream1_IRQHandler /* DMA1 Stream 1 */ + .word DMA1_Stream2_IRQHandler /* DMA1 Stream 2 */ + .word DMA1_Stream3_IRQHandler /* DMA1 Stream 3 */ + .word DMA1_Stream4_IRQHandler /* DMA1 Stream 4 */ + .word DMA1_Stream5_IRQHandler /* DMA1 Stream 5 */ + .word DMA1_Stream6_IRQHandler /* DMA1 Stream 6 */ + .word ADC_IRQHandler /* ADC1, ADC2 and ADC3s */ + .word CAN1_TX_IRQHandler /* CAN1 TX */ + .word CAN1_RX0_IRQHandler /* CAN1 RX0 */ + .word CAN1_RX1_IRQHandler /* CAN1 RX1 */ + .word CAN1_SCE_IRQHandler /* CAN1 SCE */ + .word EXTI9_5_IRQHandler /* External Line[9:5]s */ + .word TIM1_BRK_TIM9_IRQHandler /* TIM1 Break and TIM9 */ + .word TIM1_UP_TIM10_IRQHandler /* TIM1 Update and TIM10 */ .word TIM1_TRG_COM_TIM11_IRQHandler /* TIM1 Trigger and Commutation and TIM11 */ - .word TIM1_CC_IRQHandler /* TIM1 Capture Compare */ - .word TIM2_IRQHandler /* TIM2 */ - .word TIM3_IRQHandler /* TIM3 */ - .word TIM4_IRQHandler /* TIM4 */ - .word I2C1_EV_IRQHandler /* I2C1 Event */ - .word I2C1_ER_IRQHandler /* I2C1 Error */ - .word I2C2_EV_IRQHandler /* I2C2 Event */ - .word I2C2_ER_IRQHandler /* I2C2 Error */ - .word SPI1_IRQHandler /* SPI1 */ - .word SPI2_IRQHandler /* SPI2 */ - .word USART1_IRQHandler /* USART1 */ - .word USART2_IRQHandler /* USART2 */ - .word USART3_IRQHandler /* USART3 */ - .word EXTI15_10_IRQHandler /* External Line[15:10]s */ - .word RTC_Alarm_IRQHandler /* RTC Alarm (A and B) through EXTI Line */ - .word OTG_FS_WKUP_IRQHandler /* USB OTG FS Wakeup through EXTI line */ - .word TIM8_BRK_TIM12_IRQHandler /* TIM8 Break and TIM12 */ - .word TIM8_UP_TIM13_IRQHandler /* TIM8 Update and TIM13 */ + .word TIM1_CC_IRQHandler /* TIM1 Capture Compare */ + .word TIM2_IRQHandler /* TIM2 */ + .word TIM3_IRQHandler /* TIM3 */ + .word TIM4_IRQHandler /* TIM4 */ + .word I2C1_EV_IRQHandler /* I2C1 Event */ + .word I2C1_ER_IRQHandler /* I2C1 Error */ + .word I2C2_EV_IRQHandler /* I2C2 Event */ + .word I2C2_ER_IRQHandler /* I2C2 Error */ + .word SPI1_IRQHandler /* SPI1 */ + .word SPI2_IRQHandler /* SPI2 */ + .word USART1_IRQHandler /* USART1 */ + .word USART2_IRQHandler /* USART2 */ + .word USART3_IRQHandler /* USART3 */ + .word EXTI15_10_IRQHandler /* External Line[15:10]s */ + .word RTC_Alarm_IRQHandler /* RTC Alarm (A and B) through EXTI Line */ + .word OTG_FS_WKUP_IRQHandler /* USB OTG FS Wakeup through EXTI line */ + .word TIM8_BRK_TIM12_IRQHandler /* TIM8 Break and TIM12 */ + .word TIM8_UP_TIM13_IRQHandler /* TIM8 Update and TIM13 */ .word TIM8_TRG_COM_TIM14_IRQHandler /* TIM8 Trigger and Commutation and TIM14 */ - .word TIM8_CC_IRQHandler /* TIM8 Capture Compare */ - .word DMA1_Stream7_IRQHandler /* DMA1 Stream7 */ - .word FSMC_IRQHandler /* FSMC */ - .word SDIO_IRQHandler /* SDIO */ - .word TIM5_IRQHandler /* TIM5 */ - .word SPI3_IRQHandler /* SPI3 */ - .word UART4_IRQHandler /* UART4 */ - .word UART5_IRQHandler /* UART5 */ - .word TIM6_DAC_IRQHandler /* TIM6 and DAC1&2 underrun errors */ + .word TIM8_CC_IRQHandler /* TIM8 Capture Compare */ + .word DMA1_Stream7_IRQHandler /* DMA1 Stream7 */ + .word FSMC_IRQHandler /* FSMC */ + .word SDIO_IRQHandler /* SDIO */ + .word TIM5_IRQHandler /* TIM5 */ + .word SPI3_IRQHandler /* SPI3 */ + .word UART4_IRQHandler /* UART4 */ + .word UART5_IRQHandler /* UART5 */ + .word TIM6_DAC_IRQHandler /* TIM6 and DAC1&2 underrun errors */ .word TIM7_IRQHandler /* TIM7 */ - .word DMA2_Stream0_IRQHandler /* DMA2 Stream 0 */ - .word DMA2_Stream1_IRQHandler /* DMA2 Stream 1 */ - .word DMA2_Stream2_IRQHandler /* DMA2 Stream 2 */ - .word DMA2_Stream3_IRQHandler /* DMA2 Stream 3 */ - .word DMA2_Stream4_IRQHandler /* DMA2 Stream 4 */ - .word ETH_IRQHandler /* Ethernet */ - .word ETH_WKUP_IRQHandler /* Ethernet Wakeup through EXTI line */ - .word CAN2_TX_IRQHandler /* CAN2 TX */ - .word CAN2_RX0_IRQHandler /* CAN2 RX0 */ - .word CAN2_RX1_IRQHandler /* CAN2 RX1 */ - .word CAN2_SCE_IRQHandler /* CAN2 SCE */ - .word OTG_FS_IRQHandler /* USB OTG FS */ - .word DMA2_Stream5_IRQHandler /* DMA2 Stream 5 */ - .word DMA2_Stream6_IRQHandler /* DMA2 Stream 6 */ - .word DMA2_Stream7_IRQHandler /* DMA2 Stream 7 */ - .word USART6_IRQHandler /* USART6 */ - .word I2C3_EV_IRQHandler /* I2C3 event */ - .word I2C3_ER_IRQHandler /* I2C3 error */ - .word OTG_HS_EP1_OUT_IRQHandler /* USB OTG HS End Point 1 Out */ - .word OTG_HS_EP1_IN_IRQHandler /* USB OTG HS End Point 1 In */ - .word OTG_HS_WKUP_IRQHandler /* USB OTG HS Wakeup through EXTI */ - .word OTG_HS_IRQHandler /* USB OTG HS */ - .word DCMI_IRQHandler /* DCMI */ - .word CRYP_IRQHandler /* CRYP crypto */ + .word DMA2_Stream0_IRQHandler /* DMA2 Stream 0 */ + .word DMA2_Stream1_IRQHandler /* DMA2 Stream 1 */ + .word DMA2_Stream2_IRQHandler /* DMA2 Stream 2 */ + .word DMA2_Stream3_IRQHandler /* DMA2 Stream 3 */ + .word DMA2_Stream4_IRQHandler /* DMA2 Stream 4 */ + .word ETH_IRQHandler /* Ethernet */ + .word ETH_WKUP_IRQHandler /* Ethernet Wakeup through EXTI line */ + .word CAN2_TX_IRQHandler /* CAN2 TX */ + .word CAN2_RX0_IRQHandler /* CAN2 RX0 */ + .word CAN2_RX1_IRQHandler /* CAN2 RX1 */ + .word CAN2_SCE_IRQHandler /* CAN2 SCE */ + .word OTG_FS_IRQHandler /* USB OTG FS */ + .word DMA2_Stream5_IRQHandler /* DMA2 Stream 5 */ + .word DMA2_Stream6_IRQHandler /* DMA2 Stream 6 */ + .word DMA2_Stream7_IRQHandler /* DMA2 Stream 7 */ + .word USART6_IRQHandler /* USART6 */ + .word I2C3_EV_IRQHandler /* I2C3 event */ + .word I2C3_ER_IRQHandler /* I2C3 error */ + .word OTG_HS_EP1_OUT_IRQHandler /* USB OTG HS End Point 1 Out */ + .word OTG_HS_EP1_IN_IRQHandler /* USB OTG HS End Point 1 In */ + .word OTG_HS_WKUP_IRQHandler /* USB OTG HS Wakeup through EXTI */ + .word OTG_HS_IRQHandler /* USB OTG HS */ + .word DCMI_IRQHandler /* DCMI */ + .word CRYP_IRQHandler /* CRYP crypto */ .word HASH_RNG_IRQHandler /* Hash and Rng */ .word FPU_IRQHandler /* FPU */ - + /******************************************************************************* * -* Provide weak aliases for each Exception handler to the Default_Handler. -* As they are weak aliases, any function with the same name will override +* Provide weak aliases for each Exception handler to the Default_Handler. +* As they are weak aliases, any function with the same name will override * this definition. -* +* *******************************************************************************/ .weak NMI_Handler .thumb_set NMI_Handler,Default_Handler - + .weak HardFault_Handler .thumb_set HardFault_Handler,Default_Handler - + .weak MemManage_Handler .thumb_set MemManage_Handler,Default_Handler - + .weak BusFault_Handler .thumb_set BusFault_Handler,Default_Handler @@ -295,252 +308,252 @@ g_pfnVectors: .thumb_set PendSV_Handler,Default_Handler .weak SysTick_Handler - .thumb_set SysTick_Handler,Default_Handler - - .weak WWDG_IRQHandler - .thumb_set WWDG_IRQHandler,Default_Handler - - .weak PVD_IRQHandler + .thumb_set SysTick_Handler,Default_Handler + + .weak WWDG_IRQHandler + .thumb_set WWDG_IRQHandler,Default_Handler + + .weak PVD_IRQHandler .thumb_set PVD_IRQHandler,Default_Handler - - .weak TAMP_STAMP_IRQHandler + + .weak TAMP_STAMP_IRQHandler .thumb_set TAMP_STAMP_IRQHandler,Default_Handler - - .weak RTC_WKUP_IRQHandler + + .weak RTC_WKUP_IRQHandler .thumb_set RTC_WKUP_IRQHandler,Default_Handler - - .weak FLASH_IRQHandler + + .weak FLASH_IRQHandler .thumb_set FLASH_IRQHandler,Default_Handler - - .weak RCC_IRQHandler + + .weak RCC_IRQHandler .thumb_set RCC_IRQHandler,Default_Handler - - .weak EXTI0_IRQHandler + + .weak EXTI0_IRQHandler .thumb_set EXTI0_IRQHandler,Default_Handler - - .weak EXTI1_IRQHandler - .thumb_set EXTI1_IRQHandler,Default_Handler - - .weak EXTI2_IRQHandler - .thumb_set EXTI2_IRQHandler,Default_Handler - .weak EXTI3_IRQHandler + .weak EXTI1_IRQHandler + .thumb_set EXTI1_IRQHandler,Default_Handler + + .weak EXTI2_IRQHandler + .thumb_set EXTI2_IRQHandler,Default_Handler + + .weak EXTI3_IRQHandler .thumb_set EXTI3_IRQHandler,Default_Handler - - .weak EXTI4_IRQHandler + + .weak EXTI4_IRQHandler .thumb_set EXTI4_IRQHandler,Default_Handler - - .weak DMA1_Stream0_IRQHandler + + .weak DMA1_Stream0_IRQHandler .thumb_set DMA1_Stream0_IRQHandler,Default_Handler - - .weak DMA1_Stream1_IRQHandler + + .weak DMA1_Stream1_IRQHandler .thumb_set DMA1_Stream1_IRQHandler,Default_Handler - - .weak DMA1_Stream2_IRQHandler + + .weak DMA1_Stream2_IRQHandler .thumb_set DMA1_Stream2_IRQHandler,Default_Handler - - .weak DMA1_Stream3_IRQHandler - .thumb_set DMA1_Stream3_IRQHandler,Default_Handler - .weak DMA1_Stream4_IRQHandler + .weak DMA1_Stream3_IRQHandler + .thumb_set DMA1_Stream3_IRQHandler,Default_Handler + + .weak DMA1_Stream4_IRQHandler .thumb_set DMA1_Stream4_IRQHandler,Default_Handler - - .weak DMA1_Stream5_IRQHandler + + .weak DMA1_Stream5_IRQHandler .thumb_set DMA1_Stream5_IRQHandler,Default_Handler - - .weak DMA1_Stream6_IRQHandler + + .weak DMA1_Stream6_IRQHandler .thumb_set DMA1_Stream6_IRQHandler,Default_Handler - - .weak ADC_IRQHandler + + .weak ADC_IRQHandler .thumb_set ADC_IRQHandler,Default_Handler - - .weak CAN1_TX_IRQHandler + + .weak CAN1_TX_IRQHandler .thumb_set CAN1_TX_IRQHandler,Default_Handler - - .weak CAN1_RX0_IRQHandler + + .weak CAN1_RX0_IRQHandler .thumb_set CAN1_RX0_IRQHandler,Default_Handler - - .weak CAN1_RX1_IRQHandler + + .weak CAN1_RX1_IRQHandler .thumb_set CAN1_RX1_IRQHandler,Default_Handler - - .weak CAN1_SCE_IRQHandler + + .weak CAN1_SCE_IRQHandler .thumb_set CAN1_SCE_IRQHandler,Default_Handler - - .weak EXTI9_5_IRQHandler + + .weak EXTI9_5_IRQHandler .thumb_set EXTI9_5_IRQHandler,Default_Handler - - .weak TIM1_BRK_TIM9_IRQHandler + + .weak TIM1_BRK_TIM9_IRQHandler .thumb_set TIM1_BRK_TIM9_IRQHandler,Default_Handler - - .weak TIM1_UP_TIM10_IRQHandler + + .weak TIM1_UP_TIM10_IRQHandler .thumb_set TIM1_UP_TIM10_IRQHandler,Default_Handler - - .weak TIM1_TRG_COM_TIM11_IRQHandler + + .weak TIM1_TRG_COM_TIM11_IRQHandler .thumb_set TIM1_TRG_COM_TIM11_IRQHandler,Default_Handler - - .weak TIM1_CC_IRQHandler + + .weak TIM1_CC_IRQHandler .thumb_set TIM1_CC_IRQHandler,Default_Handler - - .weak TIM2_IRQHandler + + .weak TIM2_IRQHandler .thumb_set TIM2_IRQHandler,Default_Handler - - .weak TIM3_IRQHandler + + .weak TIM3_IRQHandler .thumb_set TIM3_IRQHandler,Default_Handler - - .weak TIM4_IRQHandler + + .weak TIM4_IRQHandler .thumb_set TIM4_IRQHandler,Default_Handler - - .weak I2C1_EV_IRQHandler + + .weak I2C1_EV_IRQHandler .thumb_set I2C1_EV_IRQHandler,Default_Handler - - .weak I2C1_ER_IRQHandler + + .weak I2C1_ER_IRQHandler .thumb_set I2C1_ER_IRQHandler,Default_Handler - - .weak I2C2_EV_IRQHandler + + .weak I2C2_EV_IRQHandler .thumb_set I2C2_EV_IRQHandler,Default_Handler - - .weak I2C2_ER_IRQHandler + + .weak I2C2_ER_IRQHandler .thumb_set I2C2_ER_IRQHandler,Default_Handler - - .weak SPI1_IRQHandler + + .weak SPI1_IRQHandler .thumb_set SPI1_IRQHandler,Default_Handler - - .weak SPI2_IRQHandler + + .weak SPI2_IRQHandler .thumb_set SPI2_IRQHandler,Default_Handler - - .weak USART1_IRQHandler + + .weak USART1_IRQHandler .thumb_set USART1_IRQHandler,Default_Handler - - .weak USART2_IRQHandler + + .weak USART2_IRQHandler .thumb_set USART2_IRQHandler,Default_Handler - - .weak USART3_IRQHandler + + .weak USART3_IRQHandler .thumb_set USART3_IRQHandler,Default_Handler - - .weak EXTI15_10_IRQHandler + + .weak EXTI15_10_IRQHandler .thumb_set EXTI15_10_IRQHandler,Default_Handler - - .weak RTC_Alarm_IRQHandler + + .weak RTC_Alarm_IRQHandler .thumb_set RTC_Alarm_IRQHandler,Default_Handler - - .weak OTG_FS_WKUP_IRQHandler + + .weak OTG_FS_WKUP_IRQHandler .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler - - .weak TIM8_BRK_TIM12_IRQHandler + + .weak TIM8_BRK_TIM12_IRQHandler .thumb_set TIM8_BRK_TIM12_IRQHandler,Default_Handler - - .weak TIM8_UP_TIM13_IRQHandler + + .weak TIM8_UP_TIM13_IRQHandler .thumb_set TIM8_UP_TIM13_IRQHandler,Default_Handler - - .weak TIM8_TRG_COM_TIM14_IRQHandler + + .weak TIM8_TRG_COM_TIM14_IRQHandler .thumb_set TIM8_TRG_COM_TIM14_IRQHandler,Default_Handler - - .weak TIM8_CC_IRQHandler + + .weak TIM8_CC_IRQHandler .thumb_set TIM8_CC_IRQHandler,Default_Handler - - .weak DMA1_Stream7_IRQHandler + + .weak DMA1_Stream7_IRQHandler .thumb_set DMA1_Stream7_IRQHandler,Default_Handler - - .weak FSMC_IRQHandler + + .weak FSMC_IRQHandler .thumb_set FSMC_IRQHandler,Default_Handler - - .weak SDIO_IRQHandler + + .weak SDIO_IRQHandler .thumb_set SDIO_IRQHandler,Default_Handler - - .weak TIM5_IRQHandler + + .weak TIM5_IRQHandler .thumb_set TIM5_IRQHandler,Default_Handler - - .weak SPI3_IRQHandler + + .weak SPI3_IRQHandler .thumb_set SPI3_IRQHandler,Default_Handler - - .weak UART4_IRQHandler + + .weak UART4_IRQHandler .thumb_set UART4_IRQHandler,Default_Handler - - .weak UART5_IRQHandler + + .weak UART5_IRQHandler .thumb_set UART5_IRQHandler,Default_Handler - - .weak TIM6_DAC_IRQHandler + + .weak TIM6_DAC_IRQHandler .thumb_set TIM6_DAC_IRQHandler,Default_Handler - - .weak TIM7_IRQHandler + + .weak TIM7_IRQHandler .thumb_set TIM7_IRQHandler,Default_Handler - - .weak DMA2_Stream0_IRQHandler + + .weak DMA2_Stream0_IRQHandler .thumb_set DMA2_Stream0_IRQHandler,Default_Handler - - .weak DMA2_Stream1_IRQHandler + + .weak DMA2_Stream1_IRQHandler .thumb_set DMA2_Stream1_IRQHandler,Default_Handler - - .weak DMA2_Stream2_IRQHandler + + .weak DMA2_Stream2_IRQHandler .thumb_set DMA2_Stream2_IRQHandler,Default_Handler - - .weak DMA2_Stream3_IRQHandler + + .weak DMA2_Stream3_IRQHandler .thumb_set DMA2_Stream3_IRQHandler,Default_Handler - - .weak DMA2_Stream4_IRQHandler + + .weak DMA2_Stream4_IRQHandler .thumb_set DMA2_Stream4_IRQHandler,Default_Handler - - .weak ETH_IRQHandler + + .weak ETH_IRQHandler .thumb_set ETH_IRQHandler,Default_Handler - - .weak ETH_WKUP_IRQHandler + + .weak ETH_WKUP_IRQHandler .thumb_set ETH_WKUP_IRQHandler,Default_Handler - - .weak CAN2_TX_IRQHandler + + .weak CAN2_TX_IRQHandler .thumb_set CAN2_TX_IRQHandler,Default_Handler - - .weak CAN2_RX0_IRQHandler + + .weak CAN2_RX0_IRQHandler .thumb_set CAN2_RX0_IRQHandler,Default_Handler - - .weak CAN2_RX1_IRQHandler + + .weak CAN2_RX1_IRQHandler .thumb_set CAN2_RX1_IRQHandler,Default_Handler - - .weak CAN2_SCE_IRQHandler + + .weak CAN2_SCE_IRQHandler .thumb_set CAN2_SCE_IRQHandler,Default_Handler - - .weak OTG_FS_IRQHandler + + .weak OTG_FS_IRQHandler .thumb_set OTG_FS_IRQHandler,Default_Handler - - .weak DMA2_Stream5_IRQHandler + + .weak DMA2_Stream5_IRQHandler .thumb_set DMA2_Stream5_IRQHandler,Default_Handler - - .weak DMA2_Stream6_IRQHandler + + .weak DMA2_Stream6_IRQHandler .thumb_set DMA2_Stream6_IRQHandler,Default_Handler - - .weak DMA2_Stream7_IRQHandler + + .weak DMA2_Stream7_IRQHandler .thumb_set DMA2_Stream7_IRQHandler,Default_Handler - - .weak USART6_IRQHandler + + .weak USART6_IRQHandler .thumb_set USART6_IRQHandler,Default_Handler - - .weak I2C3_EV_IRQHandler + + .weak I2C3_EV_IRQHandler .thumb_set I2C3_EV_IRQHandler,Default_Handler - - .weak I2C3_ER_IRQHandler + + .weak I2C3_ER_IRQHandler .thumb_set I2C3_ER_IRQHandler,Default_Handler - - .weak OTG_HS_EP1_OUT_IRQHandler + + .weak OTG_HS_EP1_OUT_IRQHandler .thumb_set OTG_HS_EP1_OUT_IRQHandler,Default_Handler - - .weak OTG_HS_EP1_IN_IRQHandler + + .weak OTG_HS_EP1_IN_IRQHandler .thumb_set OTG_HS_EP1_IN_IRQHandler,Default_Handler - - .weak OTG_HS_WKUP_IRQHandler + + .weak OTG_HS_WKUP_IRQHandler .thumb_set OTG_HS_WKUP_IRQHandler,Default_Handler - - .weak OTG_HS_IRQHandler + + .weak OTG_HS_IRQHandler .thumb_set OTG_HS_IRQHandler,Default_Handler - - .weak DCMI_IRQHandler + + .weak DCMI_IRQHandler .thumb_set DCMI_IRQHandler,Default_Handler - - .weak CRYP_IRQHandler + + .weak CRYP_IRQHandler .thumb_set CRYP_IRQHandler,Default_Handler - - .weak HASH_RNG_IRQHandler - .thumb_set HASH_RNG_IRQHandler,Default_Handler + + .weak HASH_RNG_IRQHandler + .thumb_set HASH_RNG_IRQHandler,Default_Handler - .weak FPU_IRQHandler - .thumb_set FPU_IRQHandler,Default_Handler - + .weak FPU_IRQHandler + .thumb_set FPU_IRQHandler,Default_Handler + /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/startup/startup_stm32f411xe.s b/src/main/startup/startup_stm32f411xe.s index f8c460a546b..4714dfe71bc 100644 --- a/src/main/startup/startup_stm32f411xe.s +++ b/src/main/startup/startup_stm32f411xe.s @@ -36,7 +36,7 @@ * ****************************************************************************** */ - + .syntax unified .cpu cortex-m4 .fpu softvfp @@ -46,10 +46,10 @@ .global Default_Handler .global irq_stack -/* start address for the initialization values of the .data section. +/* start address for the initialization values of the .data section. defined in linker script */ .word _sidata -/* start address for the .data section. defined in linker script */ +/* start address for the .data section. defined in linker script */ .word _sdata /* end address for the .data section. defined in linker script */ .word _edata @@ -63,7 +63,7 @@ defined in linker script */ * @brief This is the code that gets called when the processor first * starts execution following a reset event. Only the absolutely * necessary set is performed, after which the application - * supplied main() routine is called. + * supplied main() routine is called. * @param None * @retval : None */ @@ -71,7 +71,7 @@ defined in linker script */ .section .text.Reset_Handler .weak Reset_Handler .type Reset_Handler, %function -Reset_Handler: +Reset_Handler: ldr r0, =0x2001FFFC // mj666 ldr r1, =0xDEADBEEF // mj666 ldr r2, [r0, #0] // mj666 @@ -79,7 +79,7 @@ Reset_Handler: cmp r2, r1 // mj666 beq Reboot_Loader // mj666 -/* Copy the data segment initializers from flash to SRAM */ +/* Copy the data segment initializers from flash to SRAM */ movs r1, #0 b LoopCopyDataInit @@ -88,7 +88,7 @@ CopyDataInit: ldr r3, [r3, r1] str r3, [r0, r1] adds r1, r1, #4 - + LoopCopyDataInit: ldr r0, =_sdata ldr r3, =_edata @@ -97,16 +97,29 @@ LoopCopyDataInit: bcc CopyDataInit ldr r2, =_sbss b LoopFillZerobss -/* Zero fill the bss segment. */ +/* Zero fill the bss segment. */ FillZerobss: movs r3, #0 str r3, [r2], #4 - + LoopFillZerobss: ldr r3, = _ebss cmp r2, r3 bcc FillZerobss +/* Mark the heap and stack */ + ldr r2, =_heap_stack_begin + b LoopMarkHeapStack + +MarkHeapStack: + movs r3, 0xa5a5a5a5 + str r3, [r2], #4 + +LoopMarkHeapStack: + ldr r3, = _heap_stack_end + cmp r2, r3 + bcc MarkHeapStack + /*FPU settings*/ ldr r0, =0xE000ED88 /* Enable CP10,CP11 */ ldr r1,[r0] @@ -114,11 +127,11 @@ LoopFillZerobss: str r1,[r0] /* Call the clock system intitialization function.*/ - bl SystemInit + bl SystemInit /* Call the application's entry point.*/ bl main - bx lr + bx lr LoopForever: b LoopForever @@ -133,11 +146,11 @@ Reboot_Loader: // mj666 .size Reset_Handler, .-Reset_Handler /** - * @brief This is the code that gets called when the processor receives an + * @brief This is the code that gets called when the processor receives an * unexpected interrupt. This simply enters an infinite loop, preserving * the system state for examination by a debugger. - * @param None - * @retval None + * @param None + * @retval None */ .section .text.Default_Handler,"ax",%progbits Default_Handler: @@ -149,7 +162,7 @@ Infinite_Loop: * The minimal vector table for a Cortex M3. Note that the proper constructs * must be placed on this to ensure that it ends up at physical address * 0x0000.0000. -* +* *******************************************************************************/ .section .irqstack,"aw",%progbits irq_stack: @@ -158,7 +171,7 @@ Infinite_Loop: .section .isr_vector,"a",%progbits .type g_pfnVectors, %object .size g_pfnVectors, .-g_pfnVectors - + g_pfnVectors: .word irq_stack+1024 @@ -177,82 +190,82 @@ g_pfnVectors: .word 0 .word PendSV_Handler .word SysTick_Handler - + /* External Interrupts */ - .word WWDG_IRQHandler /* Window WatchDog */ - .word PVD_IRQHandler /* PVD through EXTI Line detection */ - .word TAMP_STAMP_IRQHandler /* Tamper and TimeStamps through the EXTI line */ - .word RTC_WKUP_IRQHandler /* RTC Wakeup through the EXTI line */ - .word FLASH_IRQHandler /* FLASH */ - .word RCC_IRQHandler /* RCC */ - .word EXTI0_IRQHandler /* EXTI Line0 */ - .word EXTI1_IRQHandler /* EXTI Line1 */ - .word EXTI2_IRQHandler /* EXTI Line2 */ - .word EXTI3_IRQHandler /* EXTI Line3 */ - .word EXTI4_IRQHandler /* EXTI Line4 */ - .word DMA1_Stream0_IRQHandler /* DMA1 Stream 0 */ - .word DMA1_Stream1_IRQHandler /* DMA1 Stream 1 */ - .word DMA1_Stream2_IRQHandler /* DMA1 Stream 2 */ - .word DMA1_Stream3_IRQHandler /* DMA1 Stream 3 */ - .word DMA1_Stream4_IRQHandler /* DMA1 Stream 4 */ - .word DMA1_Stream5_IRQHandler /* DMA1 Stream 5 */ - .word DMA1_Stream6_IRQHandler /* DMA1 Stream 6 */ + .word WWDG_IRQHandler /* Window WatchDog */ + .word PVD_IRQHandler /* PVD through EXTI Line detection */ + .word TAMP_STAMP_IRQHandler /* Tamper and TimeStamps through the EXTI line */ + .word RTC_WKUP_IRQHandler /* RTC Wakeup through the EXTI line */ + .word FLASH_IRQHandler /* FLASH */ + .word RCC_IRQHandler /* RCC */ + .word EXTI0_IRQHandler /* EXTI Line0 */ + .word EXTI1_IRQHandler /* EXTI Line1 */ + .word EXTI2_IRQHandler /* EXTI Line2 */ + .word EXTI3_IRQHandler /* EXTI Line3 */ + .word EXTI4_IRQHandler /* EXTI Line4 */ + .word DMA1_Stream0_IRQHandler /* DMA1 Stream 0 */ + .word DMA1_Stream1_IRQHandler /* DMA1 Stream 1 */ + .word DMA1_Stream2_IRQHandler /* DMA1 Stream 2 */ + .word DMA1_Stream3_IRQHandler /* DMA1 Stream 3 */ + .word DMA1_Stream4_IRQHandler /* DMA1 Stream 4 */ + .word DMA1_Stream5_IRQHandler /* DMA1 Stream 5 */ + .word DMA1_Stream6_IRQHandler /* DMA1 Stream 6 */ .word ADC_IRQHandler /* ADC1, ADC2 and ADC3s */ .word 0 /* CAN1 TX */ .word 0 /* CAN1 RX0 */ .word 0 /* CAN1 RX1 */ .word 0 /* CAN1 SCE */ - .word EXTI9_5_IRQHandler /* External Line[9:5]s */ - .word TIM1_BRK_TIM9_IRQHandler /* TIM1 Break and TIM9 */ - .word TIM1_UP_TIM10_IRQHandler /* TIM1 Update and TIM10 */ + .word EXTI9_5_IRQHandler /* External Line[9:5]s */ + .word TIM1_BRK_TIM9_IRQHandler /* TIM1 Break and TIM9 */ + .word TIM1_UP_TIM10_IRQHandler /* TIM1 Update and TIM10 */ .word TIM1_TRG_COM_TIM11_IRQHandler /* TIM1 Trigger and Commutation and TIM11 */ - .word TIM1_CC_IRQHandler /* TIM1 Capture Compare */ - .word TIM2_IRQHandler /* TIM2 */ - .word TIM3_IRQHandler /* TIM3 */ - .word TIM4_IRQHandler /* TIM4 */ - .word I2C1_EV_IRQHandler /* I2C1 Event */ - .word I2C1_ER_IRQHandler /* I2C1 Error */ - .word I2C2_EV_IRQHandler /* I2C2 Event */ - .word I2C2_ER_IRQHandler /* I2C2 Error */ - .word SPI1_IRQHandler /* SPI1 */ - .word SPI2_IRQHandler /* SPI2 */ - .word USART1_IRQHandler /* USART1 */ - .word USART2_IRQHandler /* USART2 */ + .word TIM1_CC_IRQHandler /* TIM1 Capture Compare */ + .word TIM2_IRQHandler /* TIM2 */ + .word TIM3_IRQHandler /* TIM3 */ + .word TIM4_IRQHandler /* TIM4 */ + .word I2C1_EV_IRQHandler /* I2C1 Event */ + .word I2C1_ER_IRQHandler /* I2C1 Error */ + .word I2C2_EV_IRQHandler /* I2C2 Event */ + .word I2C2_ER_IRQHandler /* I2C2 Error */ + .word SPI1_IRQHandler /* SPI1 */ + .word SPI2_IRQHandler /* SPI2 */ + .word USART1_IRQHandler /* USART1 */ + .word USART2_IRQHandler /* USART2 */ .word 0 /* USART3 */ - .word EXTI15_10_IRQHandler /* External Line[15:10]s */ - .word RTC_Alarm_IRQHandler /* RTC Alarm (A and B) through EXTI Line */ - .word OTG_FS_WKUP_IRQHandler /* USB OTG FS Wakeup through EXTI line */ + .word EXTI15_10_IRQHandler /* External Line[15:10]s */ + .word RTC_Alarm_IRQHandler /* RTC Alarm (A and B) through EXTI Line */ + .word OTG_FS_WKUP_IRQHandler /* USB OTG FS Wakeup through EXTI line */ .word 0 /* TIM8 Break and TIM12 */ .word 0 /* TIM8 Update and TIM13 */ .word 0 /* TIM8 Trigger and Commutation and TIM14 */ .word 0 /* TIM8 Capture Compare */ - .word DMA1_Stream7_IRQHandler /* DMA1 Stream7 */ + .word DMA1_Stream7_IRQHandler /* DMA1 Stream7 */ .word 0 /* FSMC */ - .word SDIO_IRQHandler /* SDIO */ - .word TIM5_IRQHandler /* TIM5 */ - .word SPI3_IRQHandler /* SPI3 */ + .word SDIO_IRQHandler /* SDIO */ + .word TIM5_IRQHandler /* TIM5 */ + .word SPI3_IRQHandler /* SPI3 */ .word 0 /* UART4 */ .word 0 /* UART5 */ .word 0 /* TIM6 and DAC1&2 underrun errors */ .word 0 /* TIM7 */ - .word DMA2_Stream0_IRQHandler /* DMA2 Stream 0 */ - .word DMA2_Stream1_IRQHandler /* DMA2 Stream 1 */ - .word DMA2_Stream2_IRQHandler /* DMA2 Stream 2 */ - .word DMA2_Stream3_IRQHandler /* DMA2 Stream 3 */ - .word DMA2_Stream4_IRQHandler /* DMA2 Stream 4 */ + .word DMA2_Stream0_IRQHandler /* DMA2 Stream 0 */ + .word DMA2_Stream1_IRQHandler /* DMA2 Stream 1 */ + .word DMA2_Stream2_IRQHandler /* DMA2 Stream 2 */ + .word DMA2_Stream3_IRQHandler /* DMA2 Stream 3 */ + .word DMA2_Stream4_IRQHandler /* DMA2 Stream 4 */ .word 0 /* Ethernet */ .word 0 /* Ethernet Wakeup through EXTI line */ .word 0 /* CAN2 TX */ .word 0 /* CAN2 RX0 */ .word 0 /* CAN2 RX1 */ .word 0 /* CAN2 SCE */ - .word OTG_FS_IRQHandler /* USB OTG FS */ - .word DMA2_Stream5_IRQHandler /* DMA2 Stream 5 */ - .word DMA2_Stream6_IRQHandler /* DMA2 Stream 6 */ - .word DMA2_Stream7_IRQHandler /* DMA2 Stream 7 */ - .word USART6_IRQHandler /* USART6 */ - .word I2C3_EV_IRQHandler /* I2C3 event */ - .word I2C3_ER_IRQHandler /* I2C3 error */ + .word OTG_FS_IRQHandler /* USB OTG FS */ + .word DMA2_Stream5_IRQHandler /* DMA2 Stream 5 */ + .word DMA2_Stream6_IRQHandler /* DMA2 Stream 6 */ + .word DMA2_Stream7_IRQHandler /* DMA2 Stream 7 */ + .word USART6_IRQHandler /* USART6 */ + .word I2C3_EV_IRQHandler /* I2C3 event */ + .word I2C3_ER_IRQHandler /* I2C3 error */ .word 0 /* USB OTG HS End Point 1 Out */ .word 0 /* USB OTG HS End Point 1 In */ .word 0 /* USB OTG HS Wakeup through EXTI */ @@ -267,20 +280,20 @@ g_pfnVectors: .word SPI5_IRQHandler /* SPI5 */ /******************************************************************************* * -* Provide weak aliases for each Exception handler to the Default_Handler. -* As they are weak aliases, any function with the same name will override +* Provide weak aliases for each Exception handler to the Default_Handler. +* As they are weak aliases, any function with the same name will override * this definition. -* +* *******************************************************************************/ .weak NMI_Handler .thumb_set NMI_Handler,Default_Handler - + .weak HardFault_Handler .thumb_set HardFault_Handler,Default_Handler - + .weak MemManage_Handler .thumb_set MemManage_Handler,Default_Handler - + .weak BusFault_Handler .thumb_set BusFault_Handler,Default_Handler @@ -297,172 +310,172 @@ g_pfnVectors: .thumb_set PendSV_Handler,Default_Handler .weak SysTick_Handler - .thumb_set SysTick_Handler,Default_Handler - - .weak WWDG_IRQHandler - .thumb_set WWDG_IRQHandler,Default_Handler - - .weak PVD_IRQHandler + .thumb_set SysTick_Handler,Default_Handler + + .weak WWDG_IRQHandler + .thumb_set WWDG_IRQHandler,Default_Handler + + .weak PVD_IRQHandler .thumb_set PVD_IRQHandler,Default_Handler - - .weak TAMP_STAMP_IRQHandler + + .weak TAMP_STAMP_IRQHandler .thumb_set TAMP_STAMP_IRQHandler,Default_Handler - - .weak RTC_WKUP_IRQHandler + + .weak RTC_WKUP_IRQHandler .thumb_set RTC_WKUP_IRQHandler,Default_Handler - - .weak FLASH_IRQHandler + + .weak FLASH_IRQHandler .thumb_set FLASH_IRQHandler,Default_Handler - - .weak RCC_IRQHandler + + .weak RCC_IRQHandler .thumb_set RCC_IRQHandler,Default_Handler - - .weak EXTI0_IRQHandler + + .weak EXTI0_IRQHandler .thumb_set EXTI0_IRQHandler,Default_Handler - - .weak EXTI1_IRQHandler - .thumb_set EXTI1_IRQHandler,Default_Handler - - .weak EXTI2_IRQHandler - .thumb_set EXTI2_IRQHandler,Default_Handler - .weak EXTI3_IRQHandler + .weak EXTI1_IRQHandler + .thumb_set EXTI1_IRQHandler,Default_Handler + + .weak EXTI2_IRQHandler + .thumb_set EXTI2_IRQHandler,Default_Handler + + .weak EXTI3_IRQHandler .thumb_set EXTI3_IRQHandler,Default_Handler - - .weak EXTI4_IRQHandler + + .weak EXTI4_IRQHandler .thumb_set EXTI4_IRQHandler,Default_Handler - - .weak DMA1_Stream0_IRQHandler + + .weak DMA1_Stream0_IRQHandler .thumb_set DMA1_Stream0_IRQHandler,Default_Handler - - .weak DMA1_Stream1_IRQHandler + + .weak DMA1_Stream1_IRQHandler .thumb_set DMA1_Stream1_IRQHandler,Default_Handler - - .weak DMA1_Stream2_IRQHandler + + .weak DMA1_Stream2_IRQHandler .thumb_set DMA1_Stream2_IRQHandler,Default_Handler - - .weak DMA1_Stream3_IRQHandler - .thumb_set DMA1_Stream3_IRQHandler,Default_Handler - .weak DMA1_Stream4_IRQHandler + .weak DMA1_Stream3_IRQHandler + .thumb_set DMA1_Stream3_IRQHandler,Default_Handler + + .weak DMA1_Stream4_IRQHandler .thumb_set DMA1_Stream4_IRQHandler,Default_Handler - - .weak DMA1_Stream5_IRQHandler + + .weak DMA1_Stream5_IRQHandler .thumb_set DMA1_Stream5_IRQHandler,Default_Handler - - .weak DMA1_Stream6_IRQHandler + + .weak DMA1_Stream6_IRQHandler .thumb_set DMA1_Stream6_IRQHandler,Default_Handler - - .weak ADC_IRQHandler + + .weak ADC_IRQHandler .thumb_set ADC_IRQHandler,Default_Handler - .weak EXTI9_5_IRQHandler + .weak EXTI9_5_IRQHandler .thumb_set EXTI9_5_IRQHandler,Default_Handler - - .weak TIM1_BRK_TIM9_IRQHandler + + .weak TIM1_BRK_TIM9_IRQHandler .thumb_set TIM1_BRK_TIM9_IRQHandler,Default_Handler - - .weak TIM1_UP_TIM10_IRQHandler + + .weak TIM1_UP_TIM10_IRQHandler .thumb_set TIM1_UP_TIM10_IRQHandler,Default_Handler - - .weak TIM1_TRG_COM_TIM11_IRQHandler + + .weak TIM1_TRG_COM_TIM11_IRQHandler .thumb_set TIM1_TRG_COM_TIM11_IRQHandler,Default_Handler - - .weak TIM1_CC_IRQHandler + + .weak TIM1_CC_IRQHandler .thumb_set TIM1_CC_IRQHandler,Default_Handler - - .weak TIM2_IRQHandler + + .weak TIM2_IRQHandler .thumb_set TIM2_IRQHandler,Default_Handler - - .weak TIM3_IRQHandler + + .weak TIM3_IRQHandler .thumb_set TIM3_IRQHandler,Default_Handler - - .weak TIM4_IRQHandler + + .weak TIM4_IRQHandler .thumb_set TIM4_IRQHandler,Default_Handler - - .weak I2C1_EV_IRQHandler + + .weak I2C1_EV_IRQHandler .thumb_set I2C1_EV_IRQHandler,Default_Handler - - .weak I2C1_ER_IRQHandler + + .weak I2C1_ER_IRQHandler .thumb_set I2C1_ER_IRQHandler,Default_Handler - - .weak I2C2_EV_IRQHandler + + .weak I2C2_EV_IRQHandler .thumb_set I2C2_EV_IRQHandler,Default_Handler - - .weak I2C2_ER_IRQHandler + + .weak I2C2_ER_IRQHandler .thumb_set I2C2_ER_IRQHandler,Default_Handler - - .weak SPI1_IRQHandler + + .weak SPI1_IRQHandler .thumb_set SPI1_IRQHandler,Default_Handler - - .weak SPI2_IRQHandler + + .weak SPI2_IRQHandler .thumb_set SPI2_IRQHandler,Default_Handler - - .weak USART1_IRQHandler + + .weak USART1_IRQHandler .thumb_set USART1_IRQHandler,Default_Handler - - .weak USART2_IRQHandler + + .weak USART2_IRQHandler .thumb_set USART2_IRQHandler,Default_Handler - .weak EXTI15_10_IRQHandler + .weak EXTI15_10_IRQHandler .thumb_set EXTI15_10_IRQHandler,Default_Handler - - .weak RTC_Alarm_IRQHandler + + .weak RTC_Alarm_IRQHandler .thumb_set RTC_Alarm_IRQHandler,Default_Handler - - .weak OTG_FS_WKUP_IRQHandler + + .weak OTG_FS_WKUP_IRQHandler .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler - .weak DMA1_Stream7_IRQHandler + .weak DMA1_Stream7_IRQHandler .thumb_set DMA1_Stream7_IRQHandler,Default_Handler - .weak SDIO_IRQHandler + .weak SDIO_IRQHandler .thumb_set SDIO_IRQHandler,Default_Handler - - .weak TIM5_IRQHandler + + .weak TIM5_IRQHandler .thumb_set TIM5_IRQHandler,Default_Handler - - .weak SPI3_IRQHandler + + .weak SPI3_IRQHandler .thumb_set SPI3_IRQHandler,Default_Handler - .weak DMA2_Stream0_IRQHandler + .weak DMA2_Stream0_IRQHandler .thumb_set DMA2_Stream0_IRQHandler,Default_Handler - - .weak DMA2_Stream1_IRQHandler + + .weak DMA2_Stream1_IRQHandler .thumb_set DMA2_Stream1_IRQHandler,Default_Handler - - .weak DMA2_Stream2_IRQHandler + + .weak DMA2_Stream2_IRQHandler .thumb_set DMA2_Stream2_IRQHandler,Default_Handler - - .weak DMA2_Stream3_IRQHandler + + .weak DMA2_Stream3_IRQHandler .thumb_set DMA2_Stream3_IRQHandler,Default_Handler - - .weak DMA2_Stream4_IRQHandler + + .weak DMA2_Stream4_IRQHandler .thumb_set DMA2_Stream4_IRQHandler,Default_Handler - .weak OTG_FS_IRQHandler + .weak OTG_FS_IRQHandler .thumb_set OTG_FS_IRQHandler,Default_Handler - - .weak DMA2_Stream5_IRQHandler + + .weak DMA2_Stream5_IRQHandler .thumb_set DMA2_Stream5_IRQHandler,Default_Handler - - .weak DMA2_Stream6_IRQHandler + + .weak DMA2_Stream6_IRQHandler .thumb_set DMA2_Stream6_IRQHandler,Default_Handler - - .weak DMA2_Stream7_IRQHandler + + .weak DMA2_Stream7_IRQHandler .thumb_set DMA2_Stream7_IRQHandler,Default_Handler - - .weak USART6_IRQHandler + + .weak USART6_IRQHandler .thumb_set USART6_IRQHandler,Default_Handler - - .weak I2C3_EV_IRQHandler + + .weak I2C3_EV_IRQHandler .thumb_set I2C3_EV_IRQHandler,Default_Handler - - .weak I2C3_ER_IRQHandler + + .weak I2C3_ER_IRQHandler .thumb_set I2C3_ER_IRQHandler,Default_Handler - .weak FPU_IRQHandler - .thumb_set FPU_IRQHandler,Default_Handler + .weak FPU_IRQHandler + .thumb_set FPU_IRQHandler,Default_Handler .weak SPI4_IRQHandler .thumb_set SPI4_IRQHandler,Default_Handler diff --git a/src/main/target/AIRHEROF3/AIRHEROF3_QUAD.mk b/src/main/target/AIRHEROF3/AIRHEROF3_QUAD.mk new file mode 100755 index 00000000000..8b137891791 --- /dev/null +++ b/src/main/target/AIRHEROF3/AIRHEROF3_QUAD.mk @@ -0,0 +1 @@ + diff --git a/src/main/target/AIRHEROF3/README.md b/src/main/target/AIRHEROF3/README.md new file mode 100755 index 00000000000..3b5df11b91a --- /dev/null +++ b/src/main/target/AIRHEROF3/README.md @@ -0,0 +1,10 @@ +# AIRHEROF3 + +* This is the AIR3 PARIS Sirius AirHERO 32 F3 board from MultiWiiCopter +* Source: http://www.multiwiicopter.com/products/air-hero-32-f3-spi-flight-controller + +## HW info + +* STM32F303RCT6 +* MPU6500 SPI +* BMP280 baro SPI \ No newline at end of file diff --git a/src/main/target/AIRHEROF3/target.c b/src/main/target/AIRHEROF3/target.c new file mode 100755 index 00000000000..08584ae4c41 --- /dev/null +++ b/src/main/target/AIRHEROF3/target.c @@ -0,0 +1,118 @@ +/* + * 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 "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const uint16_t multiPPM[] = { + PWM4 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), +#if !defined(AIRHEROF3_QUAD) + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), +#endif + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), +#if !defined(AIRHEROF3_QUAD) + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 +#endif + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM4 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), +#if !defined(AIRHEROF3_QUAD) + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 +#endif + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), +#if !defined(AIRHEROF3_QUAD) + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 +#endif + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM8 - RC8 + + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_11}, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM12 - OUT4 +#if !defined(AIRHEROF3_QUAD) + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2} // PWM14 - OUT6 +#endif +}; diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h new file mode 100755 index 00000000000..568de7bb7ab --- /dev/null +++ b/src/main/target/AIRHEROF3/target.h @@ -0,0 +1,135 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "AIR3" + +#define LED0 PB3 +#define LED1 PB4 + +#define BEEPER PA12 +#define BEEPER_INVERTED + +#define USE_EXTI +#define MPU_INT_EXTI PC13 +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU INT +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_SPI +#define USE_SPI_DEVICE_2 + +#define MPU6500_CS_PIN PB12 +#define MPU6500_SPI_INSTANCE SPI2 + +#define BMP280_SPI_INSTANCE SPI2 +#define BMP280_CS_PIN PB5 + +#define GYRO +#define USE_GYRO_SPI_MPU6500 +#define GYRO_MPU6500_ALIGN CW0_DEG + +#define ACC +#define USE_ACC_SPI_MPU6500 +#define ACC_MPU6500_ALIGN CW0_DEG + +#define BARO +#define USE_BARO_SPI_BMP280 + +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_SOFTSERIAL1 +#define USE_SOFTSERIAL2 +#define SERIAL_PORT_COUNT 5 + +#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 +#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 +#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) + +#define BOARD_HAS_VOLTAGE_DIVIDER +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 + +#define LED_STRIP +#define WS2811_TIMER TIM3 +#define WS2811_PIN PA6 +#define WS2811_DMA_CHANNEL DMA1_Channel6 +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER + +#define GPS + +#define NAV +#define NAV_AUTO_MAG_DECLINATION +#define NAV_GPS_GLITCH_DETECTION +#define NAV_MAX_WAYPOINTS 60 + +#define DEFAULT_FEATURES FEATURE_VBAT +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 +#define RX_CHANNELS_TAER + +#ifdef AIRHEROF3_QUAD + #define USE_I2C + #define I2C_DEVICE (I2CDEV_1) // SDA (PB9/AF4), SCL (PB8/AF4) + + #define I2C1_SCL_PIN PB8 + #define I2C1_SDA_PIN PB9 + + // MAG is I2C, so it is only useful when I2C is available + #define MAG + #define USE_MAG_HMC5883 + #define USE_MAG_MAG3110 + + #define MAX_PWM_OUTPUT_PORTS 8 + #define TARGET_MOTOR_COUNT 4 + #define USABLE_TIMER_CHANNEL_COUNT 12 +#else + #define MAX_PWM_OUTPUT_PORTS 10 + #define TARGET_MOTOR_COUNT 6 + #define USABLE_TIMER_CHANNEL_COUNT 14 +#endif + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTF (BIT(4)) + +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/AIRHEROF3/target.mk b/src/main/target/AIRHEROF3/target.mk new file mode 100755 index 00000000000..a0c660bb292 --- /dev/null +++ b/src/main/target/AIRHEROF3/target.mk @@ -0,0 +1,15 @@ +F3_TARGETS += $(TARGET) +HSE_VALUE = 12000000 + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_spi_bmp280.c \ + drivers/compass_hmc5883l.c \ + drivers/compass_mag3110.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/sonar_hcsr04.c \ + drivers/serial_softserial.c diff --git a/src/main/target/ALIENWIIF3/target.h b/src/main/target/ALIENWIIF3/target.h new file mode 100644 index 00000000000..ba7c9678020 --- /dev/null +++ b/src/main/target/ALIENWIIF3/target.h @@ -0,0 +1,140 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "AWF3" // AlienWii32 F3. + +#define LED0 +#define LED0_GPIO GPIOB +#define LED0_PIN Pin_4 // Blue LEDs - PB4 +#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB + +#define LED1 +#define LED1_GPIO GPIOB +#define LED1_PIN Pin_5 // Green LEDs - PB5 +#define LED1_PERIPHERAL RCC_AHBPeriph_GPIOB + +#define BEEPER +#define BEEP_GPIO GPIOA +#define BEEP_PIN Pin_5 // White LEDs - PA5 +#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOA + +#define USABLE_TIMER_CHANNEL_COUNT 11 + +// Using MPU6050 for the moment. +#define GYRO +#define USE_GYRO_MPU6050 + +#define GYRO_MPU6050_ALIGN CW270_DEG + +#define ACC +#define USE_ACC_MPU6050 + +#define ACC_MPU6050_ALIGN CW270_DEG + +// No baro support. +//#define BARO +//#define USE_BARO_MS5611 + +// No mag support for now (option to use MPU9150 in the future). +//#define MAG +//#define USE_MAG_AK8975 + +#define MAG_AK8975_ALIGN CW0_DEG_FLIP + +#define USE_VCP +#define USE_USART1 // Not connected - TX (PB6) RX PB7 (AF7) +#define USE_USART2 // Receiver - RX (PA3) +#define USE_USART3 // Not connected - 10/RX (PB11) 11/TX (PB10) +#define SERIAL_PORT_COUNT 4 + +#define UART1_TX_PIN GPIO_Pin_6 // PB6 +#define UART1_RX_PIN GPIO_Pin_7 // PB7 +#define UART1_GPIO GPIOB +#define UART1_GPIO_AF GPIO_AF_7 +#define UART1_TX_PINSOURCE GPIO_PinSource6 +#define UART1_RX_PINSOURCE GPIO_PinSource7 + +#define UART2_TX_PIN GPIO_Pin_2 // PA2 +#define UART2_RX_PIN GPIO_Pin_3 // PA3 +#define UART2_GPIO GPIOA +#define UART2_GPIO_AF GPIO_AF_7 +#define UART2_TX_PINSOURCE GPIO_PinSource2 +#define UART2_RX_PINSOURCE GPIO_PinSource3 + +#define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7) +#define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7) +#define UART3_GPIO_AF GPIO_AF_7 +#define UART3_GPIO GPIOB +#define UART3_TX_PINSOURCE GPIO_PinSource10 +#define UART3_RX_PINSOURCE GPIO_PinSource11 + + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) + +#define I2C2_SCL_GPIO GPIOA +#define I2C2_SCL_GPIO_AF GPIO_AF_4 +#define I2C2_SCL_PIN GPIO_Pin_9 +#define I2C2_SCL_PIN_SOURCE GPIO_PinSource9 +#define I2C2_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOA +#define I2C2_SDA_GPIO GPIOA +#define I2C2_SDA_GPIO_AF GPIO_AF_4 +#define I2C2_SDA_PIN GPIO_Pin_10 +#define I2C2_SDA_PIN_SOURCE GPIO_PinSource10 +#define I2C2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA + +#define USE_ADC + +#define ADC_INSTANCE ADC2 +#define ADC_DMA_CHANNEL DMA2_Channel1 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 + +//#define BOARD_HAS_VOLTAGE_DIVIDER + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_CHANNEL ADC_Channel_1 + +#define SPEKTRUM_BIND +// USART2, PA3 +#define BIND_PORT GPIOA +#define BIND_PIN Pin_3 + +// alternative defaults for AlienWii32 F3 target +#define ALIENWII32 +#define HARDWARE_BIND_PLUG + +// Hardware bind plug at PB12 (Pin 25) +#define BINDPLUG_PORT GPIOB +#define BINDPLUG_PIN Pin_12 + +#undef BLACKBOX + +#undef GPS +#undef GPS_PROTO_NMEA +#undef GPS_PROTO_UBLOX +#undef GPS_PROTO_I2C_NAV +#undef GPS_PROTO_NAZA + +#undef TELEMETRY +#undef TELEMETRY_FRSKY +#undef TELEMETRY_HOTT +#undef TELEMETRY_SMARTPORT +#undef TELEMETRY_LTM +#undef TELEMETRY_IBUS diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h index e941ce8d5c7..f8bd57f84fc 100644 --- a/src/main/target/ANYFC/target.h +++ b/src/main/target/ANYFC/target.h @@ -126,8 +126,6 @@ #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_ONESHOT125) - #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c index 6c1462c43e3..a058cda1b82 100644 --- a/src/main/target/BLUEJAYF4/config.c +++ b/src/main/target/BLUEJAYF4/config.c @@ -19,64 +19,17 @@ #include -#include "build/build_config.h" - -#include "blackbox/blackbox_io.h" - -#include "common/color.h" -#include "common/axis.h" -#include "common/filter.h" - -#include "drivers/sensor.h" -#include "drivers/accgyro.h" -#include "drivers/compass.h" -#include "drivers/system.h" -#include "drivers/timer.h" -#include "drivers/pwm_rx.h" -#include "drivers/serial.h" -#include "drivers/pwm_output.h" -#include "drivers/io.h" -#include "drivers/pwm_mapping.h" - -#include "sensors/sensors.h" -#include "sensors/gyro.h" -#include "sensors/compass.h" -#include "sensors/acceleration.h" -#include "sensors/barometer.h" -#include "sensors/boardalignment.h" -#include "sensors/battery.h" - -#include "io/beeper.h" -#include "io/serial.h" -#include "io/gimbal.h" -#include "io/escservo.h" -#include "fc/rc_controls.h" -#include "fc/rc_curves.h" -#include "io/ledstrip.h" -#include "io/gps.h" - -#include "rx/rx.h" - -#include "telemetry/telemetry.h" - -#include "flight/mixer.h" -#include "flight/pid.h" -#include "flight/failsafe.h" - -#include "fc/runtime_config.h" - #include "config/config.h" - -#include "config/config_profile.h" #include "config/config_master.h" +#include "config/config_profile.h" #include "hardware_revision.h" // alternative defaults settings for BlueJayF4 targets -void targetConfiguration(void) +void targetConfiguration(master_t *config) { if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) { - masterConfig.sensorAlignmentConfig.gyro_align = CW180_DEG; - masterConfig.sensorAlignmentConfig.acc_align = CW180_DEG; + config->sensorAlignmentConfig.gyro_align = CW180_DEG; + config->sensorAlignmentConfig.acc_align = CW180_DEG; } } diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 9d9d70951fb..becba9fc9e1 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -79,19 +79,20 @@ #endif #ifdef USE_RX_NRF24 -#define DEFAULT_RX_FEATURE FEATURE_RX_NRF24 +#define USE_RX_SPI +#define DEFAULT_RX_FEATURE FEATURE_RX_SPI #define DEFAULT_FEATURES FEATURE_SOFTSPI #define USE_RX_SYMA //#define USE_RX_V202 #define USE_RX_CX10 //#define USE_RX_H8_3D #define USE_RX_INAV -#define NRF24_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C -//#define NRF24_DEFAULT_PROTOCOL NRF24RX_V202_1M -//#define NRF24_DEFAULT_PROTOCOL NRF24RX_H8_3D +#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C +//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_V202_1M +//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_H8_3D #define USE_SOFTSPI -#define USE_NRF24_SOFTSPI +#define USE_RX_SOFTSPI // RC pinouts // RC1 GND @@ -104,14 +105,14 @@ // RC8 PA1/TIM2 CE / RX_PPM // Nordic Semiconductor uses 'CSN', STM uses 'NSS' -#define NRF24_CSN_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define NRF24_CSN_PIN PA0 -#define NRF24_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define NRF24_CE_PIN PA1 -#define NRF24_CSN_PIN PA0 -#define NRF24_SCK_PIN PB5 -#define NRF24_MOSI_PIN PB1 -#define NRF24_MISO_PIN PB0 +#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA +#define RX_NSS_PIN PA0 +#define RX_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA +#define RX_CE_PIN PA1 +#define RX_NSS_PIN PA0 +#define RX_SCK_PIN PB5 +#define RX_MOSI_PIN PB1 +#define RX_MISO_PIN PB0 #define SERIAL_PORT_COUNT 3 @@ -198,6 +199,7 @@ #endif //OPBL +#define SKIP_CLI_RESOURCES #define SKIP_RX_MSP #ifdef USE_RX_NRF24 #define SKIP_RX_PWM_PPM @@ -205,6 +207,7 @@ #undef SPEKTRUM_BIND #undef TELEMETRY #undef TELEMETRY_LTM +#undef TELEMETRY_IBUS #endif // Number of available PWM outputs diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index a18f7248870..e92817415f4 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -48,39 +48,39 @@ #define USE_RX_NRF24 #ifdef USE_RX_NRF24 -#define NRF24_SPI_INSTANCE SPI1 -#define USE_NRF24_SPI1 +#define USE_RX_SPI +#define RX_SPI_INSTANCE SPI1 // Nordic Semiconductor uses 'CSN', STM uses 'NSS' -#define NRF24_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define NRF24_CSN_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define NRF24_IRQ_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define NRF24_CE_PIN PA4 -#define NRF24_CSN_PIN PA11 -#define NRF24_SCK_PIN PA5 -#define NRF24_MISO_PIN PA6 -#define NRF24_MOSI_PIN PA7 -#define NRF24_IRQ_PIN PA8 +#define RX_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA +#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA +#define RX_IRQ_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA +#define RX_CE_PIN PA4 +#define RX_NSS_PIN PA11 +#define RX_SCK_PIN PA5 +#define RX_MISO_PIN PA6 +#define RX_MOSI_PIN PA7 +#define RX_IRQ_PIN PA8 // CJMCU has NSS on PA11, rather than the standard PA4 -#define SPI1_NSS_PIN NRF24_CSN_PIN -#define SPI1_SCK_PIN NRF24_SCK_PIN -#define SPI1_MISO_PIN NRF24_MISO_PIN -#define SPI1_MOSI_PIN NRF24_MOSI_PIN +#define SPI1_NSS_PIN RX_NSS_PIN +#define SPI1_SCK_PIN RX_SCK_PIN +#define SPI1_MISO_PIN RX_MISO_PIN +#define SPI1_MOSI_PIN RX_MOSI_PIN #define USE_RX_NRF24 -#define USE_RX_CX10 +//#define USE_RX_CX10 #define USE_RX_H8_3D #define USE_RX_INAV #define USE_RX_SYMA #define USE_RX_V202 -//#define NRF24_DEFAULT_PROTOCOL NRF24RX_SYMA_X5 -//#define NRF24_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C -//#define NRF24_DEFAULT_PROTOCOL NRF24RX_INAV -//#define NRF24_DEFAULT_PROTOCOL NRF24RX_H8_3D -#define NRF24_DEFAULT_PROTOCOL NRF24RX_CX10A -//#define NRF24_DEFAULT_PROTOCOL NRF24RX_V202_1M - -#define DEFAULT_RX_FEATURE FEATURE_RX_NRF24 +//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_SYMA_X5 +//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C +#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_INAV +//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_H8_3D +//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_CX10A +//#define RX_SPI_DEFAULT_PROTOCOL NRF24RX_V202_1M + +#define DEFAULT_RX_FEATURE FEATURE_RX_SPI //#define TELEMETRY //#define TELEMETRY_LTM //#define TELEMETRY_NRF24_LTM @@ -109,6 +109,9 @@ #undef BLACKBOX #endif +#define STACK_CHECK +#define DEBUG_STACK + // Number of available PWM outputs #define MAX_PWM_OUTPUT_PORTS 4 diff --git a/src/main/target/CRAZEPONYMINI/target.h b/src/main/target/CRAZEPONYMINI/target.h index 4a22ff9aa7d..9f069d53ed2 100644 --- a/src/main/target/CRAZEPONYMINI/target.h +++ b/src/main/target/CRAZEPONYMINI/target.h @@ -46,7 +46,11 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_1) -#define DEFAULT_RX_FEATURE FEATURE_RX_NRF24 +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define USE_RX_SPI +#define RX_SPI_INSTANCE SPI1 #define USE_RX_NRF24 //#define USE_RX_CX10 //#define USE_RX_H8_3D @@ -54,19 +58,14 @@ #define USE_RX_V202 #define NRF24_DEFAULT_PROTOCOL NRF24RX_V202_1M -#define USE_SPI -#define USE_SPI_DEVICE_1 - -#define NRF24_SPI_INSTANCE SPI1 -#define USE_NRF24_SPI1 // Nordic Semiconductor uses 'CSN', STM uses 'NSS' -#define NRF24_CE_PIN PA12 -#define NRF24_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define NRF24_CSN_PIN PA4 -#define NRF24_CSN_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define NRF24_IRQ_PIN PA15 -#define NRF24_IRQ_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA +#define RX_CE_PIN PA12 +#define RX_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA +#define RX_NSS_PIN PA4 +#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA +#define RX_IRQ_PIN PA15 +#define RX_IRQ_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA #define SKIP_RX_MSP #define SKIP_INFLIGHT_ADJUSTMENTS @@ -74,6 +73,8 @@ #undef SERIAL_RX #undef BLACKBOX +#define DEFAULT_RX_FEATURE FEATURE_RX_SPI + // Since the CrazePony MINI PCB has holes for 4 motors in each corner we can save same flash space by disabling support for other mixers. #undef USE_SERVOS #define USE_QUAD_MIXER_ONLY diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 2fd1fe89a95..7c14693401c 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -138,6 +138,7 @@ //#define USE_RX_NRF24 #ifdef USE_RX_NRF24 +#define USE_RX_SPI #define USE_RX_CX10 #define USE_RX_H8_3D @@ -151,7 +152,8 @@ //#define NRF24_DEFAULT_PROTOCOL NRF24RX_V202_1M #define USE_SOFTSPI -#define USE_NRF24_SOFTSPI +#define USE_RX_SOFTSPI + // RC pinouts // RC1 GND // RC2 power @@ -165,13 +167,13 @@ // RC10 PB1/TIM3 MOSI /softserial2 TX / sonar echo / current // Nordic Semiconductor uses 'CSN', STM uses 'NSS' -#define NRF24_CE_PIN PA1 -#define NRF24_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define NRF24_CSN_PIN PA6 -#define NRF24_CSN_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define NRF24_SCK_PIN PA7 -#define NRF24_MOSI_PIN PB1 -#define NRF24_MISO_PIN PB0 +#define RX_CE_PIN PA1 +#define RX_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA +#define RX_NSS_PIN PA6 +#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA +#define RX_SCK_PIN PA7 +#define RX_MOSI_PIN PB1 +#define RX_MISO_PIN PB0 #endif // USE_NRF24 #define USE_ADC @@ -193,10 +195,11 @@ #define SPEKTRUM_BIND // USART2, PA3 -#define BIND_PIN PA3 +#define BIND_PIN PA3 //#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define SKIP_CLI_RESOURCES #define TARGET_MOTOR_COUNT 6 #define DISABLE_UNCOMMON_MIXERS @@ -205,6 +208,7 @@ #undef TELEMETRY_HOTT #undef TELEMETRY_SMARTPORT +#undef TELEMETRY_IBUS // Disable all GPS protocols except UBLOX #undef GPS_PROTO_NMEA diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c new file mode 100644 index 00000000000..8b510392113 --- /dev/null +++ b/src/main/target/OMNIBUS/target.c @@ -0,0 +1,82 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + // PWM1 PPM Pad + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // Pin PPM - PB4 + // PB5 / TIM3 CH2 is connected to USBPresent + + // PWM2-PWM5 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // Pin PWM1 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // Pin PWM2 - PB9 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // Pin PWM3 - PA3 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // Pin PWM4 - PA2 + + // For iNav, PWM6&7 (PWM pins 5&6) are shared with UART3 + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // Pin PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // Pin PWM6 - PB11 - TIM2_CH4 / UART3_RX (AF7) +}; diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h new file mode 100644 index 00000000000..c3ece34d23e --- /dev/null +++ b/src/main/target/OMNIBUS/target.h @@ -0,0 +1,214 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE + +#define LED0 PB3 + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_EXTI +#define EXTI_CALLBACK_HANDLER_COUNT 3 // MPU data ready (mag disabled) +#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 +#define MPU_INT_EXTI PC13 +#define USE_MPU_DATA_READY_SIGNAL +#define GYRO_MPU6000_ALIGN CW90_DEG + +#define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW90_DEG + +#define BARO +#define USE_BARO_BMP280 +#define USE_BARO_SPI_BMP280 +#define BMP280_SPI_INSTANCE SPI1 +#define BMP280_CS_PIN PA13 +#define USE_BARO_BMP085 // External +#define USE_BARO_BMP180 // External +#define USE_BARO_MS5611 // External + +#define MAG +#define USE_MAG_AK8963 // External +#define USE_MAG_AK8975 // External +#define USE_MAG_HMC5883 // External +#define USE_MAG_MAG3110 // External + +#define SONAR +#define SONAR_ECHO_PIN PB2 // Has 1K series resistor +#define SONAR_TRIGGER_PIN PB4 // FT + +#define USB_IO +#define USB_CABLE_DETECTION +#define USB_DETECT_PIN PB5 + +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define SERIAL_PORT_COUNT 4 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 + +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) + +// Enable I2C instead of PWM7&8 for iNav +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL(PWM8), PB7/SDA(PWM7) +// Because the I2C is shared with PWM7&8, there are no on-board ext. pullups. +// Turn internal pullups, they are weak, but better than nothing. +#define USE_I2C_PULLUP + +// OSD disabled for now +#if 0 +// OSD define info: +// feature name (includes source) -> MAX_OSD, used in target.mk +// include the osd code +#define OSD +// include the max7456 driver +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI1 +#define MAX7456_SPI_CS_PIN PB1 +//#define MAX7456_DMA_CHANNEL_TX DMA1_Channel3 +//#define MAX7456_DMA_CHANNEL_RX DMA1_Channel2 +//#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_CH3_HANDLER +#endif + +#define USE_SPI +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SDCARD +#define USE_SDCARD_SPI2 + +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PC14 +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN + +// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 + +// Performance logging for SD card operations: +// #define AFATFS_USE_INTROSPECTIVE_LOGGING + +#define USE_ADC +//#define BOARD_HAS_VOLTAGE_DIVIDER +#define VBAT_ADC_PIN PA0 +#define CURRENT_METER_ADC_PIN PA1 +#define ADC_INSTANCE ADC1 + +// No RSSI support for OMNIBUS F3 (probably never) +//#define RSSI_ADC_PIN PB2 +//#define ADC_INSTANCE ADC2 + +#define 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_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +// No transponder support for iNav (for now?) +#if 0 +#define TRANSPONDER +#define TRANSPONDER_GPIO GPIOA +#define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define TRANSPONDER_GPIO_AF GPIO_AF_6 +#define TRANSPONDER_PIN GPIO_Pin_8 +#define TRANSPONDER_PIN_SOURCE GPIO_PinSource8 + +#define TRANSPONDER_TIMER TIM1 +#define TRANSPONDER_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 +#define TRANSPONDER_DMA_CHANNEL DMA1_Channel2 +#define TRANSPONDER_IRQ DMA1_Channel2_IRQn +#define TRANSPONDER_DMA_TC_FLAG DMA1_FLAG_TC2 +#define TRANSPONDER_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define REDUCE_TRANSPONDER_CURRENT_DRAW_WHEN_USB_CABLE_PRESENT +#endif + +// Not supported on INAV-1.2 +//#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX) + +#define NAV +#define NAV_AUTO_MAG_DECLINATION +#define NAV_GPS_GLITCH_DETECTION +#define NAV_MAX_WAYPOINTS 60 + +#define BUTTONS +#define BUTTON_A_PORT GPIOB // Non-existent (PB1 used for RSSI/MAXCS) +#define BUTTON_A_PIN Pin_1 +#define BUTTON_B_PORT GPIOB // TRIG button, used for BINDPLUG_PIN +#define BUTTON_B_PIN Pin_0 + +#define SPEKTRUM_BIND +// USART3 +#define BIND_PIN PB11 + +#define HARDWARE_BIND_PLUG +#define BINDPLUG_PIN PB0 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// Number of available PWM outputs +#define MAX_PWM_OUTPUT_PORTS 12 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) + +#define USABLE_TIMER_CHANNEL_COUNT 7 // 6 Outputs; PPM; +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk new file mode 100644 index 00000000000..4305adc694c --- /dev/null +++ b/src/main/target/OMNIBUS/target.mk @@ -0,0 +1,17 @@ +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_spi_bmp280.c \ + drivers/barometer_bmp085.c \ + drivers/barometer_ms5611.c \ + drivers/compass_ak8963.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/compass_mag3110.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_usb_vcp.c diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 7e09e8de5d6..6d4b7955363 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -119,6 +119,7 @@ #undef GPS_PROTO_NAZA #undef TELEMETRY_HOTT #undef TELEMETRY_SMARTPORT +#undef TELEMETRY_IBUS // Number of available PWM outputs #define MAX_PWM_OUTPUT_PORTS 12 diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 7ee7a3d09bb..c79aeef77b1 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -54,7 +54,7 @@ //#define USE_MAG_MAG3110 // External #define USE_MAG_HMC5883 // External -#define MAG_AK8963_ALIGN CW90_DEG_FLIP +#define MAG_AK8963_ALIGN CW270_DEG_FLIP //#define SONAR diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c new file mode 100644 index 00000000000..bf8776aa923 --- /dev/null +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -0,0 +1,110 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + // PPM Pad +#ifdef SPRACINGF3MINI_MKII_REVA + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PPM - PB5 + // PB4 / TIM3 CH1 is connected to USBPresent +#else + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PPM - PB4 + // PB5 / TIM3 CH2 is connected to USBPresent +#endif + + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM5 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM6 - PA3 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA0 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA1 + + // UART3 RX/TX + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) + + // LED Strip Pad + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP +}; + diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h new file mode 100644 index 00000000000..329b582c432 --- /dev/null +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -0,0 +1,184 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SRFM" + + +// early prototype had slightly different pin mappings. +//#define SPRACINGF3MINI_MKII_REVA + +#define LED0 PB3 + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define USE_EXTI +#define MPU_INT_EXTI PC13 +#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + +#define GYRO +//#define USE_FAKE_GYRO +#define USE_GYRO_MPU6500 +#define GYRO_MPU6500_ALIGN CW180_DEG + +#define ACC +//#define USE_FAKE_ACC +#define USE_ACC_MPU6500 +#define ACC_MPU6500_ALIGN CW180_DEG + +#define BARO +#define USE_BARO_BMP280 + +#define MAG +#define USE_MPU9250_MAG // Enables bypass configuration +#define USE_MAG_AK8963 +#define USE_MAG_HMC5883 // External +#define MAG_AK8963_ALIGN CW270_DEG_FLIP + +#define SONAR +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN PB0 + +#define USB_IO +#define USB_CABLE_DETECTION + +#define USB_DETECT_PIN PB5 + +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_SOFTSERIAL1 +#define SERIAL_PORT_COUNT 5 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 + +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) + +#define SOFTSERIAL_1_TIMER TIM2 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 9 // PA0 / PAD3 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 10 // PA1 / PAD4 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +#define USE_SPI +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SDCARD +#define USE_SDCARD_SPI2 + +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PC14 +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN + +// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 + +// Performance logging for SD card operations: +// #define AFATFS_USE_INTROSPECTIVE_LOGGING + +#define BOARD_HAS_VOLTAGE_DIVIDER +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 + +#define 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_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define TRANSPONDER +#define TRANSPONDER_GPIO GPIOA +#define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define TRANSPONDER_GPIO_AF GPIO_AF_6 +#define TRANSPONDER_PIN GPIO_Pin_8 +#define TRANSPONDER_PIN_SOURCE GPIO_PinSource8 +#define TRANSPONDER_TIMER TIM1 +#define TRANSPONDER_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 +#define TRANSPONDER_DMA_CHANNEL DMA1_Channel2 +#define TRANSPONDER_IRQ DMA1_Channel2_IRQn +#define TRANSPONDER_DMA_TC_FLAG DMA1_FLAG_TC2 +#define TRANSPONDER_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define REDUCE_TRANSPONDER_CURRENT_DRAW_WHEN_USB_CABLE_PRESENT + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX + +#define BUTTONS +#define BUTTON_A_PORT GPIOB +#define BUTTON_A_PIN Pin_1 +#define BUTTON_B_PORT GPIOB +#define BUTTON_B_PIN Pin_0 + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#define HARDWARE_BIND_PLUG +#define BINDPLUG_PIN PB0 +#define NAV +#define NAV_AUTO_MAG_DECLINATION +#define NAV_GPS_GLITCH_DETECTION +#define NAV_MAX_WAYPOINTS 60 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// Number of available PWM outputs +#define MAX_PWM_OUTPUT_PORTS 10 + +// IO - stm32f303cc in 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) + +#define USABLE_TIMER_CHANNEL_COUNT 12 // 8 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins. +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) + diff --git a/src/main/target/SPRACINGF3MINI/target.mk b/src/main/target/SPRACINGF3MINI/target.mk new file mode 100644 index 00000000000..4773508f675 --- /dev/null +++ b/src/main/target/SPRACINGF3MINI/target.mk @@ -0,0 +1,15 @@ +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/compass_ak8963.c \ + drivers/compass_hmc5883l.c \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/serial_usb_vcp.c \ + drivers/sonar_hcsr04.c \ diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index fe36327a618..14af3dcfb28 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -103,11 +103,11 @@ #define USE_RX_INAV #define USE_RX_SYMA #define USE_RX_V202 -#define NRF24_DEFAULT_PROTOCOL NRF24RX_V202_1M -#define NRF24_CSN_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define NRF24_CSN_PIN PA0 -#define NRF24_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA -#define NRF24_CE_PIN PA1 +#define RX_DEFAULT_PROTOCOL NRF24RX_V202_1M +#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA +#define RX_NSS_PIN PA0 +#define RX_CE_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOA +#define RX_CE_PIN PA1 #define USE_I2C #define I2C_DEVICE (I2CDEV_1) diff --git a/src/main/target/common.h b/src/main/target/common.h index 82eb58598ff..2b302ab2c70 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -19,6 +19,7 @@ #define I2C1_OVERCLOCK false #define I2C2_OVERCLOCK false +#define USE_I2C_PULLUP // Enable built-in pullups on all boards in case external ones are too week #define USE_SERVOS #define SERIAL_RX @@ -48,9 +49,11 @@ #define DISPLAY_ARMED_BITMAP #define TELEMETRY_MAVLINK #define BOOTLOG_DESCRIPTIONS +#define TELEMETRY_IBUS +#define USE_PMW_SERVO_DRIVER +#define PWM_DRIVER_PCA9685 #else #define SKIP_CLI_COMMAND_HELP #define SKIP_RX_MSP #define DISABLE_UNCOMMON_MIXERS #endif - diff --git a/src/main/target/stm32_flash.ld b/src/main/target/link/stm32_flash.ld similarity index 94% rename from src/main/target/stm32_flash.ld rename to src/main/target/link/stm32_flash.ld index c38f3d263aa..b4a83231812 100644 --- a/src/main/target/stm32_flash.ld +++ b/src/main/target/link/stm32_flash.ld @@ -110,6 +110,9 @@ SECTIONS } >RAM /* User_heap_stack section, used to check that there is enough RAM left */ + _heap_stack_end = ORIGIN(RAM)+LENGTH(RAM) - 8; /* 8 bytes to allow for alignment */ + _heap_stack_begin = _heap_stack_end - _Min_Stack_Size - _Min_Heap_Size; + . = _heap_stack_begin; ._user_heap_stack : { . = ALIGN(4); @@ -118,7 +121,7 @@ SECTIONS . = . + _Min_Heap_Size; . = . + _Min_Stack_Size; . = ALIGN(4); - } >RAM + } >RAM = 0xa5 /* MEMORY_bank1 section, code must be located here explicitly */ /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ diff --git a/src/main/target/stm32_flash_f103_128k.ld b/src/main/target/link/stm32_flash_f103_128k.ld similarity index 100% rename from src/main/target/stm32_flash_f103_128k.ld rename to src/main/target/link/stm32_flash_f103_128k.ld diff --git a/src/main/target/stm32_flash_f103_128k_opbl.ld b/src/main/target/link/stm32_flash_f103_128k_opbl.ld similarity index 100% rename from src/main/target/stm32_flash_f103_128k_opbl.ld rename to src/main/target/link/stm32_flash_f103_128k_opbl.ld diff --git a/src/main/target/stm32_flash_f103_256k.ld b/src/main/target/link/stm32_flash_f103_256k.ld similarity index 100% rename from src/main/target/stm32_flash_f103_256k.ld rename to src/main/target/link/stm32_flash_f103_256k.ld diff --git a/src/main/target/stm32_flash_f103_64k.ld b/src/main/target/link/stm32_flash_f103_64k.ld similarity index 100% rename from src/main/target/stm32_flash_f103_64k.ld rename to src/main/target/link/stm32_flash_f103_64k.ld diff --git a/src/main/target/stm32_flash_f303_128k.ld b/src/main/target/link/stm32_flash_f303_128k.ld similarity index 100% rename from src/main/target/stm32_flash_f303_128k.ld rename to src/main/target/link/stm32_flash_f303_128k.ld diff --git a/src/main/target/stm32_flash_f303_256k.ld b/src/main/target/link/stm32_flash_f303_256k.ld similarity index 100% rename from src/main/target/stm32_flash_f303_256k.ld rename to src/main/target/link/stm32_flash_f303_256k.ld diff --git a/src/main/target/link/stm32_flash_f405.ld b/src/main/target/link/stm32_flash_f405.ld new file mode 100644 index 00000000000..aa56378b5d2 --- /dev/null +++ b/src/main/target/link/stm32_flash_f405.ld @@ -0,0 +1,41 @@ +/* +***************************************************************************** +** +** File : stm32_flash.ld +** +** Abstract : Linker script for STM32F407VG Device with +** 1024KByte FLASH, 128KByte RAM +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** Environment : Atollic TrueSTUDIO(R) +** +** Distribution: The file is distributed “as is,” without any warranty +** of any kind. +** +** (c)Copyright Atollic AB. +** You may use this file as-is or modify it according to the needs of your +** project. Distribution of this file (unmodified or modified) is not +** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the +** rights to distribute the assembled, compiled & linked contents of this +** file as part of an application binary file, provided that it is built +** using the Atollic TrueSTUDIO(R) toolchain. +** +***************************************************************************** +*/ + +/* Specify the memory areas */ +MEMORY +{ + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 0x0e0000 - 0x64 + INFOX (rx) : ORIGIN = 0x08000000 + 0x0e0000 - 0x64, LENGTH = 0x64 + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K +} + +INCLUDE "stm32_flash.ld" \ No newline at end of file diff --git a/src/main/target/link/stm32_flash_f405_opbl.ld b/src/main/target/link/stm32_flash_f405_opbl.ld new file mode 100644 index 00000000000..c6691919fe6 --- /dev/null +++ b/src/main/target/link/stm32_flash_f405_opbl.ld @@ -0,0 +1,52 @@ +/* +***************************************************************************** +** +** File : stm32_flash.ld +** +** Abstract : Linker script for STM32F407VG Device with +** 1024KByte FLASH, 128KByte RAM +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** Environment : Atollic TrueSTUDIO(R) +** +** Distribution: The file is distributed “as is,” without any warranty +** of any kind. +** +** (c)Copyright Atollic AB. +** You may use this file as-is or modify it according to the needs of your +** project. Distribution of this file (unmodified or modified) is not +** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the +** rights to distribute the assembled, compiled & linked contents of this +** file as part of an application binary file, provided that it is built +** using the Atollic TrueSTUDIO(R) toolchain. +** +***************************************************************************** +*/ + +/* Specify the memory areas */ + +/* +0x08000000 to 0x08100000 1024kb full flash, +0x08000000 to 0x08020000 128kb OPBL, +0x08020000 to 0x08080000 384kb firmware, +0x08080000 to 0x080A0000 128kb config, +*/ + +MEMORY +{ + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 0x000A0000 + CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 0x00020000 + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K +} + +REGION_ALIAS("CCSRAM", CCM); +REGION_ALIAS("SRAM", RAM); + +INCLUDE "stm32_flash.ld" \ No newline at end of file diff --git a/src/main/target/link/stm32_flash_f411.ld b/src/main/target/link/stm32_flash_f411.ld new file mode 100644 index 00000000000..4cc85f37c9f --- /dev/null +++ b/src/main/target/link/stm32_flash_f411.ld @@ -0,0 +1,50 @@ +/* +***************************************************************************** +** +** File : stm32_flash.ld +** +** Abstract : Linker script for STM32F11 Device with +** 512KByte FLASH, 128KByte RAM +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** Environment : Atollic TrueSTUDIO(R) +** +** Distribution: The file is distributed “as is,” without any warranty +** of any kind. +** +** (c)Copyright Atollic AB. +** You may use this file as-is or modify it according to the needs of your +** project. Distribution of this file (unmodified or modified) is not +** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the +** rights to distribute the assembled, compiled & linked contents of this +** file as part of an application binary file, provided that it is built +** using the Atollic TrueSTUDIO(R) toolchain. +** +***************************************************************************** +*/ + +/* Specify the memory areas */ + +/* +0x08000000 to 0x08080000 512kb full flash, +0x08000000 to 0x08060000 384kb firmware, +0x08060000 to 0x08080000 128kb config, +*/ + +MEMORY +{ + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 0x00080000 + RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00020000 + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K +} + +REGION_ALIAS("CCSRAM", RAM); +REGION_ALIAS("SRAM", RAM); + +INCLUDE "stm32_flash.ld" \ No newline at end of file diff --git a/src/main/target/link/stm32_flash_f411_opbl.ld b/src/main/target/link/stm32_flash_f411_opbl.ld new file mode 100644 index 00000000000..021458dc336 --- /dev/null +++ b/src/main/target/link/stm32_flash_f411_opbl.ld @@ -0,0 +1,49 @@ +/* +***************************************************************************** +** +** File : stm32_flash.ld +** +** Abstract : Linker script for STM32F11 Device with +** 512KByte FLASH, 128KByte RAM +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** Environment : Atollic TrueSTUDIO(R) +** +** Distribution: The file is distributed “as is,” without any warranty +** of any kind. +** +** (c)Copyright Atollic AB. +** You may use this file as-is or modify it according to the needs of your +** project. Distribution of this file (unmodified or modified) is not +** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the +** rights to distribute the assembled, compiled & linked contents of this +** file as part of an application binary file, provided that it is built +** using the Atollic TrueSTUDIO(R) toolchain. +** +***************************************************************************** +*/ + +/* +0x08000000 to 0x08080000 512kb full flash, +0x08000000 to 0x08010000 64kb OPBL, +0x08010000 to 0x08060000 320kb firmware, +0x08060000 to 0x08080000 128kb config, +*/ + +MEMORY +{ + FLASH (rx) : ORIGIN = 0x08010000, LENGTH = 0x00070000 + RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00020000 + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K +} + +REGION_ALIAS("CCSRAM", RAM); +REGION_ALIAS("SRAM", RAM); + +INCLUDE "stm32_flash.ld" \ No newline at end of file diff --git a/src/main/target/stm32_flash_f4xx.ld b/src/main/target/link/stm32_flash_f4xx.ld similarity index 100% rename from src/main/target/stm32_flash_f4xx.ld rename to src/main/target/link/stm32_flash_f4xx.ld diff --git a/src/main/target/stm32_flash_f405.ld b/src/main/target/stm32_flash_f405.ld deleted file mode 100644 index eb2a7fbf3d3..00000000000 --- a/src/main/target/stm32_flash_f405.ld +++ /dev/null @@ -1,175 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash.ld -** -** Abstract : Linker script for STM32F407VG Device with -** 1024KByte FLASH, 128KByte RAM -** -** Set heap size, stack size and stack location according -** to application requirements. -** -** Set memory bank area and size if external memory is used. -** -** Target : STMicroelectronics STM32 -** -** Environment : Atollic TrueSTUDIO(R) -** -** Distribution: The file is distributed “as is,” without any warranty -** of any kind. -** -** (c)Copyright Atollic AB. -** You may use this file as-is or modify it according to the needs of your -** project. Distribution of this file (unmodified or modified) is not -** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the -** rights to distribute the assembled, compiled & linked contents of this -** file as part of an application binary file, provided that it is built -** using the Atollic TrueSTUDIO(R) toolchain. -** -***************************************************************************** -*/ - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0; /* required amount of heap */ -_Min_Stack_Size = 0x400; /* required amount of stack */ - -/* Specify the memory areas */ -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 0x0e0000 - 0x64 - INFOX (rx) : ORIGIN = 0x08000000 + 0x0e0000 - 0x64, LENGTH = 0x64 - RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -/* Section Definitions */ -SECTIONS -{ - /* The program code and other data goes into FLASH */ - .text : - { - PROVIDE (isr_vector_table_base = .); - KEEP(*(.isr_vector)) /* Startup code */ - . = ALIGN(4); - *(.text) /* .text sections (code) */ - *(.text*) /* .text* sections (code) */ - *(.rodata) /* .rodata sections (constants, strings, etc.) */ - *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ - *(.glue_7) /* glue arm to thumb code */ - *(.glue_7t) /* glue thumb to arm code */ - *(.eh_frame) - - KEEP (*(.init)) - KEEP (*(.fini)) - - . = ALIGN(4); - _etext = .; /* define a global symbols at end of code */ - } >FLASH - - - .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH - .ARM : { - __exidx_start = .; - *(.ARM.exidx*) - __exidx_end = .; - } >FLASH - - .preinit_array : - { - PROVIDE_HIDDEN (__preinit_array_start = .); - KEEP (*(.preinit_array*)) - PROVIDE_HIDDEN (__preinit_array_end = .); - } >FLASH - .init_array : - { - PROVIDE_HIDDEN (__init_array_start = .); - KEEP (*(SORT(.init_array.*))) - KEEP (*(.init_array*)) - PROVIDE_HIDDEN (__init_array_end = .); - } >FLASH - .fini_array : - { - PROVIDE_HIDDEN (__fini_array_start = .); - KEEP (*(.fini_array*)) - KEEP (*(SORT(.fini_array.*))) - PROVIDE_HIDDEN (__fini_array_end = .); - } >FLASH - - - /* used by the startup to initialize data */ - _sidata = .; - - /* - * Place the IRQ/bootstrap stack at the bottom of SRAM so that an overflow - * results in a hard fault. - */ - .istack (NOLOAD) : - { - . = ALIGN(4); - _irq_stack_end = . ; - *(.irqstack) - _irq_stack_top = . ; - } > RAM - - /* Initialized data sections goes into RAM, load LMA copy after code */ - .data : - { - . = ALIGN(4); - _sdata = .; /* create a global symbol at data start */ - *(.data) /* .data sections */ - *(.data*) /* .data* sections */ - - . = ALIGN(4); - _edata = .; /* define a global symbol at data end */ - } >RAM AT> FLASH - - /* Uninitialized data section */ - . = ALIGN(4); - .bss : - { - /* This is used by the startup in order to initialize the .bss secion */ - _sbss = .; /* define a global symbol at bss start */ - __bss_start__ = _sbss; - *(.bss) - *(.bss*) - *(COMMON) - - . = ALIGN(4); - _ebss = .; /* define a global symbol at bss end */ - __bss_end__ = _ebss; - } >RAM - - /* User_heap_stack section, used to check that there is enough RAM left */ - ._user_heap_stack : - { - . = ALIGN(4); - PROVIDE ( end = . ); - PROVIDE ( _end = . ); - . = . + _Min_Heap_Size; - . = . + _Min_Stack_Size; - . = ALIGN(4); - } >RAM - - /* MEMORY_bank1 section, code must be located here explicitly */ - /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ - .memory_b1_text : - { - *(.mb1text) /* .mb1text sections (code) */ - *(.mb1text*) /* .mb1text* sections (code) */ - *(.mb1rodata) /* read-only data (constants) */ - *(.mb1rodata*) - } >MEMORY_B1 - - /* Remove information from the standard libraries */ - /DISCARD/ : - { - libc.a ( * ) - libm.a ( * ) - libgcc.a ( * ) - } - - .ARM.attributes 0 : { *(.ARM.attributes) } -} diff --git a/src/main/target/stm32_flash_f405_opbl.ld b/src/main/target/stm32_flash_f405_opbl.ld deleted file mode 100644 index e1215c110c5..00000000000 --- a/src/main/target/stm32_flash_f405_opbl.ld +++ /dev/null @@ -1,186 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash.ld -** -** Abstract : Linker script for STM32F407VG Device with -** 1024KByte FLASH, 128KByte RAM -** -** Set heap size, stack size and stack location according -** to application requirements. -** -** Set memory bank area and size if external memory is used. -** -** Target : STMicroelectronics STM32 -** -** Environment : Atollic TrueSTUDIO(R) -** -** Distribution: The file is distributed “as is,” without any warranty -** of any kind. -** -** (c)Copyright Atollic AB. -** You may use this file as-is or modify it according to the needs of your -** project. Distribution of this file (unmodified or modified) is not -** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the -** rights to distribute the assembled, compiled & linked contents of this -** file as part of an application binary file, provided that it is built -** using the Atollic TrueSTUDIO(R) toolchain. -** -***************************************************************************** -*/ - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0x2000; /* required amount of heap */ -_Min_Stack_Size = 0x4000; /* required amount of stack */ - -/* Specify the memory areas */ - -/* -0x08000000 to 0x08100000 1024kb full flash, -0x08000000 to 0x08020000 128kb OPBL, -0x08020000 to 0x08080000 384kb firmware, -0x08080000 to 0x080A0000 128kb config, -*/ - -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 0x000A0000 - CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K - RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 0x00020000 - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("CCSRAM", CCM); -REGION_ALIAS("SRAM", RAM); - -/* Section Definitions */ -SECTIONS -{ - /* The program code and other data goes into FLASH */ - .text : - { - PROVIDE (isr_vector_table_base = .); - KEEP(*(.isr_vector)) /* Startup code */ - . = ALIGN(4); - *(.text) /* .text sections (code) */ - *(.text*) /* .text* sections (code) */ - *(.rodata) /* .rodata sections (constants, strings, etc.) */ - *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ - *(.glue_7) /* glue arm to thumb code */ - *(.glue_7t) /* glue thumb to arm code */ - *(.eh_frame) - - KEEP (*(.init)) - KEEP (*(.fini)) - - . = ALIGN(4); - _etext = .; /* define a global symbols at end of code */ - } >FLASH - - - .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH - .ARM : { - __exidx_start = .; - *(.ARM.exidx*) - __exidx_end = .; - } >FLASH - - .preinit_array : - { - PROVIDE_HIDDEN (__preinit_array_start = .); - KEEP (*(.preinit_array*)) - PROVIDE_HIDDEN (__preinit_array_end = .); - } >FLASH - .init_array : - { - PROVIDE_HIDDEN (__init_array_start = .); - KEEP (*(SORT(.init_array.*))) - KEEP (*(.init_array*)) - PROVIDE_HIDDEN (__init_array_end = .); - } >FLASH - .fini_array : - { - PROVIDE_HIDDEN (__fini_array_start = .); - KEEP (*(.fini_array*)) - KEEP (*(SORT(.fini_array.*))) - PROVIDE_HIDDEN (__fini_array_end = .); - } >FLASH - - - /* used by the startup to initialize data */ - _sidata = .; - - /* - * Place the IRQ/bootstrap stack at the bottom of SRAM so that an overflow - * results in a hard fault. - */ - .istack (NOLOAD) : - { - . = ALIGN(4); - _irq_stack_end = . ; - *(.irqstack) - _irq_stack_top = . ; - } > RAM - - /* Initialized data sections goes into RAM, load LMA copy after code */ - .data : - { - . = ALIGN(4); - _sdata = .; /* create a global symbol at data start */ - *(.data) /* .data sections */ - *(.data*) /* .data* sections */ - - . = ALIGN(4); - _edata = .; /* define a global symbol at data end */ - } >RAM AT> FLASH - - /* Uninitialized data section */ - . = ALIGN(4); - .bss : - { - /* This is used by the startup in order to initialize the .bss secion */ - _sbss = .; /* define a global symbol at bss start */ - __bss_start__ = _sbss; - *(.bss) - *(.bss*) - *(COMMON) - - . = ALIGN(4); - _ebss = .; /* define a global symbol at bss end */ - __bss_end__ = _ebss; - } >RAM - - /* User_heap_stack section, used to check that there is enough RAM left */ - ._user_heap_stack : - { - . = ALIGN(4); - PROVIDE ( end = . ); - PROVIDE ( _end = . ); - . = . + _Min_Heap_Size; - . = . + _Min_Stack_Size; - . = ALIGN(4); - } >RAM - - /* MEMORY_bank1 section, code must be located here explicitly */ - /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ - .memory_b1_text : - { - *(.mb1text) /* .mb1text sections (code) */ - *(.mb1text*) /* .mb1text* sections (code) */ - *(.mb1rodata) /* read-only data (constants) */ - *(.mb1rodata*) - } >MEMORY_B1 - - /* Remove information from the standard libraries */ - /DISCARD/ : - { - libc.a ( * ) - libm.a ( * ) - libgcc.a ( * ) - } - - .ARM.attributes 0 : { *(.ARM.attributes) } -} diff --git a/src/main/target/stm32_flash_f411.ld b/src/main/target/stm32_flash_f411.ld deleted file mode 100644 index a3d834a188f..00000000000 --- a/src/main/target/stm32_flash_f411.ld +++ /dev/null @@ -1,185 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash.ld -** -** Abstract : Linker script for STM32F11 Device with -** 512KByte FLASH, 128KByte RAM -** -** Set heap size, stack size and stack location according -** to application requirements. -** -** Set memory bank area and size if external memory is used. -** -** Target : STMicroelectronics STM32 -** -** Environment : Atollic TrueSTUDIO(R) -** -** Distribution: The file is distributed “as is,” without any warranty -** of any kind. -** -** (c)Copyright Atollic AB. -** You may use this file as-is or modify it according to the needs of your -** project. Distribution of this file (unmodified or modified) is not -** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the -** rights to distribute the assembled, compiled & linked contents of this -** file as part of an application binary file, provided that it is built -** using the Atollic TrueSTUDIO(R) toolchain. -** -***************************************************************************** -*/ - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0x2000; /* required amount of heap */ -_Min_Stack_Size = 0x4000; /* required amount of stack */ - -/* Specify the memory areas */ - -/* -0x08000000 to 0x08080000 512kb full flash, -0x08000000 to 0x08060000 384kb firmware, -0x08060000 to 0x08080000 128kb config, -*/ - -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 0x00080000 - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00020000 - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("CCSRAM", RAM); -REGION_ALIAS("SRAM", RAM); - - -/* Section Definitions */ -SECTIONS -{ - /* The program code and other data goes into FLASH */ - .text : - { - PROVIDE (isr_vector_table_base = .); - KEEP(*(.isr_vector)) /* Startup code */ - . = ALIGN(4); - *(.text) /* .text sections (code) */ - *(.text*) /* .text* sections (code) */ - *(.rodata) /* .rodata sections (constants, strings, etc.) */ - *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ - *(.glue_7) /* glue arm to thumb code */ - *(.glue_7t) /* glue thumb to arm code */ - *(.eh_frame) - - KEEP (*(.init)) - KEEP (*(.fini)) - - . = ALIGN(4); - _etext = .; /* define a global symbols at end of code */ - } >FLASH - - - .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH - .ARM : { - __exidx_start = .; - *(.ARM.exidx*) - __exidx_end = .; - } >FLASH - - .preinit_array : - { - PROVIDE_HIDDEN (__preinit_array_start = .); - KEEP (*(.preinit_array*)) - PROVIDE_HIDDEN (__preinit_array_end = .); - } >FLASH - .init_array : - { - PROVIDE_HIDDEN (__init_array_start = .); - KEEP (*(SORT(.init_array.*))) - KEEP (*(.init_array*)) - PROVIDE_HIDDEN (__init_array_end = .); - } >FLASH - .fini_array : - { - PROVIDE_HIDDEN (__fini_array_start = .); - KEEP (*(.fini_array*)) - KEEP (*(SORT(.fini_array.*))) - PROVIDE_HIDDEN (__fini_array_end = .); - } >FLASH - - - /* used by the startup to initialize data */ - _sidata = .; - - /* - * Place the IRQ/bootstrap stack at the bottom of SRAM so that an overflow - * results in a hard fault. - */ - .istack (NOLOAD) : - { - . = ALIGN(4); - _irq_stack_end = . ; - *(.irqstack) - _irq_stack_top = . ; - } > RAM - - /* Initialized data sections goes into RAM, load LMA copy after code */ - .data : - { - . = ALIGN(4); - _sdata = .; /* create a global symbol at data start */ - *(.data) /* .data sections */ - *(.data*) /* .data* sections */ - - . = ALIGN(4); - _edata = .; /* define a global symbol at data end */ - } >RAM AT> FLASH - - /* Uninitialized data section */ - . = ALIGN(4); - .bss : - { - /* This is used by the startup in order to initialize the .bss secion */ - _sbss = .; /* define a global symbol at bss start */ - __bss_start__ = _sbss; - *(.bss) - *(.bss*) - *(COMMON) - - . = ALIGN(4); - _ebss = .; /* define a global symbol at bss end */ - __bss_end__ = _ebss; - } >RAM - - /* User_heap_stack section, used to check that there is enough RAM left */ - ._user_heap_stack : - { - . = ALIGN(4); - PROVIDE ( end = . ); - PROVIDE ( _end = . ); - . = . + _Min_Heap_Size; - . = . + _Min_Stack_Size; - . = ALIGN(4); - } >RAM - - /* MEMORY_bank1 section, code must be located here explicitly */ - /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ - .memory_b1_text : - { - *(.mb1text) /* .mb1text sections (code) */ - *(.mb1text*) /* .mb1text* sections (code) */ - *(.mb1rodata) /* read-only data (constants) */ - *(.mb1rodata*) - } >MEMORY_B1 - - /* Remove information from the standard libraries */ - /DISCARD/ : - { - libc.a ( * ) - libm.a ( * ) - libgcc.a ( * ) - } - - .ARM.attributes 0 : { *(.ARM.attributes) } -} diff --git a/src/main/target/stm32_flash_f411_opbl.ld b/src/main/target/stm32_flash_f411_opbl.ld deleted file mode 100644 index 7ba674a6d59..00000000000 --- a/src/main/target/stm32_flash_f411_opbl.ld +++ /dev/null @@ -1,186 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash.ld -** -** Abstract : Linker script for STM32F11 Device with -** 512KByte FLASH, 128KByte RAM -** -** Set heap size, stack size and stack location according -** to application requirements. -** -** Set memory bank area and size if external memory is used. -** -** Target : STMicroelectronics STM32 -** -** Environment : Atollic TrueSTUDIO(R) -** -** Distribution: The file is distributed “as is,” without any warranty -** of any kind. -** -** (c)Copyright Atollic AB. -** You may use this file as-is or modify it according to the needs of your -** project. Distribution of this file (unmodified or modified) is not -** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the -** rights to distribute the assembled, compiled & linked contents of this -** file as part of an application binary file, provided that it is built -** using the Atollic TrueSTUDIO(R) toolchain. -** -***************************************************************************** -*/ - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0x2000; /* required amount of heap */ -_Min_Stack_Size = 0x4000; /* required amount of stack */ - -/* Specify the memory areas */ - -/* -0x08000000 to 0x08080000 512kb full flash, -0x08000000 to 0x08010000 64kb OPBL, -0x08010000 to 0x08060000 320kb firmware, -0x08060000 to 0x08080000 128kb config, -*/ - -MEMORY -{ - FLASH (rx) : ORIGIN = 0x08010000, LENGTH = 0x00070000 - RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00020000 - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("CCSRAM", RAM); -REGION_ALIAS("SRAM", RAM); - - -/* Section Definitions */ -SECTIONS -{ - /* The program code and other data goes into FLASH */ - .text : - { - PROVIDE (isr_vector_table_base = .); - KEEP(*(.isr_vector)) /* Startup code */ - . = ALIGN(4); - *(.text) /* .text sections (code) */ - *(.text*) /* .text* sections (code) */ - *(.rodata) /* .rodata sections (constants, strings, etc.) */ - *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ - *(.glue_7) /* glue arm to thumb code */ - *(.glue_7t) /* glue thumb to arm code */ - *(.eh_frame) - - KEEP (*(.init)) - KEEP (*(.fini)) - - . = ALIGN(4); - _etext = .; /* define a global symbols at end of code */ - } >FLASH - - - .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH - .ARM : { - __exidx_start = .; - *(.ARM.exidx*) - __exidx_end = .; - } >FLASH - - .preinit_array : - { - PROVIDE_HIDDEN (__preinit_array_start = .); - KEEP (*(.preinit_array*)) - PROVIDE_HIDDEN (__preinit_array_end = .); - } >FLASH - .init_array : - { - PROVIDE_HIDDEN (__init_array_start = .); - KEEP (*(SORT(.init_array.*))) - KEEP (*(.init_array*)) - PROVIDE_HIDDEN (__init_array_end = .); - } >FLASH - .fini_array : - { - PROVIDE_HIDDEN (__fini_array_start = .); - KEEP (*(.fini_array*)) - KEEP (*(SORT(.fini_array.*))) - PROVIDE_HIDDEN (__fini_array_end = .); - } >FLASH - - - /* used by the startup to initialize data */ - _sidata = .; - - /* - * Place the IRQ/bootstrap stack at the bottom of SRAM so that an overflow - * results in a hard fault. - */ - .istack (NOLOAD) : - { - . = ALIGN(4); - _irq_stack_end = . ; - *(.irqstack) - _irq_stack_top = . ; - } > RAM - - /* Initialized data sections goes into RAM, load LMA copy after code */ - .data : - { - . = ALIGN(4); - _sdata = .; /* create a global symbol at data start */ - *(.data) /* .data sections */ - *(.data*) /* .data* sections */ - - . = ALIGN(4); - _edata = .; /* define a global symbol at data end */ - } >RAM AT> FLASH - - /* Uninitialized data section */ - . = ALIGN(4); - .bss : - { - /* This is used by the startup in order to initialize the .bss secion */ - _sbss = .; /* define a global symbol at bss start */ - __bss_start__ = _sbss; - *(.bss) - *(.bss*) - *(COMMON) - - . = ALIGN(4); - _ebss = .; /* define a global symbol at bss end */ - __bss_end__ = _ebss; - } >RAM - - /* User_heap_stack section, used to check that there is enough RAM left */ - ._user_heap_stack : - { - . = ALIGN(4); - PROVIDE ( end = . ); - PROVIDE ( _end = . ); - . = . + _Min_Heap_Size; - . = . + _Min_Stack_Size; - . = ALIGN(4); - } >RAM - - /* MEMORY_bank1 section, code must be located here explicitly */ - /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ - .memory_b1_text : - { - *(.mb1text) /* .mb1text sections (code) */ - *(.mb1text*) /* .mb1text* sections (code) */ - *(.mb1rodata) /* read-only data (constants) */ - *(.mb1rodata*) - } >MEMORY_B1 - - /* Remove information from the standard libraries */ - /DISCARD/ : - { - libc.a ( * ) - libm.a ( * ) - libgcc.a ( * ) - } - - .ARM.attributes 0 : { *(.ARM.attributes) } -} diff --git a/src/main/target/system_stm32f30x.c b/src/main/target/system_stm32f30x.c index 8915972b425..c63d17361c9 100644 --- a/src/main/target/system_stm32f30x.c +++ b/src/main/target/system_stm32f30x.c @@ -331,7 +331,13 @@ void SetSysClock(void) /* PLL configuration */ RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL)); - RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9); + + if (HSE_VALUE == 12000000) { + RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL6); + } + else { + RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9); + } /* Enable PLL */ RCC->CR |= RCC_CR_PLLON; diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index fe7887fa8f3..c9c9b078fc8 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -59,6 +59,7 @@ #include "flight/navigation_rewrite.h" #include "config/config.h" +#include "config/feature.h" #include "telemetry/telemetry.h" #include "telemetry/frsky.h" diff --git a/src/main/telemetry/ibus.c b/src/main/telemetry/ibus.c new file mode 100644 index 00000000000..909293b02dd --- /dev/null +++ b/src/main/telemetry/ibus.c @@ -0,0 +1,340 @@ +/* + * 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 . + * + * FlySky iBus telemetry implementation by CraigJPerry. + * + * Many thanks to Dave Borthwick's iBus telemetry dongle converter for + * PIC 12F1572 (also distributed under GPLv3) which was referenced to + * clarify the protocol. + * + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef TELEMETRY + +#include "common/axis.h" + +#include "drivers/system.h" +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/serial.h" + +#include "io/serial.h" +#include "fc/rc_controls.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/battery.h" + +#include "telemetry/telemetry.h" +#include "telemetry/ibus.h" + +#include "fc/mw.h" + +/* + * iBus Telemetry is a half-duplex serial protocol. It shares 1 line for + * both TX and RX. It runs at a fixed baud rate of 115200. Queries are sent + * every 7ms by the iBus receiver. Multiple sensors can be daisy chained with + * ibus but not with this implementation, only because i don't have one of the + * sensors to test with! + * + * _______ + * / \ /---------\ + * | STM32 |--UART TX-->[Bi-directional @ 115200 baud]<--| IBUS RX | + * | uC |--UART RX--x[not connected] \---------/ + * \_______/ + * + * + * The protocol is driven by the iBus receiver, currently either an IA6B or + * IA10. All iBus traffic is little endian. It begins with the iBus rx + * querying for a sensor on the iBus: + * + * + * /---------\ + * | IBUS RX | > Hello sensor at address 1, are you there? + * \---------/ [ 0x04, 0x81, 0x7A, 0xFF ] + * + * 0x04 - Packet Length + * 0x81 - bits 7-4 Command (1000 = discover sensor) + * bits 3-0 Address (0001 = address 1) + * 0x7A, 0xFF - Checksum, 0xFFFF - (0x04 + 0x81) + * + * + * Due to the daisy-chaining, this hello also serves to inform the sensor + * of its address (position in the chain). There are 16 possible addresses + * in iBus, however address 0 is reserved for the rx's internally measured + * voltage leaving 0x1 to 0xF remaining. + * + * Having learned it's address, the sensor simply echos the message back: + * + * + * /--------\ + * Yes, i'm here, hello! < | Sensor | + * [ 0x04, 0x81, 0x7A, 0xFF ] \--------/ + * + * 0x04, 0x81, 0x7A, 0xFF - Echo back received packet + * + * + * On receipt of a response, the iBus rx next requests the sensor's type: + * + * + * /---------\ + * | IBUS RX | > Sensor at address 1, what type are you? + * \---------/ [ 0x04, 0x91, 0x6A, 0xFF ] + * + * 0x04 - Packet Length + * 0x91 - bits 7-4 Command (1001 = request sensor type) + * bits 3-0 Address (0001 = address 1) + * 0x6A, 0xFF - Checksum, 0xFFFF - (0x04 + 0x91) + * + * + * To which the sensor responds with its details: + * + * + * /--------\ + * Yes, i'm here, hello! < | Sensor | + * [ 0x06 0x91 0x00 0x02 0x66 0xFF ] \--------/ + * + * 0x06 - Packet Length + * 0x91 - bits 7-4 Command (1001 = request sensor type) + * bits 3-0 Address (0001 = address 1) + * 0x00 - Measurement type (0 = internal voltage) + * 0x02 - Unknown, always 0x02 + * 0x66, 0xFF - Checksum, 0xFFFF - (0x06 + 0x91 + 0x00 + 0x02) + * + * + * The iBus rx continues the discovery process by querying the next + * address. Discovery stops at the first address which does not respond. + * + * The iBus rx then begins a continual loop, requesting measurements from + * each sensor discovered: + * + * + * /---------\ + * | IBUS RX | > Sensor at address 1, please send your measurement + * \---------/ [ 0x04, 0xA1, 0x5A, 0xFF ] + * + * 0x04 - Packet Length + * 0xA1 - bits 7-4 Command (1010 = request measurement) + * bits 3-0 Address (0001 = address 1) + * 0x5A, 0xFF - Checksum, 0xFFFF - (0x04 + 0xA1) + * + * + * /--------\ + * I'm reading 0 volts < | Sensor | + * [ 0x06 0xA1 0x00 0x00 0x5E 0xFF ] \--------/ + * + * 0x06 - Packet Length + * 0xA1 - bits 7-4 Command (1010 = request measurement) + * bits 3-0 Address (0001 = address 1) + * 0x00, 0x00 - The measurement + * 0x5E, 0xFF - Checksum, 0xFF - (0x06 + 0xA1 + 0x00 + 0x00) + * + * + * Due to the limited telemetry data types possible with ibus, we + * simply send everything which can be represented. Currently this + * is voltage and temperature. + * + */ + +#define IBUS_UART_MODE (MODE_RXTX) +#define IBUS_BAUDRATE (115200) +#define IBUS_CYCLE_TIME_MS (8) + +#define IBUS_CHECKSUM_SIZE (2) + +#define IBUS_MIN_LEN (2 + IBUS_CHECKSUM_SIZE) +#define IBUS_MAX_TX_LEN (6) +#define IBUS_MAX_RX_LEN (4) +#define IBUS_RX_BUF_LEN (IBUS_MAX_RX_LEN) + +#define IBUS_TEMPERATURE_OFFSET (0x0190) + +typedef uint8_t ibusAddress_t; + +typedef enum { + IBUS_COMMAND_DISCOVER_SENSOR = 0x80, + IBUS_COMMAND_SENSOR_TYPE = 0x90, + IBUS_COMMAND_MEASUREMENT = 0xA0 +} ibusCommand_e; + +typedef enum { + IBUS_SENSOR_TYPE_INTERNAL_VOLTAGE = 0x00, + IBUS_SENSOR_TYPE_TEMPERATURE = 0x01, + IBUS_SENSOR_TYPE_RPM = 0x02, + IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE = 0x03 +} ibusSensorType_e; + +static const uint8_t SENSOR_ADDRESS_TYPE_LOOKUP[] = { + IBUS_SENSOR_TYPE_INTERNAL_VOLTAGE, // Address 0, not usable since it is reserved for internal voltage + IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE, // Address 1, VBAT + IBUS_SENSOR_TYPE_TEMPERATURE, // Address 2, Gyro Temp + IBUS_SENSOR_TYPE_RPM // Address 3, Throttle command +}; + +static serialPort_t *ibusSerialPort = NULL; +static serialPortConfig_t *ibusSerialPortConfig; + +static telemetryConfig_t *telemetryConfig; +static bool ibusTelemetryEnabled = false; +static portSharing_e ibusPortSharing; + +static uint8_t ibusReceiveBuffer[IBUS_RX_BUF_LEN] = { 0x0 }; + +static uint16_t calculateChecksum(uint8_t ibusPacket[static IBUS_CHECKSUM_SIZE], size_t packetLength) { + uint16_t checksum = 0xFFFF; + for (size_t i = 0; i < packetLength - IBUS_CHECKSUM_SIZE; i++) { + checksum -= ibusPacket[i]; + } + return checksum; +} + +static bool isChecksumOk(uint8_t ibusPacket[static IBUS_CHECKSUM_SIZE], size_t packetLength, uint16_t calcuatedChecksum) { + // Note that there's a byte order swap to little endian here + return (calcuatedChecksum >> 8) == ibusPacket[packetLength - 1] + && (calcuatedChecksum & 0xFF) == ibusPacket[packetLength - 2]; +} + +static void transmitIbusPacket(uint8_t ibusPacket[static IBUS_MIN_LEN], size_t packetLength) { + uint16_t checksum = calculateChecksum(ibusPacket, packetLength); + ibusPacket[packetLength - IBUS_CHECKSUM_SIZE] = (checksum & 0xFF); + ibusPacket[packetLength - IBUS_CHECKSUM_SIZE + 1] = (checksum >> 8); + for (size_t i = 0; i < packetLength; i++) { + serialWrite(ibusSerialPort, ibusPacket[i]); + } +} + +static void sendIbusCommand(ibusAddress_t address) { + uint8_t sendBuffer[] = { 0x04, IBUS_COMMAND_DISCOVER_SENSOR | address, 0x00, 0x00 }; + transmitIbusPacket(sendBuffer, sizeof sendBuffer); +} + +static void sendIbusSensorType(ibusAddress_t address) { + uint8_t sendBuffer[] = { 0x06, IBUS_COMMAND_SENSOR_TYPE | address, SENSOR_ADDRESS_TYPE_LOOKUP[address], 0x02, 0x0, 0x0 }; + transmitIbusPacket(sendBuffer, sizeof sendBuffer); +} + +static void sendIbusMeasurement(ibusAddress_t address, uint16_t measurement) { + uint8_t sendBuffer[] = { 0x06, IBUS_COMMAND_MEASUREMENT | address, measurement & 0xFF, measurement >> 8, 0x0, 0x0 }; + transmitIbusPacket(sendBuffer, sizeof sendBuffer); +} + +static bool isCommand(ibusCommand_e expected, uint8_t ibusPacket[static IBUS_MIN_LEN]) { + return (ibusPacket[1] & 0xF0) == expected; +} + +static ibusAddress_t getAddress(uint8_t ibusPacket[static IBUS_MIN_LEN]) { + return (ibusPacket[1] & 0x0F); +} + +static void dispatchMeasurementRequest(ibusAddress_t address) { + if (1 == address) { + sendIbusMeasurement(address, vbat * 10); + } else if (2 == address) { + sendIbusMeasurement(address, (uint16_t) telemTemperature1 + IBUS_TEMPERATURE_OFFSET); + } else if (3 == address) { + sendIbusMeasurement(address, (uint16_t) rcCommand[THROTTLE]); + } +} + +static void respondToIbusRequest(uint8_t ibusPacket[static IBUS_RX_BUF_LEN]) { + ibusAddress_t returnAddress = getAddress(ibusPacket); + + if (returnAddress < sizeof SENSOR_ADDRESS_TYPE_LOOKUP) { + if (isCommand(IBUS_COMMAND_DISCOVER_SENSOR, ibusPacket)) { + sendIbusCommand(returnAddress); + } else if (isCommand(IBUS_COMMAND_SENSOR_TYPE, ibusPacket)) { + sendIbusSensorType(returnAddress); + } else if (isCommand(IBUS_COMMAND_MEASUREMENT, ibusPacket)) { + dispatchMeasurementRequest(returnAddress); + } + } +} + +static void pushOntoTail(uint8_t buffer[IBUS_MIN_LEN], size_t bufferLength, uint8_t value) { + for (size_t i = 0; i < bufferLength - 1; i++) { + buffer[i] = buffer[i + 1]; + } + ibusReceiveBuffer[bufferLength - 1] = value; +} + +void initIbusTelemetry(telemetryConfig_t *initialTelemetryConfig) { + telemetryConfig = initialTelemetryConfig; + ibusSerialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_IBUS); + ibusPortSharing = determinePortSharing(ibusSerialPortConfig, FUNCTION_TELEMETRY_IBUS); +} + +void handleIbusTelemetry(void) { + if (!ibusTelemetryEnabled) { + return; + } + + while (serialRxBytesWaiting(ibusSerialPort) > 0) { + uint8_t c = serialRead(ibusSerialPort); + pushOntoTail(ibusReceiveBuffer, IBUS_RX_BUF_LEN, c); + uint16_t expectedChecksum = calculateChecksum(ibusReceiveBuffer, IBUS_RX_BUF_LEN); + + if (isChecksumOk(ibusReceiveBuffer, IBUS_RX_BUF_LEN, expectedChecksum)) { + respondToIbusRequest(ibusReceiveBuffer); + } + } +} + +void checkIbusTelemetryState(void) { + bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ibusPortSharing); + + if (newTelemetryEnabledValue == ibusTelemetryEnabled) { + return; + } + + if (newTelemetryEnabledValue) { + configureIbusTelemetryPort(); + } else { + freeIbusTelemetryPort(); + } +} + +void configureIbusTelemetryPort(void) { + portOptions_t portOptions; + + if (!ibusSerialPortConfig) { + return; + } + + portOptions = SERIAL_BIDIR; + + ibusSerialPort = openSerialPort(ibusSerialPortConfig->identifier, FUNCTION_TELEMETRY_IBUS, NULL, IBUS_BAUDRATE, IBUS_UART_MODE, portOptions); + + if (!ibusSerialPort) { + return; + } + + ibusTelemetryEnabled = true; +} + +void freeIbusTelemetryPort(void) { + closeSerialPort(ibusSerialPort); + ibusSerialPort = NULL; + + ibusTelemetryEnabled = false; +} + +#endif diff --git a/src/main/telemetry/ibus.h b/src/main/telemetry/ibus.h new file mode 100644 index 00000000000..1e8e87a9a3b --- /dev/null +++ b/src/main/telemetry/ibus.h @@ -0,0 +1,26 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +void initIbusTelemetry(telemetryConfig_t *); + +void handleIbusTelemetry(void); +void checkIbusTelemetryState(void); + +void configureIbusTelemetryPort(void); +void freeIbusTelemetryPort(void); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 9a3fc7514b2..173ea86a8b5 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -48,6 +48,8 @@ #include "io/gimbal.h" #include "io/gps.h" #include "io/ledstrip.h" +#include "io/motors.h" +#include "io/servos.h" #include "sensors/sensors.h" #include "sensors/acceleration.h" @@ -59,6 +61,7 @@ #include "rx/rx.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/failsafe.h" @@ -74,6 +77,7 @@ #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" #include "fc/mw.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 1cd7a82e383..a494a8cb1d5 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -33,7 +33,7 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "io/escservo.h" +#include "io/motors.h" #include "io/gps.h" #include "io/gimbal.h" #include "io/serial.h" @@ -57,6 +57,7 @@ #include "telemetry/smartport.h" #include "config/config.h" +#include "config/feature.h" enum { diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 730398b5ad0..a3cf447818d 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -44,9 +44,9 @@ #include "telemetry/hott.h" #include "telemetry/smartport.h" #include "telemetry/ltm.h" -#include "telemetry/ltm.h" #include "telemetry/mavlink.h" #include "telemetry/jetiexbus.h" +#include "telemetry/ibus.h" static telemetryConfig_t *telemetryConfig; @@ -81,6 +81,10 @@ void telemetryInit(void) initJetiExBusTelemetry(); #endif +#if defined(TELEMETRY_IBUS) + initIbusTelemetry(telemetryConfig); +#endif + telemetryCheckState(); } @@ -130,6 +134,11 @@ void telemetryCheckState(void) #if defined(TELEMETRY_JETIEXBUS) checkJetiExBusTelemetryState(); #endif + +#if defined(TELEMETRY_IBUS) + checkIbusTelemetryState(); +#endif + } void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) @@ -160,6 +169,11 @@ void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) #if defined(TELEMETRY_JETIEXBUS) handleJetiExBusTelemetry(); #endif + +#if defined(TELEMETRY_IBUS) + handleIbusTelemetry(); +#endif + } #endif diff --git a/src/main/vcp/hw_config.c b/src/main/vcp/hw_config.c index 3270d1bf799..e39c7147bdb 100644 --- a/src/main/vcp/hw_config.c +++ b/src/main/vcp/hw_config.c @@ -283,7 +283,7 @@ static void IntToUnicode(uint32_t value, uint8_t *pbuf, uint8_t len) * Output : None. * Return : None. *******************************************************************************/ -uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) +uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint8_t sendLength) { /* Last transmission hasn't finished, abort */ if (packetSent) { diff --git a/src/main/vcp/hw_config.h b/src/main/vcp/hw_config.h index de454859f38..d7f9a3fcade 100644 --- a/src/main/vcp/hw_config.h +++ b/src/main/vcp/hw_config.h @@ -55,7 +55,7 @@ void Leave_LowPowerMode(void); void USB_Interrupts_Config(void); void USB_Cable_Config(FunctionalState NewState); void Get_SerialNum(void); -uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength); // HJI +uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint8_t sendLength); // HJI uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len); // HJI uint8_t usbIsConfigured(void); // HJI uint8_t usbIsConnected(void); // HJI diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 2af4456ac8f..244420445ac 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -51,7 +51,7 @@ usbStruct_t usbData; static uint16_t VCP_Init(void); static uint16_t VCP_DeInit(void); static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len); -static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len); +static uint16_t VCP_DataTx(const uint8_t* Buf, uint32_t Len); static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len); CDC_IF_Prop_TypeDef VCP_fops = {VCP_Init, VCP_DeInit, VCP_Ctrl, VCP_DataTx, VCP_DataRx }; @@ -151,7 +151,7 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) * Output : None. * Return : None. *******************************************************************************/ -uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) +uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint8_t sendLength) { uint32_t i = 0; if(USB_Tx_State!=1) @@ -169,7 +169,7 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) * @param Len: Number of data to be sent (in bytes) * @retval Result of the opeartion: USBD_OK if all operations are OK else VCP_FAIL */ -static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) +static uint16_t VCP_DataTx(const uint8_t* Buf, uint32_t Len) { uint16_t ptr_head = APP_Rx_ptr_in; uint16_t ptr_tail = APP_Rx_ptr_out; diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index 7810f657dcc..31e50096dca 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -38,7 +38,7 @@ __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; -uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength); // HJI +uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint8_t sendLength); // HJI uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len); // HJI uint8_t usbIsConfigured(void); // HJI uint8_t usbIsConnected(void); // HJI