ELRS.c

#include <stdio.h>
#include <string.h>
#include "pico/stdlib.h"
#include "hardware/uart.h"

// CRSF Protocol defines
#define CRSF_BAUDRATE 420000
#define CRSF_FRAME_SIZE_MAX 64
#define CRSF_PAYLOAD_SIZE_MAX 60

// CRSF frame addresses
#define CRSF_ADDRESS_BROADCAST 0x00
#define CRSF_ADDRESS_FLIGHT_CONTROLLER 0xC8
#define CRSF_ADDRESS_CRSF_RECEIVER 0xEC

// CRSF frame types
#define CRSF_FRAMETYPE_RC_CHANNELS_PACKED 0x16
#define CRSF_FRAMETYPE_LINK_STATISTICS 0x14
#define CRSF_FRAMETYPE_BATTERY_SENSOR 0x08

// RC Channels (11 bit per channel, 16 channels)
#define CRSF_CHANNEL_VALUE_MIN 172
#define CRSF_CHANNEL_VALUE_MID 992
#define CRSF_CHANNEL_VALUE_MAX 1811

// UART Configuration
#define UART_ID uart0
#define UART_TX_PIN 0
#define UART_RX_PIN 1

typedef struct {
    uint8_t address;
    uint8_t length;
    uint8_t type;
    uint8_t payload[CRSF_PAYLOAD_SIZE_MAX];
    uint8_t crc;
} crsf_frame_t;

typedef struct {
    uint16_t channels[16];
} crsf_channels_t;

// CRC8 calculation for CRSF
uint8_t crsf_crc8(const uint8_t *data, uint8_t len) {
    uint8_t crc = 0;
    for (uint8_t i = 0; i < len; i++) {
        crc ^= data[i];
        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 0x80) {
                crc = (crc << 1) ^ 0xD5;
            } else {
                crc = crc << 1;
            }
        }
    }
    return crc;
}

// Parse RC channels from CRSF frame
void parse_rc_channels(const uint8_t *payload, crsf_channels_t *channels) {
    // CRSF uses 11 bits per channel, packed
    channels->channels[0]  = (uint16_t)((payload[0] | payload[1] << 8) & 0x07FF);
    channels->channels[1]  = (uint16_t)((payload[1] >> 3 | payload[2] << 5) & 0x07FF);
    channels->channels[2]  = (uint16_t)((payload[2] >> 6 | payload[3] << 2 | payload[4] << 10) & 0x07FF);
    channels->channels[3]  = (uint16_t)((payload[4] >> 1 | payload[5] << 7) & 0x07FF);
    channels->channels[4]  = (uint16_t)((payload[5] >> 4 | payload[6] << 4) & 0x07FF);
    channels->channels[5]  = (uint16_t)((payload[6] >> 7 | payload[7] << 1 | payload[8] << 9) & 0x07FF);
    channels->channels[6]  = (uint16_t)((payload[8] >> 2 | payload[9] << 6) & 0x07FF);
    channels->channels[7]  = (uint16_t)((payload[9] >> 5 | payload[10] << 3) & 0x07FF);
    channels->channels[8]  = (uint16_t)((payload[11] | payload[12] << 8) & 0x07FF);
    channels->channels[9]  = (uint16_t)((payload[12] >> 3 | payload[13] << 5) & 0x07FF);
    channels->channels[10] = (uint16_t)((payload[13] >> 6 | payload[14] << 2 | payload[15] << 10) & 0x07FF);
    channels->channels[11] = (uint16_t)((payload[15] >> 1 | payload[16] << 7) & 0x07FF);
    channels->channels[12] = (uint16_t)((payload[16] >> 4 | payload[17] << 4) & 0x07FF);
    channels->channels[13] = (uint16_t)((payload[17] >> 7 | payload[18] << 1 | payload[19] << 9) & 0x07FF);
    channels->channels[14] = (uint16_t)((payload[19] >> 2 | payload[20] << 6) & 0x07FF);
    channels->channels[15] = (uint16_t)((payload[20] >> 5 | payload[21] << 3) & 0x07FF);
}

