diff --git a/docs/Cli.md b/docs/Cli.md index 966c5ec23ea..9399bef6a65 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -71,6 +71,7 @@ While connected to the CLI, all Logical Switches are temporarily disabled (5.1.0 | `batch` | Start or end a batch of commands | | `battery_profile` | Change battery profile | | `beeper` | Show/set beeper (buzzer) [usage](Buzzer.md) | +| `bind_msp_rx` | Initiate binding for MSP receivers (mLRS) | | `bind_rx` | Initiate binding for SRXL2 or CRSF receivers | | `blackbox` | Configure blackbox fields | | `bootlog` | Show init logs from [serial_printf_debugging](./development/serial_printf_debugging.md) | @@ -99,6 +100,7 @@ While connected to the CLI, all Logical Switches are temporarily disabled (5.1.0 | `msc` | Enter USB Mass storage mode. See [USB MSC documentation](USB_Mass_Storage_(MSC)_mode.md) for usage information. | | `osd_layout` | Get or set the layout of OSD items | | `pid` | Configurable PID controllers | +| `piniopwm` | Set PINIO PWM duty cycle. See [PINIO PWM](PINIO%20PWM.md) | | `play_sound` | ``, or none for next item | | `control_profile` | Change profile | | `resource` | View currently used resources | diff --git a/docs/LED pin PWM.md b/docs/LED pin PWM.md deleted file mode 100644 index 8ed96a357e2..00000000000 --- a/docs/LED pin PWM.md +++ /dev/null @@ -1,96 +0,0 @@ -# LED pin PWM - -Normally LED pin is used to drive WS2812 led strip. LED pin is held low, and every 10ms or 20ms a set of pulses is sent to change color of the 32 LEDs: - -![alt text](/docs/assets/images/ws2811_packets.png "ws2811 packets") -![alt text](/docs/assets/images/ws2811_data.png "ws2811 data") - -As alternative function, it is possible to generate PWM signal with specified duty ratio on the LED pin. - -Feature can be used to drive external devices such as a VTX power switch. Setting the PWM duty cycle to 100% or 0% can -provide an extra PINIO pin. It is also used to simulate [OSD joystick](OSD%20Joystick.md) to control cameras. - -PWM frequency is fixed to 24kHz with duty ratio between 0 and 100%: - -![alt text](/docs/assets/images/led_pin_pwm.png "led pin pwm") - -Note that the LED feature needs to be enabled when using the PIN in this mode (feature LED_STRIP). - -There are four modes of operation: -- low -- high -- shared_low -- shared_high - -Mode is configured using ```led_pin_pwm_mode``` setting: ```LOW```, ```HIGH```, ```SHARED_LOW```, ```SHARED_HIGH``` - -*Note that in any mode, there will be ~2 seconds LOW pulse on boot.* - -## LOW -LED Pin is initialized to output low level by default and can be used to generate PWM signal. - -ws2812 strip can not be controlled. - -## HIGH -LED Pin is initialized to output high level by default and can be used to generate PWM signal. - -ws2812 strip can not be controlled. - -## SHARED_LOW (default) -LED Pin is used to drive WS2812 strip. Pauses between pulses are low: - -![alt text](/docs/assets/images/ws2811_packets.png "ws2811 packets") - -It is possible to generate PWM signal with duty ratio >0...100%. - -While PWM signal is generated, ws2811 strip is not updated. - -When PWM generation is disabled, LED pin is used to drive ws2812 strip. - -Total ws2812 pulses duration is ~1ms with ~9ms pauses. Thus connected device should ignore PWM signal with duty ratio < ~10%. - -## SHARED_HIGH -LED Pin is used to drive WS2812 strip. Pauses between pulses are high. ws2812 pulses are prefixed with 50us low 'reset' pulse: - -![alt text](/docs/assets/images/ws2811_packets_high.png "ws2811 packets_high") -![alt text](/docs/assets/images/ws2811_data_high.png "ws2811 data_high") - - It is possible to generate PWM signal with duty ratio 0...<100%. - - While PWM signal is generated, ws2811 strip is not updated. - - When PWM generation is disabled, LED pin is used to drive ws2812 strip. Total ws2812 pulses duration is ~1ms with ~9ms pauses. Thus connected device should ignore PWM signal with duty ratio > ~90%. - - After sending ws2812 protocol pulses for 32 LEDS, we held line high for 9ms, then send 50us low 'reset' pulse. Datasheet for ws2812 protocol does not describe behavior for long high pulse, but in practice it works the same as 'reset' pulse. To be safe, we also send correct low 'reset' pulse before starting next LEDs update sequence. - - This mode is used to simulate OSD joystick. It is Ok that effectively voltage level is held >90% while driving LEDs, because OSD joystick keypress voltages are below 90%. - - See [OSD Joystick](OSD%20Joystick.md) for more information. - -# Generating PWM signal with programming framework - -See "LED Pin PWM" operation in [Programming Framework](Programming%20Framework.md) - - -# Generating PWM signal from CLI - -```ledpinpwm ``` - value = 0...100 - enable PWM generation with specified duty cycle - -```ledpinpwm``` - disable PWM generation ( disable to allow ws2812 LEDs updates in shared modes ) - - -# Example of driving LED - -It is possible to drive single color LED with brightness control. Current consumption should not be greater then 1-2ma, thus LED can be used for indication only. - -![alt text](/docs/assets/images/ledpinpwmled.png "led pin pwm led") - -# Example of driving powerfull white LED - -To drive power LED with brightness control, Mosfet should be used: - -![alt text](/docs/assets/images/ledpinpwmpowerled.png "led pin pwm power_led") - -# Programming tab example for using the LED pin as a PINIO, such as for turning a VTX or camera on and off -![screenshot of programming tab using led as pinio](/docs/assets/images/led-as-pinio.png) - diff --git a/docs/OSD Joystick.md b/docs/OSD Joystick.md index 9e0f677e455..9c62eb850d3 100644 --- a/docs/OSD Joystick.md +++ b/docs/OSD Joystick.md @@ -1,8 +1,8 @@ # OSD joystick -LED pin can be used to emulate 5key OSD joystick for OSD camera pin, while still driving ws2812 LEDs (shared functionality). +A PINIO channel can be used to emulate a 5-key OSD joystick for OSD camera control. -See [LED pin PWM](LED%20pin%20PWM.md) for more details. +See [PINIO PWM](PINIO%20PWM.md) for more details. Note that for cameras which support RuncamDevice protocol, there is alternative functionality using serial communication: [Runcam device](Runcam%20device.md) @@ -22,17 +22,17 @@ To simulate 5key joystick, it is sufficient to generate correct voltage on camer # Enabling OSD Joystick emulation -```set led_pin_pwm_mode=shared_high``` - ```set osd_joystick_enabled=on``` -Also enable "Multi-color RGB LED Strip support" in Configuration tab. +```set osd_joystick_pinio_channel=``` + +Where `` is the PINIO channel (0-3) connected to the camera OSD pin. # Connection diagram -We use LED pin PWM functionality with RC filter to generate voltage: +We use PINIO PWM with an RC filter to generate voltage: -![alt text](/docs/assets/images/ledpinpwmfilter.png "led pin pwm filter") +![alt text](/docs/assets/images/ledpinpwmfilter.png "PINIO PWM filter") # Example PCB layout (SMD components) @@ -48,7 +48,7 @@ If default voltages does not work with your camera model, then you have to measu 2. Measure voltages on OSD pin while each key is pressed. 3. Connect camera to FC throught RC filter as shown on schematix above. 4. Enable OSD Joystick emulation (see "Enabling OSD Joystick emulation" above) -4. Use cli command ```led_pin_pwm ```, value = 0...100 to find out PWM values for each voltage. +4. Use CLI command `piniopwm `, value = 0...100 to find out PWM values for each voltage. 5. Specify PWM values in configuration and save: ```set osd_joystick_down=0``` @@ -87,7 +87,7 @@ There are 3 RC Boxes which can be used in armed and unarmed state: - Camera 2 - Up - Camera 3 - Down -Other keys can be emulated using Programming framework ( see [LED pin PWM](LED%20pin%20PWM.md) for more details ). +Other keys can be emulated using the Programming framework (see [PINIO PWM](PINIO%20PWM.md) for more details). # Behavior on boot diff --git a/docs/PINIO PWM.md b/docs/PINIO PWM.md new file mode 100644 index 00000000000..b1be27cc046 --- /dev/null +++ b/docs/PINIO PWM.md @@ -0,0 +1,93 @@ +# PINIO PWM + +INAV provides two mechanisms for generating output signals on GPIO pins: + +1. **PINIO channels (1-4)** — PWM-capable timer outputs, either defined in the target (`PINIOx_PIN`) or assigned by the user in the configurator's Output tab. Supports full 0-100% duty cycle PWM at 24 kHz. +2. **LED strip idle level (channel 0)** — The WS2812 LED strip pin can be switched between idle-LOW and idle-HIGH between LED update bursts. Binary on/off only. + +## PINIO PWM channels + +PINIO channels can come from two sources: + +1. **Target-defined:** `PINIO1_PIN` through `PINIO4_PIN` in `target.h`. These are always available on supported boards. +2. **User-assigned:** Any timer output pad can be set to PINIO mode in the configurator's Output tab (timer_output_mode = PINIO). No target.h changes needed. + +When a PINIO pin has a hardware timer, it is automatically configured as a 24 kHz PWM output. Pins without a timer fall back to GPIO on/off. + +PWM duty cycle can be controlled via: +- **CLI:** `piniopwm ` (channel = 1-4, duty = 0-100) +- **Programming framework:** Operation 52, Operand A = duty (0-100), Operand B = channel (1-4) +- **Mode boxes:** USER1-USER4 in the Modes tab toggle the channel on/off + +Setting duty to 0 stops PWM generation (pin goes LOW, or HIGH if `PINIO_FLAGS_INVERTED` is set in target.h). + +Feature can be used to drive external devices such as a VTX power switch. Setting the PWM duty cycle to 100% or 0% effectively provides a digital on/off output. It is also used to simulate [OSD joystick](OSD%20Joystick.md) to control cameras. + +PWM frequency is fixed to 24kHz with duty ratio between 0 and 100%: + +![alt text](/docs/assets/images/led_pin_pwm.png "PINIO PWM signal") + +## Mode box and programming framework interaction + +PINIO channels can be controlled by both mode boxes (Modes tab) and the programming framework (Programming tab). When both are used: + +- The **programming framework** sets the duty cycle (0-100%). +- The **mode box** gates the output on or off. When the mode is active, the programmed duty is output. When inactive, the pin is driven to 0%. + +If no RC channel range is assigned to the USERx mode in the Modes tab, the programming framework has exclusive uninterrupted control — the mode box does not interfere. + +The default duty (before the programming framework sets a value) is 100%, so toggling a mode box without any programming gives full on/off behavior. + +## LED strip idle level (channel 0) + +When the LED strip feature is enabled, the WS2812 pin sends data bursts (~1 ms) every 10-20 ms. Between bursts, the pin idles at a configurable level. + +The LED strip idle level is accessible as channel `0`: + +- **CLI:** `piniopwm 0 ` — value > 0 sets idle HIGH, 0 sets idle LOW +- **Programming framework:** Operation 52, Operand A = value (>0 = HIGH, 0 = LOW), Operand B = 0 + +This can be used to drive a MOSFET or similar device connected to the LED pin, toggled by the programming framework based on flight mode, RC channel, GPS state, etc. + +*Note: there will be a ~2 second LOW pulse on the LED pin during boot.* + +### LED strip idle level timing + +Normally LED pin is held low between WS2812 updates: + +![alt text](/docs/assets/images/ws2811_packets.png "ws2811 packets") +![alt text](/docs/assets/images/ws2811_data.png "ws2811 data") + +When idle is set HIGH, the pin is held high between updates. Total ws2812 pulse duration is ~1ms with ~9ms pauses. Connected devices should be tolerant of these brief transients. + +# Channel numbering + +| Channel | Target | Description | +|---------|--------|-------------| +| 0 | LED strip | Binary idle level (HIGH/LOW) | +| 1-4 | PINIO 1-4 | Full 0-100% PWM at 24 kHz | + +This numbering is consistent across the CLI (`piniopwm`), programming framework (operation 52), and the `pinio_box1`-`pinio_box4` settings. + +# Generating PWM/output signals from CLI + +`piniopwm [channel] ` — channel = 0-4, duty = 0-100 + +- One argument: sets LED idle level (channel 0, backward compatible) +- Two arguments: first is channel (0 = LED idle, 1-4 = PINIO), second is duty +- No arguments: stops PWM on PINIO 1 + +# Example of driving LED + +It is possible to drive single color LED with brightness control. Current consumption should not be greater then 1-2ma, thus LED can be used for indication only. + +![alt text](/docs/assets/images/ledpinpwmled.png "PINIO PWM LED") + +# Example of driving powerful white LED + +To drive power LED with brightness control, a MOSFET should be used: + +![alt text](/docs/assets/images/ledpinpwmpowerled.png "PINIO PWM power LED") + +# Programming tab example for using a PINIO channel to switch a VTX or camera on and off +![screenshot of programming tab using PINIO](/docs/assets/images/led-as-pinio.png) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 4233f78487d..c30f5a69aea 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -114,7 +114,7 @@ for complete documentation on using JavaScript to program your flight controller | 49 | Timer | A simple on - off timer. `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. | | 50 | Delta | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. ( \|ΔA\| >= B ) | | 51 | Approx Equals (A ~ B) | `true` if `Operand B` is within 1% of `Operand A`. | -| 52 | LED Pin PWM | Value `Operand A` from [`0` : `100`] PWM / PINIO generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes). | +| 52 | PINIO PWM | `Operand A` = duty cycle (0-100). `Operand B` = channel (0 for LED strip idle level, 1-4 for PINIO channels). Channels 1-4 support full PWM; channel 0 is binary (>0 = HIGH). See [PINIO PWM](PINIO%20PWM.md). | | 53 | Disable GPS Sensor Fix | Disables the GNSS sensor fix. For testing GNSS failure. | | 54 | Mag calibration | Trigger a magnetometer calibration. | | 55 | Set Gimbal Sensitivity | Scales `Operand A` from [`-16` : `15`] diff --git a/docs/Rx.md b/docs/Rx.md index 43f9d852716..0f0ae64a453 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -185,7 +185,7 @@ set rssi_channel = 0 Setting these values differently may have an adverse effects on RSSI readings. -#### CLI Bind Command +#### CLI Bind Commands This command will put the receiver into bind mode without the need to reboot the FC as it was required with the older `spektrum_sat_bind` command. @@ -193,6 +193,12 @@ This command will put the receiver into bind mode without the need to reboot the bind_rx ``` +This command will send a bind request to an MSP receiver on the specified port. + +``` +bind_msp_rx +``` + ## MultiWii serial protocol (MSP RX) Allows you to use MSP commands as the RC input. Up to 18 channels are supported. diff --git a/docs/Settings.md b/docs/Settings.md index 4e577bbedd5..4b6ae28160e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2332,16 +2332,6 @@ Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened w --- -### led_pin_pwm_mode - -PWM mode of LED pin. - -| Default | Min | Max | -| --- | --- | --- | -| SHARED_LOW | | | - ---- - ### limit_attn_filter_cutoff Throttle attenuation PI control output filter cutoff frequency @@ -5072,6 +5062,16 @@ PWM value for LEFT key --- +### osd_joystick_pinio_channel + +PINIO channel index (0-3) for the camera OSD control pin + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 3 | + +--- + ### osd_joystick_right PWM value for RIGHT key @@ -5574,41 +5574,41 @@ Pilot name ### pinio_box1 -Mode assignment for PINIO#1 +Mode box assignment for PINIO channel 1 | Default | Min | Max | | --- | --- | --- | -| `BOX_PERMANENT_ID_NONE` | 0 | 255 | +| `BOX_PERMANENT_ID_USER1` | 0 | 255 | --- ### pinio_box2 -Mode assignment for PINIO#1 +Mode box assignment for PINIO channel 2 | Default | Min | Max | | --- | --- | --- | -| `BOX_PERMANENT_ID_NONE` | 0 | 255 | +| `BOX_PERMANENT_ID_USER2` | 0 | 255 | --- ### pinio_box3 -Mode assignment for PINIO#1 +Mode box assignment for PINIO channel 3 | Default | Min | Max | | --- | --- | --- | -| `BOX_PERMANENT_ID_NONE` | 0 | 255 | +| `BOX_PERMANENT_ID_USER3` | 0 | 255 | --- ### pinio_box4 -Mode assignment for PINIO#1 +Mode box assignment for PINIO channel 4 | Default | Min | Max | | --- | --- | --- | -| `BOX_PERMANENT_ID_NONE` | 0 | 255 | +| `BOX_PERMANENT_ID_USER4` | 0 | 255 | --- diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index 7f31af7908b..d85974a65c0 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -412,7 +412,8 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i [8723 - MSP2_INAV_SET_GEOZONE_VERTEX](#msp2_inav_set_geozone_vertex) [8724 - MSP2_INAV_SET_GVAR](#msp2_inav_set_gvar) [8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose) -[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) +[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) +[12289 - MSP2_RX_BIND](#msp2_rx_bind) ## `MSP_API_VERSION (1 / 0x1)` **Description:** Provides the MSP protocol version and the INAV API version. @@ -4534,3 +4535,20 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **Notes:** Requires `rxConfig()->receiverType == RX_TYPE_SERIAL`. Requires `USE_SERIALRX_CRSF` or `USE_SERIALRX_SRXL2`. Calls `crsfBind()` or `srxl2Bind()` respectively. Returns error if receiver type or provider is not supported for binding. +## `MSP2_RX_BIND (12289 / 0x3001)` +**Description:** Initiates binding for MSP receivers (mLRS). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `port_id` | `uint8_t` | 1 | Port ID | +| `reserved_for_custom_use` | `uint8_t[3]` | 3 | Reserved for custom use | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `port_id` | `uint8_t` | 1 | Port ID | +| `reserved_for_custom_use` | `uint8_t[3]` | 3 | Reserved for custom use | + +**Notes:** Requires a receiver using MSP as the protocol, sends MSP2_RX_BIND to the receiver. + diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 84d603de8e1..1c0e36c6375 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -10944,5 +10944,47 @@ "reply": null, "notes": "Requires `rxConfig()->receiverType == RX_TYPE_SERIAL`. Requires `USE_SERIALRX_CRSF` or `USE_SERIALRX_SRXL2`. Calls `crsfBind()` or `srxl2Bind()` respectively. Returns error if receiver type or provider is not supported for binding.", "description": "Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2)." + }, + "MSP2_RX_BIND": { + "code": 12289, + "mspv": 2, + "request": { + "payload": [ + { + "name": "port_id", + "ctype": "uint8_t", + "desc": "Port ID", + "units": "" + }, + { + "name": "reserved_for_custom_use", + "ctype": "uint8_t", + "desc": "Reserved for custom use", + "units": "", + "array": true, + "array_size": 3 + } + ] + }, + "reply": { + "payload": [ + { + "name": "port_id", + "ctype": "uint8_t", + "desc": "Port ID", + "units": "" + }, + { + "name": "reserved_for_custom_use", + "ctype": "uint8_t", + "desc": "Reserved for custom use", + "units": "", + "array": true, + "array_size": 3 + } + ] + }, + "notes": "Requires a receiver using MSP as the protocol, sends MSP2_RX_BIND to the receiver.", + "description": "Initiates binding for MSP receivers (mLRS)." } } 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 d05ecf5f8e1..54acdd7a7c6 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 @@ -125,6 +125,7 @@ extern USBD_Class_cb_TypeDef USBD_CDC_cb; /** @defgroup USB_CORE_Exported_Functions * @{ */ +void USBD_CDC_ReceivePacket(void *pdev); /** * @} */ diff --git a/lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c b/lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c index a82ef08a2f0..2f3eca62e6e 100644 --- a/lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c +++ b/lib/main/STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c @@ -674,7 +674,10 @@ uint8_t usbd_cdc_DataOut (void *pdev, uint8_t epnum) /* USB data will be immediately processed, this allow next USB traffic being NAKed till the end of the application Xfer */ - APP_FOPS.pIf_DataRx(USB_Rx_Buffer, USB_Rx_Cnt); + if (APP_FOPS.pIf_DataRx(USB_Rx_Buffer, USB_Rx_Cnt) != USBD_OK) + { + return USBD_OK; + } /* Prepare Out endpoint to receive next packet */ DCD_EP_PrepareRx(pdev, @@ -685,6 +688,14 @@ uint8_t usbd_cdc_DataOut (void *pdev, uint8_t epnum) return USBD_OK; } +void USBD_CDC_ReceivePacket(void *pdev) +{ + DCD_EP_PrepareRx(pdev, + CDC_OUT_EP, + (uint8_t*)(USB_Rx_Buffer), + CDC_DATA_OUT_PACKET_SIZE); +} + /** * @brief usbd_audio_SOF * Start Of Frame event management @@ -696,7 +707,7 @@ uint8_t usbd_cdc_SOF (void *pdev) { static uint32_t FrameCount = 0; - if (FrameCount++ == CDC_IN_FRAME_INTERVAL) + if (FrameCount++ >= CDC_IN_FRAME_INTERVAL) { /* Reset the frame counter */ FrameCount = 0; diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index fe5f405d032..cc052fcd872 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -43,26 +43,17 @@ #include "drivers/timer.h" #include "drivers/light_ws2811strip.h" -#include "config/parameter_group_ids.h" -#include "fc/settings.h" #include "fc/runtime_config.h" #define WS2811_PERIOD (WS2811_TIMER_HZ / WS2811_CARRIER_HZ) #define WS2811_BIT_COMPARE_1 ((WS2811_PERIOD * 2) / 3) #define WS2811_BIT_COMPARE_0 (WS2811_PERIOD / 3) -PG_REGISTER_WITH_RESET_TEMPLATE(ledPinConfig_t, ledPinConfig, PG_LEDPIN_CONFIG, 0); - -PG_RESET_TEMPLATE(ledPinConfig_t, ledPinConfig, - .led_pin_pwm_mode = SETTING_LED_PIN_PWM_MODE_DEFAULT -); - static DMA_RAM timerDMASafeType_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; static IO_t ws2811IO = IO_NONE; static TCH_t * ws2811TCH = NULL; static bool ws2811Initialised = false; -static bool pwmMode = false; static hsvColor_t ledColorBuffer[WS2811_LED_STRIP_LENGTH]; @@ -112,14 +103,6 @@ bool ledConfigureDMA(void) { return timerPWMConfigChannelDMA(ws2811TCH, ledStripDMABuffer, sizeof(ledStripDMABuffer[0]), WS2811_DMA_BUFFER_SIZE); } -void ledConfigurePWM(void) { - timerConfigBase(ws2811TCH, 100, WS2811_TIMER_HZ ); - timerPWMConfigChannel(ws2811TCH, 0); - timerPWMStart(ws2811TCH); - timerEnable(ws2811TCH); - pwmMode = true; -} - void ws2811LedStripInit(void) { const timerHardware_t * timHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); @@ -141,28 +124,17 @@ void ws2811LedStripInit(void) IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); IOConfigGPIOAF(ws2811IO, IOCFG_AF_PP_FAST, timHw->alternateFunction); - if (ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW) { - ledConfigurePWM(); - *timerCCR(ws2811TCH) = 0; - } else if (ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH) { - ledConfigurePWM(); - *timerCCR(ws2811TCH) = 100; - } else { - if (!ledConfigureDMA()) { - // If DMA failed - abort - ws2811Initialised = false; - return; - } - - // Zero out DMA buffer - memset(&ledStripDMABuffer, 0, sizeof(ledStripDMABuffer)); - if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_SHARED_HIGH ) { - ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE-1] = 255; - } - ws2811Initialised = true; - - ws2811UpdateStrip(); + if (!ledConfigureDMA()) { + // If DMA failed - abort + ws2811Initialised = false; + return; } + + // Zero out DMA buffer — LED pin idles LOW between WS2812 bursts + memset(&ledStripDMABuffer, 0, sizeof(ledStripDMABuffer)); + ws2811Initialised = true; + + ws2811UpdateStrip(); } bool isWS2811LedStripReady(void) @@ -191,7 +163,7 @@ void ws2811UpdateStrip(void) static rgbColor24bpp_t *rgb24; // don't wait - risk of infinite block, just get an update next time round - if (pwmMode || timerPWMDMAInProgress(ws2811TCH)) { + if (timerPWMDMAInProgress(ws2811TCH)) { return; } @@ -216,40 +188,9 @@ void ws2811UpdateStrip(void) timerPWMStartDMA(ws2811TCH); } -//value -void ledPinStartPWM(uint16_t value) { - if (ws2811TCH == NULL) { - return; - } - - if ( !pwmMode ) { - timerPWMStopDMA(ws2811TCH); - //FIXME: implement method to release DMA - ws2811TCH->dma->owner = OWNER_FREE; - - ledConfigurePWM(); - } - *timerCCR(ws2811TCH) = value; -} - -void ledPinStopPWM(void) { - if (ws2811TCH == NULL || !pwmMode ) { - return; - } - - if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH ) { - *timerCCR(ws2811TCH) = 100; - return; - } else if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW ) { - *timerCCR(ws2811TCH) = 0; - return; - } - pwmMode = false; - - if (!ledConfigureDMA()) { - ws2811Initialised = false; - } +void ws2811SetIdleHigh(bool high) +{ + ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE - 1] = high ? 255 : 0; } - #endif diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 94c36445ec7..d0edcc276ea 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -16,8 +16,10 @@ */ #pragma once + +#include + #include "common/color.h" -#include "config/parameter_group.h" #define WS2811_LED_STRIP_LENGTH 128 #define WS2811_BITS_PER_LED 24 @@ -30,27 +32,8 @@ #define WS2811_TIMER_HZ 2400000 #define WS2811_CARRIER_HZ 800000 -typedef enum { - LED_PIN_PWM_MODE_SHARED_LOW = 0, - LED_PIN_PWM_MODE_SHARED_HIGH = 1, - LED_PIN_PWM_MODE_LOW = 2, - LED_PIN_PWM_MODE_HIGH = 3 -} led_pin_pwm_mode_e; - -typedef struct ledPinConfig_s { - uint8_t led_pin_pwm_mode; //led_pin_pwm_mode_e -} ledPinConfig_t; - -PG_DECLARE(ledPinConfig_t, ledPinConfig); - void ws2811LedStripInit(void); -void ws2811LedStripHardwareInit(void); -void ws2811LedStripDMAEnable(void); -bool ws2811LedStripDMAInProgress(void); - -//value 0...100 -void ledPinStartPWM(uint16_t value); -void ledPinStopPWM(void); +void ws2811SetIdleHigh(bool high); void ws2811UpdateStrip(void); diff --git a/src/main/drivers/pinio.c b/src/main/drivers/pinio.c index 3e89e5fccca..04f29c5cf59 100644 --- a/src/main/drivers/pinio.c +++ b/src/main/drivers/pinio.c @@ -28,6 +28,18 @@ #include "common/memory.h" #include "drivers/io.h" #include "drivers/pinio.h" +#ifdef USE_LED_STRIP +#include "drivers/light_ws2811strip.h" +#endif + +// CCR = duty% directly; 2.4 MHz / 100 = 24 kHz PWM, above audible range +#define PINIO_PWM_PERIOD 100 +#define PINIO_PWM_BASE_HZ 2400000 + +static inline uint8_t pinioEffectiveDuty(uint8_t duty, bool inverted) +{ + return inverted ? (100 - duty) : duty; +} /*** Hardware definitions ***/ const pinioHardware_t pinioHardware[] = { @@ -65,50 +77,136 @@ const int pinioHardwareCount = ARRAYLEN(pinioHardware); /*** Runtime configuration ***/ typedef struct pinioRuntime_s { IO_t io; + volatile timCCR_t *ccr; // Cached CCR register pointer (NULL for GPIO-only pins) bool inverted; - bool state; + bool active; // Mode box state; defaults to true (no RC channel = always active) + uint8_t duty; // Timer mode: duty level (0–100) applied by pinioSet(true); + // updated by pinioSetDuty(). Defaults to 100 so a mode box + // activating with no programming framework condition gives full on. } pinioRuntime_t; static pinioRuntime_t pinioRuntime[PINIO_COUNT]; +static int pinioRuntimeCount = 0; -void pinioInit(void) +// Configure one PINIO runtime slot in PWM mode. Returns false if no TCH available. +static bool pinioInitTimerPWM(int slot, IO_t io, const timerHardware_t *timHw, bool inverted) { - if (pinioHardwareCount == 0) { - return; + TCH_t *tch = timerGetTCH(timHw); + if (!tch) { + return false; } + IOInit(io, OWNER_PINIO, RESOURCE_OUTPUT, RESOURCE_INDEX(slot)); + IOConfigGPIOAF(io, IOCFG_AF_PP, timHw->alternateFunction); + timerConfigBase(tch, PINIO_PWM_PERIOD, PINIO_PWM_BASE_HZ); + timerPWMConfigChannel(tch, 0); + timerPWMStart(tch); + timerEnable(tch); + pinioRuntime[slot].ccr = timerCCR(tch); + pinioRuntime[slot].io = io; + pinioRuntime[slot].inverted = inverted; + pinioRuntime[slot].active = true; + pinioRuntime[slot].duty = 100; // default: mode box on = full on + *pinioRuntime[slot].ccr = pinioEffectiveDuty(0, inverted); // start off + return true; +} - for (int i = 0; i < pinioHardwareCount; i++) { - IO_t io = IOGetByTag(pinioHardware[i].ioTag); +void pinioInit(void) +{ + int runtimeCount = 0; + // Pass 1: target-defined PINIO pins (PINIO1_PIN–PINIO4_PIN in target.h). + // Timer-capable pins are configured as PWM; GPIO-only pins fall back to IOWrite. + // pwmMotorAndServoInit() runs before pinioInit(), so motor/servo pins are already + // owned and the OWNER_FREE check correctly skips dual-assigned pads. + for (int i = 0; i < pinioHardwareCount && runtimeCount < PINIO_COUNT; i++) { + IO_t io = IOGetByTag(pinioHardware[i].ioTag); if (!io) { continue; } - IOInit(io, OWNER_PINIO, RESOURCE_OUTPUT, RESOURCE_INDEX(i)); + bool inverted = (pinioHardware[i].flags & PINIO_FLAGS_INVERTED) != 0; + const timerHardware_t *timHw = timerGetByTag(pinioHardware[i].ioTag, TIM_USE_ANY); + if (timHw && IOGetOwner(io) == OWNER_FREE && pinioInitTimerPWM(runtimeCount, io, timHw, inverted)) { + runtimeCount++; + continue; + } + + // GPIO fallback: no timer available or pin already claimed + IOInit(io, OWNER_PINIO, RESOURCE_OUTPUT, RESOURCE_INDEX(runtimeCount)); IOConfigGPIO(io, pinioHardware[i].ioMode); + pinioRuntime[runtimeCount].inverted = inverted; + pinioRuntime[runtimeCount].io = io; + inverted ? IOHi(io) : IOLo(io); + runtimeCount++; + } - if (pinioHardware[i].flags & PINIO_FLAGS_INVERTED) { - pinioRuntime[i].inverted = true; - IOHi(io); - } else { - pinioRuntime[i].inverted = false; - IOLo(io); + // Pass 2: timer outputs assigned PINIO mode via the mixer (TIM_USE_PINIO flag). + // These pins have no PINIO_N_PIN target definition; the user assigns them in the + // configurator. pwmMotorAndServoInit() left them unclaimed; pick them up here. + for (int i = 0; i < timerHardwareCount && runtimeCount < PINIO_COUNT; i++) { + const timerHardware_t *timHw = &timerHardware[i]; + if (!TIM_IS_PINIO(timHw->usageFlags)) { + continue; } + IO_t io = IOGetByTag(timHw->tag); + if (!io || IOGetOwner(io) != OWNER_FREE) { + continue; + } + if (pinioInitTimerPWM(runtimeCount, io, timHw, false)) { + runtimeCount++; + } + } + + pinioRuntimeCount = runtimeCount; +} - pinioRuntime[i].io = io; - pinioRuntime[i].state = false; +int pinioGetRuntimeCount(void) +{ + return pinioRuntimeCount; +} + +void pinioSetDuty(int index, uint8_t duty) +{ +#ifdef USE_LED_STRIP + if (index == 0) { + ws2811SetIdleHigh(duty > 0); + return; + } +#endif + index--; // user-facing 1-4 → runtime 0-3 + if ((unsigned)index >= (unsigned)pinioRuntimeCount) { + return; + } + if (duty > 100) { + duty = 100; + } + if (pinioRuntime[index].ccr) { + pinioRuntime[index].duty = duty; + if (pinioRuntime[index].active) { + *pinioRuntime[index].ccr = pinioEffectiveDuty(duty, pinioRuntime[index].inverted); + } + } else { + IOWrite(pinioRuntime[index].io, (duty > 0) ^ pinioRuntime[index].inverted); } } +// pinioSet is called by PINIOBOX when an RC mode box is assigned to this channel. +// For GPIO channels: drives the pin high or low directly. +// For timer channels: active = output at stored duty (set by pinioSetDuty, default 100%); +// inactive = 0%. The stored duty is NOT modified, so deactivating and reactivating the +// mode box restores the programmed level. Channels with no mode box assigned are never +// called from PINIOBOX, giving the programming framework exclusive uninterrupted control. void pinioSet(int index, bool newState) { - if (!pinioRuntime[index].io) { + if ((unsigned)index >= (unsigned)pinioRuntimeCount) { return; } - - if (newState != pinioRuntime[index].state) { + if (pinioRuntime[index].ccr) { + pinioRuntime[index].active = newState; + uint8_t duty = newState ? pinioRuntime[index].duty : 0; + *pinioRuntime[index].ccr = pinioEffectiveDuty(duty, pinioRuntime[index].inverted); + } else { IOWrite(pinioRuntime[index].io, newState ^ pinioRuntime[index].inverted); - pinioRuntime[index].state = newState; } } #endif diff --git a/src/main/drivers/pinio.h b/src/main/drivers/pinio.h index a1de21c12de..cc55fad00d8 100644 --- a/src/main/drivers/pinio.h +++ b/src/main/drivers/pinio.h @@ -24,6 +24,7 @@ #include #include "drivers/io_types.h" +#include "drivers/timer.h" #define PINIO_COUNT 4 #define PINIO_FLAGS_INVERTED 0x80 @@ -39,3 +40,5 @@ extern const int pinioHardwareCount; void pinioInit(void); void pinioSet(int index, bool newState); +void pinioSetDuty(int index, uint8_t duty); +int pinioGetRuntimeCount(void); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 2d01127a504..32f279ba117 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -232,6 +232,10 @@ static void timerHardwareOverride(timerHardware_t * timer) { timer->usageFlags &= ~(TIM_USE_MOTOR|TIM_USE_SERVO); timer->usageFlags |= TIM_USE_LED; break; + case OUTPUT_MODE_PINIO: + timer->usageFlags &= ~(TIM_USE_MOTOR|TIM_USE_SERVO|TIM_USE_LED); + timer->usageFlags |= TIM_USE_PINIO; + break; } } diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 3f08d9b500a..c860166bc74 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -55,7 +55,8 @@ typedef enum { typedef enum { PIN_LABEL_NONE = 0, - PIN_LABEL_LED + PIN_LABEL_LED, + PIN_LABEL_PINIO_BASE = 2 // values 2..5 = USER1..USER4 (add channel index 0-3) } pinLabel_e; typedef enum { diff --git a/src/main/drivers/serial.c b/src/main/drivers/serial.c index 590a36002b2..1cba2609914 100644 --- a/src/main/drivers/serial.c +++ b/src/main/drivers/serial.c @@ -71,6 +71,15 @@ uint8_t serialRead(serialPort_t *instance) return instance->vTable->serialRead(instance); } +uint32_t serialReadBuf(serialPort_t *instance, uint8_t *data, uint32_t maxLen) +{ + uint32_t count = 0; + while (count < maxLen && serialRxBytesWaiting(instance)) { + data[count++] = instance->vTable->serialRead(instance); + } + return count; +} + void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate) { instance->vTable->serialSetBaudRate(instance, baudRate); diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index 8e66b5f8445..6ca0a7d1ec2 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -113,6 +113,7 @@ uint32_t serialRxBytesWaiting(const serialPort_t *instance); uint32_t serialTxBytesFree(const serialPort_t *instance); void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count); uint8_t serialRead(serialPort_t *instance); +uint32_t serialReadBuf(serialPort_t *instance, uint8_t *data, uint32_t maxLen); void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate); void serialSetMode(serialPort_t *instance, portMode_t mode); void serialSetOptions(serialPort_t *instance, portOptions_t options); diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index f076e0bfee2..950cc39cdf9 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -274,4 +274,21 @@ uint32_t usbVcpGetBaudRate(serialPort_t *instance) return CDC_BaudRate(); } +portOptions_t usbVcpGetLineCoding(void) +{ + portOptions_t options = SERIAL_NOT_INVERTED; + + // stop bits: CDC format 0=1 stop, 2=2 stop (1.5 not supported) + if (CDC_StopBits() == 2) { + options |= SERIAL_STOPBITS_2; + } + + // parity: CDC 0=none, 2=even (odd parity not supported in INAV) + if (CDC_Parity() == 2) { + options |= SERIAL_PARITY_EVEN; + } + + return options; +} + #endif diff --git a/src/main/drivers/serial_usb_vcp.h b/src/main/drivers/serial_usb_vcp.h index 4557421b638..fd47022ceae 100644 --- a/src/main/drivers/serial_usb_vcp.h +++ b/src/main/drivers/serial_usb_vcp.h @@ -31,3 +31,4 @@ void usbVcpInitHardware(void); serialPort_t *usbVcpOpen(void); struct serialPort_s; uint32_t usbVcpGetBaudRate(struct serialPort_s *instance); +portOptions_t usbVcpGetLineCoding(void); diff --git a/src/main/drivers/serial_usb_vcp_at32f43x.c b/src/main/drivers/serial_usb_vcp_at32f43x.c index 96b283ec363..32fd67fdb75 100644 --- a/src/main/drivers/serial_usb_vcp_at32f43x.c +++ b/src/main/drivers/serial_usb_vcp_at32f43x.c @@ -495,7 +495,25 @@ uint32_t usbVcpGetBaudRate(serialPort_t *instance) UNUSED(instance); cdc_struct_type *pcdc = (cdc_struct_type *)otg_core_struct.dev.class_handler->pdata; return pcdc->linecoding.bitrate; - // return CDC_BaudRate(); + // return CDC_BaudRate(); +} + +portOptions_t usbVcpGetLineCoding(void) +{ + portOptions_t options = 0; + cdc_struct_type *pcdc = (cdc_struct_type *)otg_core_struct.dev.class_handler->pdata; + + // stop bits: CDC format 0=1 stop, 2=2 stop (1.5 not supported) + if (pcdc->linecoding.format == 2) { + options |= SERIAL_STOPBITS_2; + } + + // parity: CDC 0=none, 2=even (odd parity not supported in INAV) + if (pcdc->linecoding.parity == 2) { + options |= SERIAL_PARITY_EVEN; + } + + return options; } #endif diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index d87e0400d52..217dea9ab18 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -117,6 +117,7 @@ typedef enum { //TIM_USE_FW_SERVO = (1 << 6), TIM_USE_LED = (1 << 24), // Remapping needs to be in the lower 8 bits. TIM_USE_BEEPER = (1 << 25), + TIM_USE_PINIO = (1 << 26), } timerUsageFlag_e; #define TIM_USE_OUTPUT_AUTO (TIM_USE_MOTOR | TIM_USE_SERVO) @@ -124,6 +125,7 @@ typedef enum { #define TIM_IS_MOTOR(flags) ((flags) & TIM_USE_MOTOR) #define TIM_IS_SERVO(flags) ((flags) & TIM_USE_SERVO) #define TIM_IS_LED(flags) ((flags) & TIM_USE_LED) +#define TIM_IS_PINIO(flags) ((flags) & TIM_USE_PINIO) #define TIM_IS_MOTOR_ONLY(flags) (TIM_IS_MOTOR(flags) && !TIM_IS_SERVO(flags)) #define TIM_IS_SERVO_ONLY(flags) (!TIM_IS_MOTOR(flags) && TIM_IS_SERVO(flags)) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 6c60f08c6ed..8778255adba 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -58,6 +58,8 @@ bool cliMode = false; #include "drivers/flash.h" #include "drivers/io.h" #include "drivers/io_impl.h" +#include "drivers/light_ws2811strip.h" +#include "drivers/pinio.h" #include "drivers/osd_symbols.h" #include "drivers/persistent.h" #include "drivers/sdcard/sdcard.h" @@ -68,8 +70,6 @@ bool cliMode = false; #include "drivers/time.h" #include "drivers/usb_msc.h" #include "drivers/vtx_common.h" -#include "drivers/light_ws2811strip.h" - #include "fc/fc_core.h" #include "fc/cli.h" #include "fc/config.h" @@ -106,6 +106,9 @@ bool cliMode = false; #include "rx/srxl2.h" #include "rx/crsf.h" +#include "msp/msp_serial.h" +#include "msp/msp_protocol_v2_common.h" + #include "scheduler/scheduler.h" #include "sensors/acceleration.h" @@ -170,6 +173,7 @@ static const char * outputModeNames[] = { "MOTORS", "SERVOS", "LED", + "PINIO", NULL }; @@ -2165,20 +2169,45 @@ static void cliModeColor(char *cmdline) } } -static void cliLedPinPWM(char *cmdline) + +#endif // USE_LED_STRIP + +#ifdef USE_PINIO +// Channel numbering: 0 = LED strip idle level, 1-4 = PINIO channels (matches programming framework) +static void cliPinioPwm(char *cmdline) { - int i; + int channel = 0; + int duty; if (isEmpty(cmdline)) { - ledPinStopPWM(); - cliPrintLine("PWM stopped"); + pinioSetDuty(1, 0); + cliPrintLine("PWM stopped on PINIO 1"); + return; + } + + const char *dutyStr = nextArg(cmdline); + if (dutyStr) { + channel = fastA2I(cmdline); + duty = fastA2I(dutyStr); } else { - i = fastA2I(cmdline); - ledPinStartPWM(i); - cliPrintLinef("PWM started: %d%%",i); + // One arg: duty on channel 0 (LED idle, backward compat with old LED_PIN_PWM) + duty = fastA2I(cmdline); + } + + const int maxChannel = MAX(pinioGetRuntimeCount(), PINIO_COUNT); + if (channel < 0 || channel > maxChannel) { + cliShowArgumentRangeError("channel", 0, maxChannel); + return; + } + if (duty < 0 || duty > 100) { + cliShowArgumentRangeError("duty", 0, 100); + return; } + + pinioSetDuty(channel, (uint8_t)duty); + cliPrintLinef("PWM ch %d: %d%%", channel, duty); } -#endif +#endif // USE_PINIO static void cliDelay(char* cmdLine) { int ms = 0; @@ -3188,6 +3217,8 @@ static void cliTimerOutputMode(char *cmdline) mode = OUTPUT_MODE_SERVOS; } else if(!sl_strcasecmp("LED", tok)) { mode = OUTPUT_MODE_LED; + } else if(!sl_strcasecmp("PINIO", tok)) { + mode = OUTPUT_MODE_PINIO; } else { cliShowParseError(); return; @@ -3600,6 +3631,41 @@ void cliRxBind(char *cmdline){ } #endif +static void cliBindMspRx(char *cmdline) +{ + if (isEmpty(cmdline)) { + cliShowParseError(); + return; + } + + int portIndex = fastA2I(cmdline); + + if (portIndex < 0 || portIndex > 7) { + cliShowArgumentRangeError("port", 0, 7); + return; + } + + serialPortUsage_t *portUsage = findSerialPortUsageByIdentifier(portIndex); + if (!portUsage || !portUsage->serialPort) { + cliPrintErrorLinef("Serial port %d is not open", portIndex); + return; + } + + mspPort_t *mspPort = mspSerialPortFind(portUsage->serialPort); + if (!mspPort) { + cliPrintErrorLinef("Serial port %d is not configured for MSP", portIndex); + return; + } + + uint8_t payload[4] = { portIndex, 0, 0, 0 }; + int sent = mspSerialPushPort(MSP2_RX_BIND, payload, sizeof(payload), mspPort, MSP_V2_NATIVE); // this is sent as a response + if (sent > 0) { + cliPrintLinef("Sent MSP2_RX_BIND to serial port %d", portIndex); + } else { + cliPrintErrorLinef("Failed to send MSP2_RX_BIND to serial port %d", portIndex); + } +} + static void cliExit(char *cmdline) { UNUSED(cmdline); @@ -4829,6 +4895,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n" "\t<+|->[name]", cliBeeper), #endif + CLI_COMMAND_DEF("bind_msp_rx", "initiate binding for MSP receivers (mLRS)", "", cliBindMspRx), #if defined (USE_SERIALRX_SRXL2) CLI_COMMAND_DEF("bind_rx", "initiate binding for RX SPI or SRXL2", NULL, cliRxBind), #endif @@ -4880,7 +4947,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("help", NULL, NULL, cliHelp), #ifdef USE_LED_STRIP CLI_COMMAND_DEF("led", "configure leds", NULL, cliLed), - CLI_COMMAND_DEF("ledpinpwm", "start/stop PWM on LED pin, 0..100 duty ratio", "[]\r\n", cliLedPinPWM), +#endif +#ifdef USE_PINIO + CLI_COMMAND_DEF("piniopwm", "set PINIO PWM duty cycle", "[] \r\n", cliPinioPwm), #endif CLI_COMMAND_DEF("map", "configure rc channel order", "[]", cliMap), CLI_COMMAND_DEF("memory", "view memory usage", NULL, cliMemory), @@ -4941,7 +5010,7 @@ const clicmd_t cmdTable[] = { #ifdef USE_OSD CLI_COMMAND_DEF("osd_layout", "get or set the layout of OSD items", "[ [ [ []]]]", cliOsdLayout), #endif - CLI_COMMAND_DEF("timer_output_mode", "get or set the outputmode for a given timer.", "[ []]", cliTimerOutputMode), + CLI_COMMAND_DEF("timer_output_mode", "get or set the outputmode for a given timer.", "[ []]", cliTimerOutputMode), }; static void cliHelp(char *cmdline) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 45379d1caa4..09ccd7eaee5 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -55,6 +55,9 @@ #include "drivers/osd.h" #include "drivers/osd_symbols.h" #include "drivers/pwm_mapping.h" +#ifdef USE_PINIO +#include "drivers/pinio.h" +#endif #include "drivers/sdcard/sdcard.h" #include "drivers/serial.h" #include "drivers/system.h" @@ -1672,6 +1675,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #if !defined(SITL_BUILD) && defined(WS2811_PIN) ioTag_t led_tag = IO_TAG(WS2811_PIN); #endif + #ifdef USE_PINIO + int nextPinioIndex = pinioHardwareCount; + #endif for (uint8_t i = 0; i < timerHardwareCount; ++i) if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) { @@ -1681,12 +1687,33 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, timer2id(timerHardware[i].tim)); #endif sbufWriteU32(dst, timerHardware[i].usageFlags); - #if defined(SITL_BUILD) || !defined(WS2811_PIN) + #if defined(SITL_BUILD) sbufWriteU8(dst, 0); #else - // Extra label to help identify repurposed PINs. - // Eventually, we can try to add more labels for PPM pins, etc. - sbufWriteU8(dst, timerHardware[i].tag == led_tag ? PIN_LABEL_LED : PIN_LABEL_NONE); + { + uint8_t specialLabel = PIN_LABEL_NONE; + #if defined(WS2811_PIN) + if (timerHardware[i].tag == led_tag) { + specialLabel = PIN_LABEL_LED; + } + #endif + #ifdef USE_PINIO + if (specialLabel == PIN_LABEL_NONE) { + for (int j = 0; j < pinioHardwareCount; j++) { + if (timerHardware[i].tag == pinioHardware[j].ioTag) { + specialLabel = PIN_LABEL_PINIO_BASE + j; + break; + } + } + } + // Timer-override PINIO pins: assign next USER index (up to PINIO_COUNT) + if (specialLabel == PIN_LABEL_NONE && (timerHardware[i].usageFlags & TIM_USE_PINIO) && nextPinioIndex < PINIO_COUNT) { + specialLabel = PIN_LABEL_PINIO_BASE + nextPinioIndex; + nextPinioIndex++; + } + #endif + sbufWriteU8(dst, specialLabel); + } #endif } } diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 4ca125d5c6a..5cfb7dec86d 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -420,7 +420,7 @@ void fcTasksInit(void) #endif #endif #ifdef USE_RCDEVICE -#ifdef USE_LED_STRIP +#ifdef USE_PINIO setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled() || osdJoystickEnabled()); #else setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled()); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 476dfe09ff7..ede01910544 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -189,9 +189,6 @@ tables: - name: nav_mc_althold_throttle values: ["STICK", "MID_STICK", "HOVER"] enum: navMcAltHoldThrottle_e - - name: led_pin_pwm_mode - values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"] - enum: led_pin_pwm_mode_e - name: gyro_filter_mode values: ["OFF", "STATIC", "DYNAMIC", "ADAPTIVE"] enum: gyroFilterType_e @@ -4107,35 +4104,31 @@ groups: members: - name: pinio_box1 field: permanentId[0] - description: "Mode assignment for PINIO#1" - default_value: "target specific" + description: "Mode box assignment for PINIO channel 1" min: 0 max: 255 - default_value: :BOX_PERMANENT_ID_NONE + default_value: :BOX_PERMANENT_ID_USER1 type: uint8_t - name: pinio_box2 field: permanentId[1] - default_value: "target specific" - description: "Mode assignment for PINIO#1" + description: "Mode box assignment for PINIO channel 2" min: 0 max: 255 - default_value: :BOX_PERMANENT_ID_NONE + default_value: :BOX_PERMANENT_ID_USER2 type: uint8_t - name: pinio_box3 field: permanentId[2] - default_value: "target specific" - description: "Mode assignment for PINIO#1" + description: "Mode box assignment for PINIO channel 3" min: 0 max: 255 - default_value: :BOX_PERMANENT_ID_NONE + default_value: :BOX_PERMANENT_ID_USER3 type: uint8_t - name: pinio_box4 field: permanentId[3] - default_value: "target specific" - description: "Mode assignment for PINIO#1" + description: "Mode box assignment for PINIO channel 4" min: 0 max: 255 - default_value: :BOX_PERMANENT_ID_NONE + default_value: :BOX_PERMANENT_ID_USER4 type: uint8_t - name: PG_LOG_CONFIG @@ -4265,27 +4258,22 @@ groups: field: attnFilterCutoff max: 100 - - name: PG_LEDPIN_CONFIG - type: ledPinConfig_t - headers: ["drivers/light_ws2811strip.h"] - members: - - name: led_pin_pwm_mode - condition: USE_LED_STRIP - description: "PWM mode of LED pin." - default_value: "SHARED_LOW" - field: led_pin_pwm_mode - table: led_pin_pwm_mode - - name: PG_OSD_JOYSTICK_CONFIG type: osdJoystickConfig_t headers: ["io/osd_joystick.h"] - condition: USE_RCDEVICE && USE_LED_STRIP + condition: USE_RCDEVICE && USE_PINIO members: - name: osd_joystick_enabled description: "Enable OSD Joystick emulation" default_value: OFF field: osd_joystick_enabled type: bool + - name: osd_joystick_pinio_channel + description: "PINIO channel index (0-3) for the camera OSD control pin" + default_value: 0 + field: pinio_channel + min: 0 + max: 3 - name: osd_joystick_down description: "PWM value for DOWN key" default_value: 0 diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 12688bd2c09..6c4370d4176 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -48,7 +48,8 @@ typedef enum { OUTPUT_MODE_AUTO = 0, OUTPUT_MODE_MOTORS, OUTPUT_MODE_SERVOS, - OUTPUT_MODE_LED + OUTPUT_MODE_LED, + OUTPUT_MODE_PINIO } outputMode_e; typedef struct motorAxisCorrectionLimits_s { diff --git a/src/main/io/osd_joystick.c b/src/main/io/osd_joystick.c index c1d9dee5a5a..9fba216fe6d 100644 --- a/src/main/io/osd_joystick.c +++ b/src/main/io/osd_joystick.c @@ -17,7 +17,7 @@ #include "fc/runtime_config.h" #include "drivers/time.h" -#include "drivers/light_ws2811strip.h" +#include "drivers/pinio.h" #include "io/serial.h" #include "io/rcdevice.h" @@ -25,13 +25,14 @@ #include "osd_joystick.h" #ifdef USE_RCDEVICE -#ifdef USE_LED_STRIP +#ifdef USE_PINIO -PG_REGISTER_WITH_RESET_TEMPLATE(osdJoystickConfig_t, osdJoystickConfig, PG_OSD_JOYSTICK_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(osdJoystickConfig_t, osdJoystickConfig, PG_OSD_JOYSTICK_CONFIG, 1); PG_RESET_TEMPLATE(osdJoystickConfig_t, osdJoystickConfig, .osd_joystick_enabled = SETTING_OSD_JOYSTICK_ENABLED_DEFAULT, + .pinio_channel = SETTING_OSD_JOYSTICK_PINIO_CHANNEL_DEFAULT, .osd_joystick_down = SETTING_OSD_JOYSTICK_DOWN_DEFAULT, .osd_joystick_up = SETTING_OSD_JOYSTICK_UP_DEFAULT, .osd_joystick_left = SETTING_OSD_JOYSTICK_LEFT_DEFAULT, @@ -45,28 +46,29 @@ bool osdJoystickEnabled(void) { void osdJoystickSimulate5KeyButtonPress(uint8_t operation) { + const int ch = osdJoystickConfig()->pinio_channel + 1; // setting is 0-indexed, pinioSetDuty is 1-indexed switch (operation) { case RCDEVICE_CAM_KEY_ENTER: - ledPinStartPWM( osdJoystickConfig()->osd_joystick_enter ); + pinioSetDuty(ch, osdJoystickConfig()->osd_joystick_enter); break; case RCDEVICE_CAM_KEY_LEFT: - ledPinStartPWM( osdJoystickConfig()->osd_joystick_left ); + pinioSetDuty(ch, osdJoystickConfig()->osd_joystick_left); break; case RCDEVICE_CAM_KEY_UP: - ledPinStartPWM( osdJoystickConfig()->osd_joystick_up ); + pinioSetDuty(ch, osdJoystickConfig()->osd_joystick_up); break; case RCDEVICE_CAM_KEY_RIGHT: - ledPinStartPWM( osdJoystickConfig()->osd_joystick_right ); + pinioSetDuty(ch, osdJoystickConfig()->osd_joystick_right); break; case RCDEVICE_CAM_KEY_DOWN: - ledPinStartPWM( osdJoystickConfig()->osd_joystick_down ); + pinioSetDuty(ch, osdJoystickConfig()->osd_joystick_down); break; } } void osdJoystickSimulate5KeyButtonRelease(void) { - ledPinStopPWM(); + pinioSetDuty(osdJoystickConfig()->pinio_channel + 1, 0); } diff --git a/src/main/io/osd_joystick.h b/src/main/io/osd_joystick.h index 574b8e3b776..15f52134692 100644 --- a/src/main/io/osd_joystick.h +++ b/src/main/io/osd_joystick.h @@ -2,11 +2,12 @@ #include "config/parameter_group.h" -#ifdef USE_RCDEVICE -#ifdef USE_LED_STRIP +#ifdef USE_RCDEVICE +#ifdef USE_PINIO typedef struct osdJoystickConfig_s { bool osd_joystick_enabled; + uint8_t pinio_channel; // PINIO index for the cam-control output pin uint8_t osd_joystick_down; uint8_t osd_joystick_up; uint8_t osd_joystick_left; diff --git a/src/main/io/piniobox.c b/src/main/io/piniobox.c index 4fbcc657e4a..e9942a58d0b 100644 --- a/src/main/io/piniobox.c +++ b/src/main/io/piniobox.c @@ -63,7 +63,8 @@ void pinioBoxInit(void) void pinioBoxUpdate(void) { for (int i = 0; i < PINIO_COUNT; i++) { - if (pinioBoxRuntimeConfig.boxId[i] != BOXID_NONE) { + if (pinioBoxRuntimeConfig.boxId[i] != BOXID_NONE && + isModeActivationConditionPresent(pinioBoxRuntimeConfig.boxId[i])) { pinioSet(i, IS_RC_MODE_ACTIVE(pinioBoxRuntimeConfig.boxId[i])); } } diff --git a/src/main/io/rcdevice_cam.c b/src/main/io/rcdevice_cam.c index 0edbc8bf979..1388ca170ee 100644 --- a/src/main/io/rcdevice_cam.c +++ b/src/main/io/rcdevice_cam.c @@ -49,7 +49,7 @@ bool waitingDeviceResponse = false; static bool isFeatureSupported(uint8_t feature) { #ifndef UNIT_TEST -#ifdef USE_LED_STRIP +#ifdef USE_PINIO if (!rcdeviceIsEnabled() && osdJoystickEnabled() ) { return true; } @@ -117,7 +117,7 @@ static void rcdeviceCameraControlProcess(void) switchStates[switchIndex].isActivated = true; } #ifndef UNIT_TEST -#ifdef USE_LED_STRIP +#ifdef USE_PINIO else if ((behavior1 != RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION) && osdJoystickEnabled()) { switch (behavior1) { case RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN: @@ -137,7 +137,7 @@ static void rcdeviceCameraControlProcess(void) UNUSED(behavior1); } else { #ifndef UNIT_TEST -#ifdef USE_LED_STRIP +#ifdef USE_PINIO if (osdJoystickEnabled() && switchStates[switchIndex].isActivated) { osdJoystickSimulate5KeyButtonRelease(); } @@ -275,7 +275,7 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) waitingDeviceResponse = true; } #ifndef UNIT_TEST -#ifdef USE_LED_STRIP +#ifdef USE_PINIO else if (osdJoystickEnabled()) { osdJoystickSimulate5KeyButtonRelease(); isButtonPressed = false; @@ -320,7 +320,7 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) waitingDeviceResponse = true; } #ifndef UNIT_TEST -#ifdef USE_LED_STRIP +#ifdef USE_PINIO else if (osdJoystickEnabled()) { if ( key == RCDEVICE_CAM_KEY_CONNECTION_OPEN ) { rcdeviceInMenu = true; diff --git a/src/main/io/serial.c b/src/main/io/serial.c index d6b82da02b8..d88f071e7cd 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -509,6 +509,85 @@ void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort) }; } +void escapeSequenceInit(escapeSequenceState_t *state) +{ + state->lastCharTime = 0; + state->lastPlusTime = 0; + state->count = 0; +} + +void escapeSequenceProcessChar(escapeSequenceState_t *state, uint8_t c, uint32_t now) +{ + if (c == '+') { + if (state->count == 0) { + // First '+': check for leading guard interval + if (now - state->lastCharTime >= 1000) { + state->count = 1; + state->lastPlusTime = now; + } + } else if (state->count < 3) { + // Subsequent '+': must arrive within 1 second of previous '+' + if (now - state->lastPlusTime < 1000) { + state->count++; + state->lastPlusTime = now; + } else { + state->count = 0; // too much time between pluses + } + } else { + // More than 3 pluses - not a valid escape sequence + state->count = 0; + } + } else { + // Non-'+' character resets sequence + state->count = 0; + } + state->lastCharTime = now; +} + +bool escapeSequenceCheckGuard(escapeSequenceState_t *state, uint32_t now) +{ + if (state->count == 3) { + if (now - state->lastPlusTime >= 1000) { + return true; + } + } + return false; +} + +static bool serialPassthroughTransfer(serialPort_t *src, serialPort_t *dst, serialConsumer *consumer, escapeSequenceState_t *escapeState, uint32_t now) +{ + uint8_t buf[64]; + uint32_t available = serialRxBytesWaiting(src); + uint32_t free = serialTxBytesFree(dst); + uint32_t count = (available < free) ? available : free; + if (count > sizeof(buf)) { + count = sizeof(buf); + } + + if (count > 0) { + LED0_ON; + serialBeginWrite(dst); + serialReadBuf(src, buf, count); + serialWriteBuf(dst, buf, count); + for (uint32_t i = 0; i < count; i++) { + consumer(buf[i]); + // Hayes escape sequence detection: [1s silence]+++[1s silence] + // https://en.wikipedia.org/wiki/Escape_sequence#Modem_control + if (escapeState) { + escapeSequenceProcessChar(escapeState, buf[i], now); + } + } + serialEndWrite(dst); + LED0_OFF; + } else { + if (escapeState) { + return escapeSequenceCheckGuard(escapeState, now); + } + } + + return false; +} + #if defined(USE_GPS) || defined(USE_SERIAL_PASSTHROUGH) // Default data consumer for serialPassThrough. static void nopConsumer(uint8_t data) @@ -535,32 +614,54 @@ void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer LED0_OFF; LED1_OFF; +#ifdef USE_VCP + // Track current encoding applied to right port for VCP mirroring + portOptions_t currentOptions = right->options; + uint32_t currentBaudRate = right->baudRate; + bool leftIsVcp = (left->identifier == SERIAL_PORT_USB_VCP); + uint32_t lastMirrorTime = 0; +#endif + + static escapeSequenceState_t escapeSequenceState; + escapeSequenceInit(&escapeSequenceState); + // Either port might be open in a mode other than MODE_RXTX. We rely on // serialRxBytesWaiting() to do the right thing for a TX only port. No // special handling is necessary OR performed. while (1) { - // TODO: maintain a timestamp of last data received. Use this to - // implement a guard interval and check for `+++` as an escape sequence - // to return to CLI command mode. - // https://en.wikipedia.org/wiki/Escape_sequence#Modem_control - if (serialRxBytesWaiting(left)) { - LED0_ON; - uint8_t c = serialRead(left); - // Make sure there is space in the tx buffer - while (!serialTxBytesFree(right)); - serialWrite(right, c); - leftC(c); - LED0_OFF; - } - if (serialRxBytesWaiting(right)) { - LED0_ON; - uint8_t c = serialRead(right); - // Make sure there is space in the tx buffer - while (!serialTxBytesFree(left)); - serialWrite(left, c); - rightC(c); - LED0_OFF; - } + uint32_t now = millis(); +#ifdef USE_VCP + // Mirror line coding from host PC to passthrough port (rate-limited to 15ms) + if (leftIsVcp) { + if (now - lastMirrorTime >= 15) { + lastMirrorTime = now; + uint32_t hostBaudRate = usbVcpGetBaudRate(left); + portOptions_t hostOptions = usbVcpGetLineCoding(); + + // apply baud rate change + if (hostBaudRate != currentBaudRate && hostBaudRate != 0) { + serialSetBaudRate(right, hostBaudRate); + currentBaudRate = hostBaudRate; + } + + // apply encoding change (parity/stop bits) + if (hostOptions != currentOptions) { + serialSetOptions(right, hostOptions); + currentOptions = hostOptions; + } + } + } +#endif + + // Left (USB) to right (UART) + if (serialPassthroughTransfer(left, right, leftC, &escapeSequenceState, now)) { + return; + } + + // Right (UART) to left (USB) + serialPassthroughTransfer(right, left, rightC, NULL, now); } } #endif + + diff --git a/src/main/io/serial.h b/src/main/io/serial.h index a746dc77177..3e41a4475f9 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -139,6 +139,18 @@ PG_DECLARE(serialConfig_t, serialConfig); typedef void serialConsumer(uint8_t); +// Hayes escape sequence detection state: [1s silence]+++[1s silence] +// https://en.wikipedia.org/wiki/Escape_sequence#Modem_control +typedef struct escapeSequenceState_s { + uint32_t lastCharTime; + uint32_t lastPlusTime; + uint8_t count; +} escapeSequenceState_t; + +void escapeSequenceInit(escapeSequenceState_t *state); +void escapeSequenceProcessChar(escapeSequenceState_t *state, uint8_t c, uint32_t now); +bool escapeSequenceCheckGuard(escapeSequenceState_t *state, uint32_t now); + // // configuration // diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h index 411fbecf046..98f782b4896 100644 --- a/src/main/msp/msp_protocol_v2_common.h +++ b/src/main/msp/msp_protocol_v2_common.h @@ -39,3 +39,4 @@ #define MSP2_COMMON_GET_RADAR_GPS 0x100F //get radar position for other planes #define MSP2_BETAFLIGHT_BIND 0x3000 +#define MSP2_RX_BIND 0x3001 diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index edba68b8e94..500070316b9 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -58,7 +58,7 @@ #include "io/vtx.h" #include "drivers/vtx_common.h" -#include "drivers/light_ws2811strip.h" +#include "drivers/pinio.h" PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 4); @@ -506,16 +506,11 @@ static int logicConditionCompute( } break; -#ifdef USE_LED_STRIP - case LOGIC_CONDITION_LED_PIN_PWM: - - if (operandA >=0 && operandA <= 100) { - ledPinStartPWM((uint8_t)operandA); - } else { - ledPinStopPWM(); - } +#ifdef USE_PINIO + case LOGIC_CONDITION_PINIO_PWM: + // operandA = duty cycle (0-100), operandB = channel (0=LED idle, 1-4=PINIO) + pinioSetDuty(operandB, (uint8_t)constrain(operandA, 0, 100)); return operandA; - break; #endif #ifdef USE_GPS_FIX_ESTIMATION case LOGIC_CONDITION_DISABLE_GPS_FIX: diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 74ea96c98ee..859096aeee5 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -81,7 +81,7 @@ typedef enum { LOGIC_CONDITION_TIMER = 49, LOGIC_CONDITION_DELTA = 50, LOGIC_CONDITION_APPROX_EQUAL = 51, - LOGIC_CONDITION_LED_PIN_PWM = 52, + LOGIC_CONDITION_PINIO_PWM = 52, LOGIC_CONDITION_DISABLE_GPS_FIX = 53, LOGIC_CONDITION_RESET_MAG_CALIBRATION = 54, LOGIC_CONDITION_SET_GIMBAL_SENSITIVITY = 55, diff --git a/src/main/target/FLYDRAGONPRO/CMakeLists.txt b/src/main/target/FLYDRAGONPRO/CMakeLists.txt new file mode 100644 index 00000000000..33dbab6238c --- /dev/null +++ b/src/main/target/FLYDRAGONPRO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(FLYDRAGONPRO) diff --git a/src/main/target/FLYDRAGONPRO/README.md b/src/main/target/FLYDRAGONPRO/README.md new file mode 100644 index 00000000000..3fa344b7ff4 --- /dev/null +++ b/src/main/target/FLYDRAGONPRO/README.md @@ -0,0 +1,82 @@ +FlydragonRC Flydragon Pro +========================= + +Flight controllers originally designed for helicopters using Rotorflight. +Two variants exist, one with ICM42688P IMU and one with MPU6000. Both share the same target in INAV. + +Built-in periperals +------------------- + +Both models contain +- STM32F722RET6 MCU +- SPL06-001 barometer +- W25N01GVZEIG 1GBit Blackbox +- ExpressLRS receiver using ESP8285 and SX1280 connected to UART1. ELRS target is "FD R24D 2.4GHz RX" +- large button connected to BOOT0 +- large button is backlit by WS2812B RGB LED +- "GPS" expansion port with 5V, UART5, I2C and GND +- "DSM" expansion port with 3.3V, GND and UART3 RX +- "EXT" expansion port with BAT+, GND, BZZP and 5V. + +Buzzer should be connected between 5V and BZZP. +The BAT+ pin measures voltage up to 62V. When measuring the flight battery it is recommended to connect BAT+ but not GND to prevent unintended current paths. + +None of the external connections route to the receiver, they are all connected to the STM32F7 Flight Controller. +The receiver can be disabled using USER1, which controls a pinio on pin PA15. + +Pin configuration +----------------- + +The ESC, RPM, RX2 and TX2 pins are Servo/Motor outputs by default. However, when UART4 or UART2 are assigned a function in the ports tab, the pins will become a UART instead. See the table below. + +**The RPM pin features a filtering circuit that limits UART4 RX to 115200 baud. This means CRSF won't work on UART4, while slower protocols like SBUS will.** + +| Marking on the case | Both UART2 and UART4 unused | UART2 in use | UART4 in use | Both UART2 and UART4 in use | +|---------------------|-----------------------------|-------------------------|-------------------------|-----------------------------| +| TAIL | Output S1 | Output S1 | Output S1 | Output S1 | +| CH3 | Output S2 | Output S2 | Output S2 | Output S2 | +| CH2 | Output S3 | Output S3 | Output S3 | Output S3 | +| CH1 | Output S4 | Output S4 | Output S4 | Output S4 | +| ESC | Output S5 | Output S5 | UART4 TX | UART4 TX | +| RPM | Output S6 | Output S6 | UART4 RX | UART4 RX | +| RX2 | Output S7 | UART2 RX | Output S5 | UART2 RX | +| TX2 | Output S8 | UART2 TX | Output S6 | UART2 TX | +| AUX | Output S9 | Output S7 | Output S7 | Output S5 | +| GPS RX5 | UART5 RX | UART5 RX | UART5 RX | UART5 RX | +| GPS TX5 | UART5 TX | UART5 TX | UART5 TX | UART5 TX | +| GPS SCL | I2C SCL | I2C SCL | I2C SCL | I2C SCL | +| GPS SDA | I2C SDA | I2C SDA | I2C SDA | I2C SDA | +| DSM RX3 | UART3 RX | UART3 RX | UART3 RX | UART3 RX | +| EXT BAT+ | battery voltage max 62V | battery voltage max 62V | battery voltage max 62V | battery voltage max 62V | +| EXT BZZP | Buzzer positive pin | Buzzer positive pin | Buzzer positive pin | Buzzer positive pin | +| built-in ELRS | UART1 | UART1 | UART1 | UART1 | + + +Hardware layout +--------------- + + +| Marking on the case | STM32 pin | Servo | UART | I2C | +|---------------------|-----------|------------------------------------------:|---------------------:|-------------------------------------------:| +| TAIL | PC9 | TIM3CH4
TIM8CH4 | n/a | I2C3 SDA | +| CH3 | PC8 | TIM3CH3
TIM8CH3 | n/a | n/a | +| CH2 | PC7 | TIM3CH2
TIM8CH2 | n/a | n/a | +| CH1 | PC6 | TIM3CH1
TIM8CH1 | n/a | n/a | +| ESC | PA0 | TIM2CH1
TIM5CH1 | UART4 TX | n/a | +| RPM | PA1 | TIM2CH2
TIM5CH2 | UART4 RX | n/a | +| RX2 | PA3 | TIM2CH4
TIM5CH4
TIM9CH2 | UART2 RX | n/a | +| TX2 | PA2 | TIM2CH3
TIM5CH3
TIM9CH1 | UART2 TX | n/a | +| AUX | PB9 | TIM4CH4
TIM11CH1 | n/a | I2C1 SDA | +| GPS RX5 | PD2 | n/a | UART5 RX | n/a | +| GPS TX5 | PC12 | n/a | UART5 TX | n/a | +| GPS SCL | PB6 | TIM4CH1 | UART1 TX | I2C1 SCL | +| GPS SDA | PB7 | TIM4CH2 | UART1 RX | I2C1 SDA | +| DSM RX3 | PC11 | n/a | UART3 RX
UART4 RX | n/a | +| EXT BAT+ | PC0 | n/a | n/a | n/a | +| EXT BZZP | PA8 | TIM1CH1
pin is behind a FET for buzzer | n/a | I2C3 SCL
pin is behind a FET for buzzer | +| RGB LED | PB8 | TIM4CH1 | UART1 TX | I2C1 SCL | +| BUTTON | BOOT0 | n/a | n/a | n/a | +| built-in ELRS | PA9/PA10 | n/a | UART1 | n/a | + +PC1 is ADC for servo plug bank. +PC2 is ADC for the built-in BEC. \ No newline at end of file diff --git a/src/main/target/FLYDRAGONPRO/config.c b/src/main/target/FLYDRAGONPRO/config.c new file mode 100644 index 00000000000..b245dfea524 --- /dev/null +++ b/src/main/target/FLYDRAGONPRO/config.c @@ -0,0 +1,46 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include + +#include "platform.h" + +#include "drivers/pwm_mapping.h" + +#include "fc/fc_msp_box.h" + +#include "io/piniobox.h" +#include "io/serial.h" + +#include "io/ledstrip.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + + // configure the built-in WS2812B led as makeshift indicator led + ledStripConfig_t *config = ledStripConfigMutable(); + ledConfig_t *lc = config->ledConfigs; + DEFINE_LED(lc, 0, 0, COLOR_BLUE, 0, LED_FUNCTION_COLOR, LED_FLAG_OVERLAY(LED_OVERLAY_WARNING), 0); +} diff --git a/src/main/target/FLYDRAGONPRO/target.c b/src/main/target/FLYDRAGONPRO/target.c new file mode 100644 index 00000000000..39c405ebd20 --- /dev/null +++ b/src/main/target/FLYDRAGONPRO/target.c @@ -0,0 +1,45 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "target.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "TAIL" + + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "CH3" + DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "CH2" + DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "CH1" + + DEF_TIM(TIM2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "ESC", clashes with UART4 TX + DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "RPM", clashes with UART4 RX + + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "RX2", clashes with UART2 RX + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "TX2", clashes with UART2 TX + + DEF_TIM(TIM11, CH1, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "AUX" + + DEF_TIM(TIM4, CH3, PB8, TIM_USE_LED, 0, 0), // WS2812B +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FLYDRAGONPRO/target.h b/src/main/target/FLYDRAGONPRO/target.h new file mode 100644 index 00000000000..383446eaa10 --- /dev/null +++ b/src/main/target/FLYDRAGONPRO/target.h @@ -0,0 +1,164 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "RTFL" + +#define USBD_PRODUCT_STRING "FLYDRAGONPRO" + +// indicator led on this board is a single WS2812B LED +// no traditional indicator leds +#define LED0 NONE +#define LED1 NONE +#define USE_LED_STRIP +#define WS2811_PIN PB8 + +#define BEEPER PA8 +#define BEEPER_INVERTED + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +// ICM42605 variant +#define USE_IMU_ICM42605 // is actually ICM42688P +#define IMU_ICM42605_ALIGN CW0_DEG +#define ICM42605_CS_PIN PB0 +#define ICM42605_EXTI_PIN PB3 +#define ICM42605_SPI_BUS BUS_SPI1 + +// MPU6000 variant +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW90_DEG +#define MPU6000_CS_PIN PB0 +#define MPU6000_SPI_BUS BUS_SPI1 + + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 +#define DEFAULT_I2C BUS_I2C1 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_SPL06 +#define SPL06_I2C_ADDR 118 + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C + +#define PITOT_I2C_BUS DEFAULT_I2C + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS DEFAULT_I2C + +// *************** SPI2 Blackbox ******************* +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_FLASHFS +#define USE_FLASH_W25N01G +#define W25N01G_SPI_BUS BUS_SPI2 +#define W25N01G_CS_PIN PB12 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 // internal ELRS receiver +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 // pin labelled as "TX2" +#define UART2_RX_PIN PA3 // pin labelled as "RX2" + +#define USE_UART3 +#define UART3_TX_PIN NONE +#define UART3_RX_PIN PC11 // pin labelled "RX3" on the "DSM" port + +#define USE_UART4 +#define UART4_TX_PIN PA0 // pin labelled "ESC" +#define UART4_RX_PIN PA1 // pin labelled "RPM" + +#define USE_UART5 +#define UART5_TX_PIN PC12 // pin labelled "TX5" on the "GPS" port +#define UART5_RX_PIN PD2 // pin labelled "RX5" on the "GPS" port + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART1 +#define GPS_UART SERIAL_PORT_USART5 + +#define SENSORS_SET (SENSOR_ACC|SENSOR_BARO) + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC0 + +#define VBAT_ADC_CHANNEL ADC_CHN_3 // pin labelled "BAT+" on the "EXT" port +//BEC ADC is ADC_CHN_2 +//BUS ADC is ADC_CHN_1 + +#define VBAT_SCALE_DEFAULT 1898 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PA15 // enable pin for internal ELRS receiver +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED // turn on by default + +#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_LED_STRIP ) + +#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 MAX_PWM_OUTPUT_PORTS 9 + +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR +#define USE_SMARTPORT_MASTER // no internal current sensor, enable SMARTPORT_MASTER so external ones can be used + +#define USE_DSHOT_DMAR +#define TARGET_MOTOR_COUNT 8 // more than 8 DSHOT motors crashes the FC diff --git a/src/main/target/SIMPLIFLYH7/CMakeLists.txt b/src/main/target/SIMPLIFLYH7/CMakeLists.txt new file mode 100644 index 00000000000..a3b52aeaacd --- /dev/null +++ b/src/main/target/SIMPLIFLYH7/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(SIMPLIFLYH7) diff --git a/src/main/target/SIMPLIFLYH7/config.c b/src/main/target/SIMPLIFLYH7/config.c new file mode 100644 index 00000000000..04bae8c7e42 --- /dev/null +++ b/src/main/target/SIMPLIFLYH7/config.c @@ -0,0 +1,38 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + + +#include +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "io/serial.h" +#include "drivers/pwm_mapping.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].functionMask = FUNCTION_ESCSERIAL; + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + + timerOverridesMutable(timer2id(TIM8))->outputMode = OUTPUT_MODE_MOTORS; + timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_LED; +} diff --git a/src/main/target/SIMPLIFLYH7/target.c b/src/main/target/SIMPLIFLYH7/target.c new file mode 100644 index 00000000000..35cddd9243d --- /dev/null +++ b/src/main/target/SIMPLIFLYH7/target.c @@ -0,0 +1,49 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_ICM42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); + + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 DMA_NONE + + DEF_TIM(TIM3, CH2, PB5, TIM_USE_LED, 0, 9), // LED + DEF_TIM(TIM2, CH2, PB3, TIM_USE_ANY, 0, 0), // Camera Control +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SIMPLIFLYH7/target.h b/src/main/target/SIMPLIFLYH7/target.h new file mode 100644 index 00000000000..7b56c9e636b --- /dev/null +++ b/src/main/target/SIMPLIFLYH7/target.h @@ -0,0 +1,164 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SFH7" +#define USBD_PRODUCT_STRING "SIMPLIFLYH743" + +#define LED0 PD1 + +#define USE_BEEPER +#define BEEPER PD3 +#define BEEPER_INVERTED + +// *************** UART ***************************** +#define USB_IO +#define USE_VCP + + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PB2 +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART7 +#define UART7_RX_PIN PE7 +#define UART7_TX_PIN PE8 + +#define SERIAL_PORT_COUNT 6 + +// *************** Gyro & ACC ********************** +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG_FLIP +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PC4 + +// *************** I2C(Baro & I2C) ************************** +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// Baro +#define USE_BARO +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 +#define BARO_I2C_BUS BUS_I2C1 + +// Mag +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +// *************** Internal SD card ************************** + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PD6 + +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PD0 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** OSD ***************************** +#define USE_OSD +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC3 + + +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC0 +#define ADC_CHANNEL_3_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 +#define RSSI_ADC_CHANNEL ADC_CHN_2 + +// *************** LED ***************************** +#define USE_LED_STRIP +#define WS2811_PIN PB5 + +// ********** Optiical Flow adn Lidar ************** +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define SERIALRX_UART SERIAL_PORT_USART4 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define MAX_PWM_OUTPUT_PORTS 8 + +#define CURRENT_METER_SCALE 490 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff diff --git a/src/main/vcp_hal/usbd_cdc_interface.c b/src/main/vcp_hal/usbd_cdc_interface.c index 7ad16084ad9..6a819461adb 100644 --- a/src/main/vcp_hal/usbd_cdc_interface.c +++ b/src/main/vcp_hal/usbd_cdc_interface.c @@ -53,6 +53,7 @@ #include "usbd_cdc.h" #include "usbd_cdc_interface.h" #include "stdbool.h" +#include #include "drivers/time.h" #include "drivers/nvic.h" #include "build/atomic.h" @@ -61,6 +62,7 @@ /* Private define ------------------------------------------------------------*/ #define APP_RX_DATA_SIZE 4096 #define APP_TX_DATA_SIZE 4096 +#define USB_RX_STALL_THRESHOLD 128 // Must be >= max USB packet size (64) /* Private macro -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ @@ -72,17 +74,27 @@ USBD_CDC_LineCodingTypeDef LineCoding = 0x08 /* nb. of bits 8*/ }; -uint8_t UserRxBuffer[APP_RX_DATA_SIZE];/* Received Data over USB are stored in this buffer */ +/* + APP RX is the circular buffer for data that is received from the USB device (host) + to the APP (flight controller). +*/ +uint8_t AppRxBuffer[APP_RX_DATA_SIZE]; +volatile uint32_t AppRxPtrIn = 0; +/* Increment this buffer position or roll it back to + start address when writing received data + in the buffer APP_Rx_Buffer. */ +volatile uint32_t AppRxPtrOut = 0; +static volatile bool packetReceiveStalled; + +uint8_t UsbRxBuffer[64]; /* Small buffer for USB hardware to write into */ + uint8_t UserTxBuffer[APP_TX_DATA_SIZE];/* Received Data over UART (CDC interface) are stored in this buffer */ uint32_t BuffLength; uint32_t UserTxBufPtrIn = 0;/* Increment this pointer or roll it back to - start address when data are received over USART */ + start address when data are received over USART */ uint32_t UserTxBufPtrOut = 0; /* Increment this pointer or roll it back to start address when data are sent over USB */ -uint32_t rxAvailable = 0; -uint8_t* rxBuffPtr = NULL; - /* TIM handler declaration */ TIM_HandleTypeDef TimHandle; /* USB handler declaration */ @@ -146,10 +158,18 @@ static int8_t CDC_Itf_Init(void) /*##-5- Set Application Buffers ############################################*/ USBD_CDC_SetTxBuffer(&USBD_Device, UserTxBuffer, 0); - USBD_CDC_SetRxBuffer(&USBD_Device, UserRxBuffer); + USBD_CDC_SetRxBuffer(&USBD_Device, UsbRxBuffer); ctrlLineStateCb = NULL; baudRateCb = NULL; + + // Reset RX flow control state + AppRxPtrIn = 0; + AppRxPtrOut = 0; + packetReceiveStalled = false; + + // Prepare to receive first packet + USBD_CDC_ReceivePacket(&USBD_Device); return (USBD_OK); } @@ -286,20 +306,45 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) * @brief CDC_Itf_DataRx * Data received over USB OUT endpoint are sent over CDC interface * through this function. - * @param Buf: Buffer of data to be transmitted + * + * @note + * This function will block any OUT packet reception on USB endpoint + * until exiting this function. If you exit this function before transfer + * is complete on CDC interface (ie. using DMA controller) it will result + * in receiving more data while previous ones are still not sent. + * + * @param Buf: Buffer of data to be received * @param Len: Number of data received (in bytes) * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL */ static int8_t CDC_Itf_Receive(uint8_t* Buf, uint32_t *Len) { - rxAvailable = *Len; - rxBuffPtr = Buf; - if (!rxAvailable) { - // Received an empty packet, trigger receiving the next packet. - // This will happen after a packet that's exactly 64 bytes is received. - // The USB protocol requires that an empty (0 byte) packet immediately follow. + // Copy received data to ring buffer, handling wrap-around + uint32_t len = *Len; + uint32_t ptrIn = AppRxPtrIn; + uint32_t tailRoom = APP_RX_DATA_SIZE - ptrIn; + + if (len <= tailRoom) { + // Data fits without wrapping + memcpy(&AppRxBuffer[ptrIn], Buf, len); + ptrIn = (ptrIn + len) % APP_RX_DATA_SIZE; + } else { + // Data wraps around - copy in two parts + memcpy(&AppRxBuffer[ptrIn], Buf, tailRoom); + memcpy(&AppRxBuffer[0], Buf + tailRoom, len - tailRoom); + ptrIn = len - tailRoom; + } + AppRxPtrIn = ptrIn; + + // Check if we have room for another packet; if not, stall + uint32_t free = (APP_RX_DATA_SIZE + AppRxPtrOut - ptrIn - 1) % APP_RX_DATA_SIZE; + if (free <= USB_RX_STALL_THRESHOLD) { + packetReceiveStalled = true; + return USBD_FAIL; // Don't arm next receive + } else { USBD_CDC_ReceivePacket(&USBD_Device); } + return (USBD_OK); } @@ -373,27 +418,52 @@ static void Error_Handler(void) /* Add your own code here */ } +/******************************************************************************* + * Function Name : Receive DATA . + * Description : Copy received USB data from the ring buffer to the application buffer + * Input : None. + * Output : None. + * Return : None. + *******************************************************************************/ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) { - uint32_t count = 0; - if ( (rxBuffPtr != NULL)) - { - while ((rxAvailable > 0) && count < len) - { - recvBuf[count] = rxBuffPtr[0]; - rxBuffPtr++; - rxAvailable--; - count++; - if (rxAvailable < 1) - USBD_CDC_ReceivePacket(&USBD_Device); + uint32_t available = (AppRxPtrIn + APP_RX_DATA_SIZE - AppRxPtrOut) % APP_RX_DATA_SIZE; + uint32_t count = (len < available) ? len : available; + uint32_t ptrOut = AppRxPtrOut; + uint32_t tailRoom = APP_RX_DATA_SIZE - ptrOut; + + if (count == 0) { + return 0; + } + + if (count <= tailRoom) { + // Data is contiguous + memcpy(recvBuf, &AppRxBuffer[ptrOut], count); + ptrOut = (ptrOut + count) % APP_RX_DATA_SIZE; + } else { + // Data wraps around + memcpy(recvBuf, &AppRxBuffer[ptrOut], tailRoom); + memcpy(recvBuf + tailRoom, &AppRxBuffer[0], count - tailRoom); + ptrOut = count - tailRoom; + } + AppRxPtrOut = ptrOut; + + // If stalled, check if we have room to resume receiving + if (packetReceiveStalled) { + uint32_t free = (APP_RX_DATA_SIZE + AppRxPtrOut - AppRxPtrIn - 1) % APP_RX_DATA_SIZE; + if (free > USB_RX_STALL_THRESHOLD) { + packetReceiveStalled = false; + USBD_CDC_ReceivePacket(&USBD_Device); } } + return count; } uint32_t CDC_Receive_BytesAvailable(void) { - return rxAvailable; + /* return the bytes available in the receive circular buffer */ + return (AppRxPtrIn + APP_RX_DATA_SIZE - AppRxPtrOut) % APP_RX_DATA_SIZE; } uint32_t CDC_Send_FreeBytes(void) @@ -484,6 +554,30 @@ uint32_t CDC_BaudRate(void) return LineCoding.bitrate; } +/******************************************************************************* + * Function Name : CDC_StopBits. + * Description : Get the current stop bits setting + * Input : None. + * Output : None. + * Return : Stop bits (0=1, 1=1.5, 2=2) + *******************************************************************************/ +uint8_t CDC_StopBits(void) +{ + return LineCoding.format; +} + +/******************************************************************************* + * Function Name : CDC_Parity. + * Description : Get the current parity setting + * Input : None. + * Output : None. + * Return : Parity (0=none, 1=odd, 2=even, 3=mark, 4=space) + *******************************************************************************/ +uint8_t CDC_Parity(void) +{ + return LineCoding.paritytype; +} + /******************************************************************************* * Function Name : CDC_SetBaudRateCb * Description : Set a callback to call when baud rate changes diff --git a/src/main/vcp_hal/usbd_cdc_interface.h b/src/main/vcp_hal/usbd_cdc_interface.h index c6025e68841..bd521eb32bf 100644 --- a/src/main/vcp_hal/usbd_cdc_interface.h +++ b/src/main/vcp_hal/usbd_cdc_interface.h @@ -86,6 +86,8 @@ uint32_t CDC_Receive_BytesAvailable(void); uint8_t usbIsConfigured(void); uint8_t usbIsConnected(void); uint32_t CDC_BaudRate(void); +uint8_t CDC_StopBits(void); +uint8_t CDC_Parity(void); void CDC_SetCtrlLineStateCb(void (*cb)(void *context, uint16_t ctrlLineState), void *context); void CDC_SetBaudRateCb(void (*cb)(void *context, uint32_t baud), void *context); diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 7511f617eb4..dd023bdd7e0 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -21,8 +21,10 @@ /* Includes ------------------------------------------------------------------*/ #include "usbd_cdc_vcp.h" +#include "usbd_cdc_core.h" #include "stm32f4xx_conf.h" #include +#include #include "drivers/time.h" #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED @@ -45,14 +47,18 @@ extern volatile uint32_t APP_Rx_ptr_out; start address when writing received data in the buffer APP_Rx_Buffer. */ extern volatile uint32_t APP_Rx_ptr_in; +static volatile bool packetReceiveStalled; +#define USB_RX_STALL_THRESHOLD 128 // Must be >= max USB packet size (64) + +// runtime-configurable tx poll interval (default matches CDC_IN_FRAME_INTERVAL) /* APP TX is the circular buffer for data that is transmitted from the APP (host) to the USB device (flight controller). */ static uint8_t APP_Tx_Buffer[APP_TX_DATA_SIZE]; -static uint32_t APP_Tx_ptr_out = 0; -static uint32_t APP_Tx_ptr_in = 0; +static volatile uint32_t APP_Tx_ptr_out = 0; +static volatile uint32_t APP_Tx_ptr_in = 0; /* Private function prototypes -----------------------------------------------*/ static uint16_t VCP_Init(void); @@ -80,6 +86,12 @@ static uint16_t VCP_Init(void) bDeviceState = CONFIGURED; ctrlLineStateCb = NULL; baudRateCb = NULL; + + // Reset RX flow control state + APP_Tx_ptr_in = 0; + APP_Tx_ptr_out = 0; + packetReceiveStalled = false; + return USBD_OK; } @@ -240,7 +252,7 @@ static uint16_t VCP_DataTx(const uint8_t* Buf, uint32_t Len) /******************************************************************************* * Function Name : Receive DATA . - * Description : receive the data from the PC to STM32 and send it through USB + * Description : Copy received USB data from the ring buffer to the application buffer * Input : None. * Output : None. * Return : None. @@ -249,11 +261,22 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) { uint32_t count = 0; + // Drain the ring buffer into the user buffer while (APP_Tx_ptr_out != APP_Tx_ptr_in && (count < len)) { recvBuf[count] = APP_Tx_Buffer[APP_Tx_ptr_out]; APP_Tx_ptr_out = (APP_Tx_ptr_out + 1) % APP_TX_DATA_SIZE; count++; } + + // If stalled, check if we have room to resume receiving + if (packetReceiveStalled) { + uint32_t free = (APP_TX_DATA_SIZE + APP_Tx_ptr_out - APP_Tx_ptr_in - 1) % APP_TX_DATA_SIZE; + if (free > USB_RX_STALL_THRESHOLD) { + packetReceiveStalled = false; + USBD_CDC_ReceivePacket(&USB_OTG_dev); + } + } + return count; } @@ -280,13 +303,27 @@ uint32_t CDC_Receive_BytesAvailable(void) */ static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) { - if (CDC_Receive_BytesAvailable() + Len > APP_TX_DATA_SIZE) { - return USBD_FAIL; + // Copy received data to ring buffer, handling wrap-around + uint32_t ptrIn = APP_Tx_ptr_in; + uint32_t tailRoom = APP_TX_DATA_SIZE - ptrIn; + + if (Len <= tailRoom) { + // Data fits without wrapping + memcpy(&APP_Tx_Buffer[ptrIn], Buf, Len); + ptrIn = (ptrIn + Len) % APP_TX_DATA_SIZE; + } else { + // Data wraps around - copy in two parts + memcpy(&APP_Tx_Buffer[ptrIn], Buf, tailRoom); + memcpy(&APP_Tx_Buffer[0], Buf + tailRoom, Len - tailRoom); + ptrIn = Len - tailRoom; } + APP_Tx_ptr_in = ptrIn; - for (uint32_t i = 0; i < Len; i++) { - APP_Tx_Buffer[APP_Tx_ptr_in] = Buf[i]; - APP_Tx_ptr_in = (APP_Tx_ptr_in + 1) % APP_TX_DATA_SIZE; + // Check if we have room for another packet; if not, stall + uint32_t free = (APP_TX_DATA_SIZE + APP_Tx_ptr_out - ptrIn - 1) % APP_TX_DATA_SIZE; + if (free <= USB_RX_STALL_THRESHOLD) { + packetReceiveStalled = true; + return USBD_FAIL; // Don't arm next receive } return USBD_OK; @@ -328,6 +365,30 @@ uint32_t CDC_BaudRate(void) return g_lc.bitrate; } +/******************************************************************************* + * Function Name : CDC_StopBits. + * Description : Get the current stop bits setting + * Input : None. + * Output : None. + * Return : Stop bits (0=1, 1=1.5, 2=2) + *******************************************************************************/ +uint8_t CDC_StopBits(void) +{ + return g_lc.format; +} + +/******************************************************************************* + * Function Name : CDC_Parity. + * Description : Get the current parity setting + * Input : None. + * Output : None. + * Return : Parity (0=none, 1=odd, 2=even, 3=mark, 4=space) + *******************************************************************************/ +uint8_t CDC_Parity(void) +{ + return g_lc.paritytype; +} + /******************************************************************************* * Function Name : CDC_SetBaudRateCb * Description : Set a callback to call when baud rate changes diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index fc27a6d98ee..fcd37639771 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -44,6 +44,8 @@ uint32_t CDC_Receive_BytesAvailable(void); uint8_t usbIsConfigured(void); // HJI uint8_t usbIsConnected(void); // HJI uint32_t CDC_BaudRate(void); +uint8_t CDC_StopBits(void); +uint8_t CDC_Parity(void); void CDC_SetCtrlLineStateCb(void (*cb)(void *context, uint16_t ctrlLineState), void *context); void CDC_SetBaudRateCb(void (*cb)(void *context, uint32_t baud), void *context);