395 lines
11 KiB
C
395 lines
11 KiB
C
/**
|
|
* @file rcsignal.c
|
|
* @brief RC PWM signal reading implementation using edge capture
|
|
*/
|
|
|
|
#include "rcsignal.h"
|
|
|
|
#include "driver/gpio.h"
|
|
#include "esp_timer.h"
|
|
#include "esp_log.h"
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
|
|
#ifdef USE_SBUS_MODE
|
|
#include "driver/uart.h"
|
|
#endif
|
|
|
|
#include <string.h>
|
|
|
|
static const char *TAG = "RCSIGNAL";
|
|
|
|
#ifdef USE_SBUS_MODE
|
|
// SBUS protocol constants
|
|
#define SBUS_FRAME_SIZE 25
|
|
#define SBUS_HEADER 0x0F
|
|
#define SBUS_FOOTER 0x00
|
|
#define SBUS_FOOTER2 0x04 // Alternative footer
|
|
#define SBUS_BAUDRATE 100000
|
|
#define SBUS_CH_MIN 172
|
|
#define SBUS_CH_CENTER 992
|
|
#define SBUS_CH_MAX 1811
|
|
#define SBUS_THRESHOLD_LOW 800
|
|
#define SBUS_THRESHOLD_HIGH 1100
|
|
#define UART_NUM UART_NUM_1
|
|
#define UART_BUF_SIZE 256
|
|
#else
|
|
// PWM mode constants
|
|
#define PULSE_THRESHOLD_US 1500
|
|
#define SIGNAL_TIMEOUT_MS 100
|
|
#endif
|
|
static struct
|
|
{
|
|
int8_t gpio_pin;
|
|
#ifdef USE_SBUS_MODE
|
|
volatile uint16_t channels[SBUS_NUM_CHANNELS];
|
|
volatile int64_t last_frame_time;
|
|
volatile bool signal_active;
|
|
volatile bool pull_detected;
|
|
uint8_t rx_buffer[SBUS_FRAME_SIZE];
|
|
#else
|
|
volatile uint32_t pulse_width_us;
|
|
volatile int64_t last_edge_time;
|
|
volatile int64_t pulse_start_time;
|
|
volatile bool last_level;
|
|
volatile bool signal_active;
|
|
volatile bool pull_detected;
|
|
#endif
|
|
uint8_t current_mode;
|
|
rcsignal_mode_change_callback_t callback;
|
|
bool initialized;
|
|
TaskHandle_t monitor_task;
|
|
} rcsignal = {
|
|
.gpio_pin = -1,
|
|
#ifdef USE_SBUS_MODE
|
|
.channels = {0},
|
|
.last_frame_time = 0,
|
|
.signal_active = false,
|
|
.pull_detected = false,
|
|
.rx_buffer = {0},
|
|
#else
|
|
.pulse_width_us = 0,
|
|
.last_edge_time = 0,
|
|
.pulse_start_time = 0,
|
|
.last_level = false,
|
|
.signal_active = false,
|
|
.pull_detected = false,
|
|
#endif
|
|
.current_mode = 0,
|
|
.callback = NULL,
|
|
.initialized = false,
|
|
.monitor_task = NULL,
|
|
};
|
|
|
|
#ifndef USE_SBUS_MODE
|
|
// PWM Mode: GPIO ISR handler
|
|
static void IRAM_ATTR gpio_isr_handler(void *arg)
|
|
{
|
|
int64_t now = esp_timer_get_time();
|
|
bool level = gpio_get_level(rcsignal.gpio_pin);
|
|
|
|
if (level && !rcsignal.last_level)
|
|
{
|
|
// Rising edge - start of pulse
|
|
rcsignal.pulse_start_time = now;
|
|
}
|
|
else if (!level && rcsignal.last_level)
|
|
{
|
|
// Falling edge - end of pulse
|
|
if (rcsignal.pulse_start_time > 0)
|
|
{
|
|
rcsignal.pulse_width_us = (uint32_t)(now - rcsignal.pulse_start_time);
|
|
rcsignal.last_edge_time = now;
|
|
rcsignal.signal_active = true;
|
|
}
|
|
}
|
|
|
|
rcsignal.last_level = level;
|
|
}
|
|
#else
|
|
// SBUS Mode: Parse SBUS frame
|
|
static bool parse_sbus_frame(const uint8_t *frame, uint16_t *channels)
|
|
{
|
|
// Check header and footer
|
|
if (frame[0] != SBUS_HEADER || (frame[24] != SBUS_FOOTER && frame[24] != SBUS_FOOTER2))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// SBUS uses 11 bits per channel, packed into bytes
|
|
channels[0] = ((frame[1] | frame[2] << 8) & 0x07FF);
|
|
channels[1] = ((frame[2] >> 3 | frame[3] << 5) & 0x07FF);
|
|
channels[2] = ((frame[3] >> 6 | frame[4] << 2 | frame[5] << 10) & 0x07FF);
|
|
channels[3] = ((frame[5] >> 1 | frame[6] << 7) & 0x07FF);
|
|
channels[4] = ((frame[6] >> 4 | frame[7] << 4) & 0x07FF);
|
|
channels[5] = ((frame[7] >> 7 | frame[8] << 1 | frame[9] << 9) & 0x07FF);
|
|
channels[6] = ((frame[9] >> 2 | frame[10] << 6) & 0x07FF);
|
|
channels[7] = ((frame[10] >> 5 | frame[11] << 3) & 0x07FF);
|
|
channels[8] = ((frame[12] | frame[13] << 8) & 0x07FF);
|
|
channels[9] = ((frame[13] >> 3 | frame[14] << 5) & 0x07FF);
|
|
channels[10] = ((frame[14] >> 6 | frame[15] << 2 | frame[16] << 10) & 0x07FF);
|
|
channels[11] = ((frame[16] >> 1 | frame[17] << 7) & 0x07FF);
|
|
channels[12] = ((frame[17] >> 4 | frame[18] << 4) & 0x07FF);
|
|
channels[13] = ((frame[18] >> 7 | frame[19] << 1 | frame[20] << 9) & 0x07FF);
|
|
channels[14] = ((frame[20] >> 2 | frame[21] << 6) & 0x07FF);
|
|
channels[15] = ((frame[21] >> 5 | frame[22] << 3) & 0x07FF);
|
|
|
|
return true;
|
|
}
|
|
#endif
|
|
|
|
static void monitor_task(void *arg)
|
|
{
|
|
#ifdef USE_SBUS_MODE
|
|
while (1)
|
|
{
|
|
// Read SBUS data from UART
|
|
int len = uart_read_bytes(UART_NUM, rcsignal.rx_buffer, SBUS_FRAME_SIZE, pdMS_TO_TICKS(20));
|
|
|
|
if (len == SBUS_FRAME_SIZE)
|
|
{
|
|
uint16_t temp_channels[SBUS_NUM_CHANNELS];
|
|
|
|
if (parse_sbus_frame(rcsignal.rx_buffer, temp_channels))
|
|
{
|
|
// Copy parsed channels
|
|
for (int i = 0; i < SBUS_NUM_CHANNELS; i++)
|
|
{
|
|
rcsignal.channels[i] = temp_channels[i];
|
|
}
|
|
|
|
rcsignal.last_frame_time = esp_timer_get_time();
|
|
rcsignal.signal_active = true;
|
|
|
|
// Check channel 4 for mode trigger
|
|
uint16_t ch4_value = rcsignal.channels[SBUS_TRIGGER_CHANNEL];
|
|
|
|
// Detect pull low
|
|
if (ch4_value < SBUS_THRESHOLD_LOW)
|
|
{
|
|
rcsignal.pull_detected = true;
|
|
}
|
|
|
|
// Detect rising edge (pull high after low)
|
|
if (ch4_value > SBUS_THRESHOLD_HIGH && rcsignal.pull_detected)
|
|
{
|
|
rcsignal.pull_detected = false;
|
|
|
|
if (rcsignal.callback)
|
|
{
|
|
rcsignal.callback();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Check for signal timeout
|
|
int64_t now = esp_timer_get_time();
|
|
if (rcsignal.signal_active && (now - rcsignal.last_frame_time) > (100 * 1000))
|
|
{
|
|
rcsignal.signal_active = false;
|
|
memset((void *)rcsignal.channels, 0, sizeof(rcsignal.channels));
|
|
}
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(5));
|
|
}
|
|
#else
|
|
// PWM mode
|
|
uint32_t last_pulse_width = 0;
|
|
|
|
while (1)
|
|
{
|
|
vTaskDelay(pdMS_TO_TICKS(10));
|
|
|
|
// Check for signal timeout
|
|
int64_t now = esp_timer_get_time();
|
|
if (rcsignal.signal_active && (now - rcsignal.last_edge_time) > (SIGNAL_TIMEOUT_MS * 1000))
|
|
{
|
|
rcsignal.signal_active = false;
|
|
rcsignal.pulse_width_us = 0;
|
|
}
|
|
|
|
// Detect mode change (rising edge on PWM signal > 1500us)
|
|
if (rcsignal.pulse_width_us != last_pulse_width)
|
|
{
|
|
last_pulse_width = rcsignal.pulse_width_us;
|
|
|
|
if (rcsignal.pulse_width_us < PULSE_THRESHOLD_US)
|
|
{
|
|
rcsignal.pull_detected = true;
|
|
}
|
|
|
|
if (rcsignal.pulse_width_us > PULSE_THRESHOLD_US && rcsignal.pull_detected)
|
|
{
|
|
// Mode change detected
|
|
rcsignal.pull_detected = false;
|
|
|
|
if (rcsignal.callback)
|
|
{
|
|
rcsignal.callback();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
esp_err_t rcsignal_init(int8_t pin)
|
|
{
|
|
if (pin < 0)
|
|
{
|
|
ESP_LOGI(TAG, "RC signal disabled (no pin configured)");
|
|
return ESP_OK;
|
|
}
|
|
|
|
rcsignal.gpio_pin = pin;
|
|
|
|
#ifdef USE_SBUS_MODE
|
|
// SBUS Mode: Configure UART with inverted RX
|
|
ESP_LOGI(TAG, "Initializing SBUS mode on GPIO%d", pin);
|
|
|
|
uart_config_t uart_config = {
|
|
.baud_rate = SBUS_BAUDRATE,
|
|
.data_bits = UART_DATA_8_BITS,
|
|
.parity = UART_PARITY_EVEN,
|
|
.stop_bits = UART_STOP_BITS_2,
|
|
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
|
.source_clk = UART_SCLK_APB,
|
|
};
|
|
|
|
ESP_ERROR_CHECK(uart_param_config(UART_NUM, &uart_config));
|
|
ESP_ERROR_CHECK(uart_set_pin(UART_NUM, UART_PIN_NO_CHANGE, pin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
|
|
ESP_ERROR_CHECK(uart_driver_install(UART_NUM, UART_BUF_SIZE * 2, 0, 0, NULL, 0));
|
|
|
|
// Set inverted RX for FrSky receivers (they output inverted SBUS)
|
|
ESP_ERROR_CHECK(uart_set_line_inverse(UART_NUM, UART_SIGNAL_RXD_INV));
|
|
|
|
ESP_LOGI(TAG, "SBUS UART configured with inverted RX");
|
|
#else
|
|
// PWM Mode: Configure GPIO with interrupts
|
|
ESP_LOGI(TAG, "Initializing PWM mode on GPIO%d", pin);
|
|
|
|
gpio_config_t io_conf = {
|
|
.pin_bit_mask = (1ULL << pin),
|
|
.mode = GPIO_MODE_INPUT,
|
|
.pull_up_en = GPIO_PULLUP_ENABLE,
|
|
.pull_down_en = GPIO_PULLDOWN_DISABLE,
|
|
.intr_type = GPIO_INTR_ANYEDGE,
|
|
};
|
|
ESP_ERROR_CHECK(gpio_config(&io_conf));
|
|
|
|
// Install ISR service
|
|
ESP_ERROR_CHECK(gpio_isr_handler_add(pin, gpio_isr_handler, NULL));
|
|
#endif
|
|
|
|
// Create monitor task
|
|
BaseType_t ret = xTaskCreate(monitor_task, "rcsignal_monitor", 2048, NULL, 5, &rcsignal.monitor_task);
|
|
if (ret != pdPASS)
|
|
{
|
|
#ifdef USE_SBUS_MODE
|
|
uart_driver_delete(UART_NUM);
|
|
#else
|
|
gpio_isr_handler_remove(pin);
|
|
gpio_uninstall_isr_service();
|
|
#endif
|
|
return ESP_FAIL;
|
|
}
|
|
|
|
rcsignal.initialized = true;
|
|
ESP_LOGI(TAG, "RC signal initialized successfully");
|
|
|
|
return ESP_OK;
|
|
}
|
|
|
|
void rcsignal_deinit(void)
|
|
{
|
|
if (!rcsignal.initialized)
|
|
return;
|
|
|
|
if (rcsignal.monitor_task)
|
|
{
|
|
vTaskDelete(rcsignal.monitor_task);
|
|
rcsignal.monitor_task = NULL;
|
|
}
|
|
|
|
#ifdef USE_SBUS_MODE
|
|
uart_driver_delete(UART_NUM);
|
|
#else
|
|
if (rcsignal.gpio_pin >= 0)
|
|
{
|
|
gpio_isr_handler_remove(rcsignal.gpio_pin);
|
|
}
|
|
#endif
|
|
|
|
rcsignal.initialized = false;
|
|
}
|
|
|
|
void rcsignal_register_callback(rcsignal_mode_change_callback_t callback)
|
|
{
|
|
rcsignal.callback = callback;
|
|
}
|
|
|
|
uint32_t rcsignal_get_pulse_width(void)
|
|
{
|
|
#ifdef USE_SBUS_MODE
|
|
// In SBUS mode, return channel 4 value mapped to microseconds
|
|
// SBUS: 172-1811 -> PWM: ~1000-2000us
|
|
if (rcsignal.signal_active)
|
|
{
|
|
uint16_t ch_val = rcsignal.channels[SBUS_TRIGGER_CHANNEL];
|
|
return 1000 + ((ch_val - SBUS_CH_MIN) * 1000) / (SBUS_CH_MAX - SBUS_CH_MIN);
|
|
}
|
|
return 0;
|
|
#else
|
|
return rcsignal.pulse_width_us;
|
|
#endif
|
|
}
|
|
|
|
bool rcsignal_is_active(void)
|
|
{
|
|
return rcsignal.signal_active;
|
|
}
|
|
|
|
uint8_t rcsignal_get_current_mode(void)
|
|
{
|
|
return rcsignal.current_mode;
|
|
}
|
|
|
|
#ifdef USE_SBUS_MODE
|
|
uint16_t rcsignal_get_sbus_channel(uint8_t channel)
|
|
{
|
|
if (channel >= SBUS_NUM_CHANNELS)
|
|
{
|
|
return 0;
|
|
}
|
|
return rcsignal.channels[channel];
|
|
}
|
|
|
|
void rcsignal_debug_print_channels(void)
|
|
{
|
|
if (!rcsignal.signal_active)
|
|
{
|
|
ESP_LOGW(TAG, "No SBUS signal active");
|
|
return;
|
|
}
|
|
|
|
ESP_LOGI(TAG, "SBUS Channels:");
|
|
ESP_LOGI(TAG, " CH1: %4d CH2: %4d CH3: %4d CH4: %4d",
|
|
rcsignal.channels[0], rcsignal.channels[1],
|
|
rcsignal.channels[2], rcsignal.channels[3]);
|
|
ESP_LOGI(TAG, " CH5: %4d CH6: %4d CH7: %4d CH8: %4d",
|
|
rcsignal.channels[4], rcsignal.channels[5],
|
|
rcsignal.channels[6], rcsignal.channels[7]);
|
|
ESP_LOGI(TAG, " CH9: %4d CH10: %4d CH11: %4d CH12: %4d",
|
|
rcsignal.channels[8], rcsignal.channels[9],
|
|
rcsignal.channels[10], rcsignal.channels[11]);
|
|
ESP_LOGI(TAG, " CH13: %4d CH14: %4d CH15: %4d CH16: %4d",
|
|
rcsignal.channels[12], rcsignal.channels[13],
|
|
rcsignal.channels[14], rcsignal.channels[15]);
|
|
|
|
// Highlight the trigger channel
|
|
ESP_LOGI(TAG, "Trigger channel (CH%d): %d", SBUS_TRIGGER_CHANNEL + 1,
|
|
rcsignal.channels[SBUS_TRIGGER_CHANNEL]);
|
|
}
|
|
#endif
|