Skip to content

Commit

Permalink
Added F.Port Channel Decoding
Browse files Browse the repository at this point in the history
  • Loading branch information
Witty-Wizard committed Jun 30, 2024
1 parent 331d488 commit d43bc71
Show file tree
Hide file tree
Showing 11 changed files with 231 additions and 122 deletions.
23 changes: 23 additions & 0 deletions Docs/fport.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
# FrSky F.Port {#frskyfport}

FrSky F.Port is a protocol developed by FrSky Electronic Co., Ltd., designed for communication between receivers and connected devices like servos or sensors. It is a one-line bus system that supports both control and data transmission, operating at a higher speed (115200 baud per second) compared to its predecessor, S.Port.

## Specifications

- [Physical Layer](#physical_fport)
- [Message Format](#format_fport)

### Physical Layer {#physical_fport}

The physical layer of the FrSky F.Port protocol utilizes UART (Universal Asynchronous Receiver-Transmitter) communication at a baud rate of 115200.

#### Uart Configuration

When it comes to the configuration of the UART communication for SBus, it uses the following format:

- **8 bits of data**: Each data frame consists of 8 bits, representing the information being transmitted.
- **No parity**: It does not use parity bit instead it uses a checksum at the end of the whole packet.
- **1 stop bits**: Following the data bits and the parity bit, there are 1 stop bits. Stop bits indicate the end of a data frame and provide timing for the receiving device to process the data.

### Message Format {#format_fport}
For the data format and more details go to [this link](https://github.com/betaflight/betaflight/files/1491056/F.Port.protocol.betaFlight.V2.1.2017.11.21.pdf)
87 changes: 45 additions & 42 deletions Docs/index.md
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,13 @@ SerialIO is a common library designed to simplify the implementation of RC proto

# Supported Protocol

- [Futaba SBus protocol](#sbus)
- [Futaba SBus protocol](#futabasbus)
- [Crossfire RC protocol](https://github.com/crsf-wg/crsf/wiki)
- [Flysky IBus protocol](https://basejunction.wordpress.com/2015/08/23/en-flysky-i6-14-channels-part1/)
- [FrSky F.Port](#frskyfport)

# Getting Started

- [Installation](#installation)
- [Tutorial](#tutorial)
- [Examples](#example)
Expand Down Expand Up @@ -36,64 +38,64 @@ lib_deps = Witty-Wizard/SerialIO

To use the library for decoding RC protocols in your Arduino project, follow these steps:

1. **Include Necessary Libraries**:
Include the required libraries at the beginning of your sketch:
```cpp
#include <SerialIO.h>
```
2. **Define Channel Data Structure**:
Define a structure to hold the decoded RC channel data.
```cpp
crsf_channels_t channelData;
```
3. **Instantiate SerialIO Object**:
Create an instance of the SerialIO class, specifying the serial port, RX pin, and TX pin:
```cpp
SerialIO *receiver = new crsf(&Serial1, pinRX, pinTX);
```
To instantiate a SerialIO object for receiving data only, you can create an instance of the crsf class specifying the serial port, RX pin:
```cpp
SerialIO *receiver = new crsf(&Serial1, pinRX);
```
4. **Initialize Communication**:
Call the begin() method to initialize communication with the specified serial port:
```cpp
void setup() {
receiver->begin();
}
```
5. **Process Incoming Data**:
In the loop() function, call the processIncoming() method to process incoming bytes:
1. **Include Necessary Libraries**:
Include the required libraries at the beginning of your sketch:
```cpp
#include <SerialIO.h>
```
2. **Define Channel Data Structure**:
Define a structure to hold the decoded RC channel data.
```cpp
crsf_channels_t channelData;
```
3. **Instantiate SerialIO Object**:
Create an instance of the SerialIO class, specifying the serial port, RX pin, and TX pin:
```cpp
SerialIO *receiver = new crsf(&Serial1, pinRX, pinTX);
```
To instantiate a SerialIO object for receiving data only, you can create an instance of the crsf class specifying the serial port, RX pin:
```cpp
receiver->processIncoming();
SerialIO *receiver = new crsf(&Serial1, pinRX);
```
6. **Retrieve Channel Data**:
To retrieve the decoded RC channel data, call the getChannel() method, passing a pointer to the channelData structure:
4. **Initialize Communication**:
Call the begin() method to initialize communication with the specified serial port:

```cpp
receiver->getChannel(&channelData);
void setup() {
receiver->begin();
}
```
**see also**:
- @ref SerialIO
- @ref sbus
- @ref crsf
- @ref ibus

5. **Process Incoming Data**:
In the loop() function, call the processIncoming() method to process incoming bytes:

```cpp
receiver->processIncoming();
```

6. **Retrieve Channel Data**:
To retrieve the decoded RC channel data, call the getChannel() method, passing a pointer to the channelData structure:

```cpp
receiver->getChannel(&channelData);
```

**see also**: - @ref SerialIO - @ref sbus - @ref crsf - @ref ibus

## Examples {#example}

### CRSF Basic Example

@include "./examples/espresiff/crsf_basic/crsf_basic.ino"

### SBUS Basic Example
@include "./examples/espresiff/sbus_basic/sbus_basic.ino"


@include "./examples/espresiff/sbus_basic/sbus_basic.ino"

# Guide to Adding More Protocols

## Steps

To add more protocols to your project, follow these steps:

1. **Create a New Protocol File** : Start by creating a new header file for each additional protocol you want to add. For example, if you want to add a protocol named "XYZ", create a file named `xyz_protocol.h`.
Expand Down Expand Up @@ -151,4 +153,5 @@ private:
```

# License

This library is distributed under the GNU [General Public License version 3.0](https://www.gnu.org/licenses/gpl-3.0.html).
8 changes: 4 additions & 4 deletions Docs/sbus.md
Original file line number Diff line number Diff line change
@@ -1,14 +1,14 @@
# Futaba SBus {#sbus}
# Futaba SBus {#futabasbus}

The Futaba S.Bus protocol was introduced around 2009. It was developed by Futaba, a leading manufacturer of RC systems, to address the limitations of traditional PWM (Pulse Width Modulation) systems used for RC control.Unlike PWM, which requires separate wires for each channel, S.Bus uses a single serial connection to transmit data for multiple channels, reducing the complexity of wiring in RC models.
\image html sbus.png width=700cm

## Specifications

- [Physical Layer](#physical)
- [Physical Layer](#physical_sbus)
- [Message Format](#format)

### Physical Layer {#physical}
### Physical Layer {#physical_sbus}

The physical layer of the Futaba SBus protocol utilizes UART (Universal Asynchronous Receiver-Transmitter) communication at a baud rate of 100000.

Expand All @@ -25,7 +25,7 @@ When it comes to the configuration of the UART communication for SBus, it uses t
For using SBus with a microcontroller, an inverter is typically required for the inverted UART logic level. This inversion is necessary because traditional UART operates with an active high level, while SBus uses inverted UART with an active low level.
\image html inverter.png width=600cm

### Message Format {#format}
### Message Format {#format_sbus}
\image html formatsbus.png

The SBus protocol uses a specific message format for transmitting control data from the transmitter to the receiver.
Expand Down
19 changes: 8 additions & 11 deletions src/SerialIO.h
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,10 @@
#pragma once
#ifndef SerialIO_H
#define SerialIO_H
#include "crsf/crsf_protocol.h"
#include <Arduino.h>

#define PACKED __attribute__((packed))

/**************************************************************************/
/*!
@brief Class that stores state and functions for initialising and decoding
Expand Down Expand Up @@ -52,18 +53,14 @@ class SerialIO {

protected:
Stream
*_rxPort; // Pointer to the hardware serial port used for communication.
bool _headerDetected; // Flag indicating whether a header has been detected
// in the incoming data.
bool _inverted; // Indicates whether the serial signal is inverted (true) or
// not (false).
uint8_t _rxIndex; // Index for the receive_buffer.
int _rxPin; // The RX pin number.
int _txPin; // The TX pin number.
uint8_t _buffer;
uint8_t _prevBuffer;
*_rxPort; // Pointer to the hardware serial port used for communication.
bool _inverted; // Indicates whether the serial signal is inverted (true) or
// not (false).
int _rxPin; // The RX pin number.
int _txPin; // The TX pin number.
};
#include "crsf/crsf.h"
#include "fport/fport.h"
#include "ibus/ibus.h"
#include "sbus/sbus.h"

Expand Down
6 changes: 6 additions & 0 deletions src/crsf/crsf.h
Original file line number Diff line number Diff line change
@@ -1,13 +1,15 @@
/*!
* @file crsf.h
* @brief Header file for the CRSF protocol implementation.
* @author Witty-Wizard
*/

#pragma once
#ifndef CRSF_H
#define CRSF_H

#include "../SerialIO.h" // Include header file for the serial IO class
#include "crsf_protocol.h"

#define CRC8_POLY_D5 0xD5

Expand All @@ -18,6 +20,10 @@ class crsf : public SerialIO {
private:
crsf_channels_t channelData;
uint8_t _rxData[CRSF_MAX_PACKET_SIZE];
bool _headerDetected; // Flag indicating whether a header has been detected in
// the incoming data.
uint8_t _rxIndex; // Index for the receive_buffer.
uint8_t _buffer;

public:
/**
Expand Down
44 changes: 44 additions & 0 deletions src/fport/fport.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
/*!
* @file fport.cpp
* @brief Source file for the F.Port implementations
* @author Witty-Wizard
*/

#include "fport.h"

fport::fport(Stream *rxPort, int rxPin, int txPin, bool inverted)
: SerialIO(rxPort, rxPin, txPin, inverted) {}

void fport::begin() {

// Initialize the serial port
#if defined(ARDUINO_ARCH_ESP32)
HardwareSerial *serialPort = (HardwareSerial *)_rxPort;
serialPort->begin(FPORT_BAUDRATE, SERIAL_8N1, _rxPin, _txPin, _inverted);
#elif defined(ARDUINO_ARCH_RP2040)
SerialUART *serialPort = (SerialUART *)_rxPort;
serialPort->setPinout(_txPin, _rxPin);
serialPort->begin(FPORT_BAUDRATE, SERIAL_8N1);
#else
#warning "Unsupported hardware platform."
#endif
}

void fport::processIncoming() {
while (_rxPort->available()) {
_rx_buffer[FPORT_MAX_PACKET_SIZE - 1] = _rxPort->read();
if (_rx_buffer[0] == 0x7E &&
_rx_buffer[FPORT_MAX_PACKET_SIZE - 1] == 0x7E) {
memcpy(&_channelData, &_rx_buffer[4], sizeof(_channelData));
} else {
leftShift(_rx_buffer, sizeof(_rx_buffer));
}
}
}

void fport::getChannel(void *channelData) {}

void fport::leftShift(uint8_t arr[], size_t size) {
memmove(arr, arr + 1, (size - 1));
arr[size - 1] = 0xFF;
}
48 changes: 48 additions & 0 deletions src/fport/fport.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
/*!
* @file fport.h
* @brief Header file for the FPort Protocol definations
* @author Witty-Wizard
*/

#pragma once
#ifndef FPORT_H
#define FPORT_H

#include "../SerialIO.h"

#define FPORT_BAUDRATE 115200 ///< F.Port baudrate
#define FPORT_MAX_PACKET_SIZE 29 ///< F.Port maximum packet length

typedef struct fport_channels_s {
unsigned channel1 : 11;
unsigned channel2 : 11;
unsigned channel3 : 11;
unsigned channel4 : 11;
unsigned channel5 : 11;
unsigned channel6 : 11;
unsigned channel7 : 11;
unsigned channel8 : 11;
unsigned channel9 : 11;
unsigned channel10 : 11;
unsigned channel11 : 11;
unsigned channel12 : 11;
unsigned channel13 : 11;
unsigned channel14 : 11;
unsigned channel15 : 11;
unsigned channel16 : 11;
} PACKED fport_channels_t;

class fport : public SerialIO {
private:
uint8_t _rx_buffer[FPORT_MAX_PACKET_SIZE];
fport_channels_t _channelData;

public:
explicit fport(Stream *rxPort, int rxPin = -1, int txPin = -1,
bool inverted = true);
void begin() override;
void processIncoming() override;
void getChannel(void *channelData) override;
void leftShift(uint8_t arr[],size_t size);
};
#endif
Loading

0 comments on commit d43bc71

Please sign in to comment.