// Process received CRSF frame
void process_crsf_frame(crsf_frame_t *frame) {
    // Verify CRC
    uint8_t calculated_crc = crsf_crc8(&frame->type, frame->length - 1);
    
    if (calculated_crc != frame->crc) {
        printf("CRC Error! Expected: 0x%02X, Got: 0x%02X\n", calculated_crc, frame->crc);
        return;
    }
    
    switch (frame->type) {
        case CRSF_FRAMETYPE_RC_CHANNELS_PACKED: {
            crsf_channels_t channels;
            parse_rc_channels(frame->payload, &channels);
            
            // Map values to percentage (172-1811 -> 0-100%)
            int ch1_percent = (channels.channels[0] - CRSF_CHANNEL_VALUE_MIN) * 100 / (CRSF_CHANNEL_VALUE_MAX - CRSF_CHANNEL_VALUE_MIN);
            int ch3_percent = (channels.channels[2] - CRSF_CHANNEL_VALUE_MIN) * 100 / (CRSF_CHANNEL_VALUE_MAX - CRSF_CHANNEL_VALUE_MIN);
            
            printf("CH1: %d%%  CH3: %d%%\n", ch1_percent, ch3_percent);
            break;
        }
        
        case CRSF_FRAMETYPE_LINK_STATISTICS:
            // printf("Link Stats - RSSI: %d dBm, LQ: %d%%, SNR: %d dB\n", 
            //        -(int8_t)frame->payload[0],
            //        frame->payload[1],
            //        (int8_t)frame->payload[2]);
            break;
            
        default:
            printf("Frame Type: 0x%02X (Length: %d)\n", frame->type, frame->length);
            break;
    }
}

int main() {
    stdio_init_all();
    
    // Initialize UART for CRSF communication
    uart_init(UART_ID, CRSF_BAUDRATE);
    gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
    gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);
    
    // Set UART format: 8N1
    uart_set_format(UART_ID, 8, 1, UART_PARITY_NONE);
    
    // Enable UART FIFO
    uart_set_fifo_enabled(UART_ID, true);
    
    printf("ELRS/CRSF Reader Started\n");
    printf("Waiting for ER8GV receiver data...\n");
    
    uint8_t buffer[CRSF_FRAME_SIZE_MAX];
    uint8_t buffer_index = 0;
    bool frame_started = false;
    crsf_frame_t current_frame;
    
    while (true) {
        // Check if data is available
        if (uart_is_readable(UART_ID)) {
            uint8_t byte = uart_getc(UART_ID);
            
            if (!frame_started) {
                // Look for valid address byte
                if (byte == CRSF_ADDRESS_FLIGHT_CONTROLLER || 
                    byte == CRSF_ADDRESS_BROADCAST ||
                    byte == CRSF_ADDRESS_CRSF_RECEIVER) {
                    current_frame.address = byte;
                    frame_started = true;
                    buffer_index = 0;
                }
            } else {
                if (buffer_index == 0) {
                    // Length byte
                    current_frame.length = byte;
                    if (current_frame.length > CRSF_PAYLOAD_SIZE_MAX + 2) {
                        // Invalid length, reset
                        frame_started = false;
                        continue;
                    }
                } else if (buffer_index == 1) {
                    // Type byte
                    current_frame.type = byte;
                } else if (buffer_index < current_frame.length) {
                    // Payload bytes
                    current_frame.payload[buffer_index - 2] = byte;
                } else if (buffer_index == current_frame.length) {
                    // CRC byte
                    current_frame.crc = byte;
                    
                    // Process complete frame
                    process_crsf_frame(&current_frame);
                    
                    // Reset for next frame
                    frame_started = false;
                    buffer_index = 0;
                    continue;
                }
                buffer_index++;
            }
        }
        
        // Small delay to prevent busy waiting
        sleep_us(100);
    }
    
    return 0;
}