adding SBUS to RMT

radityankn · Sep 21, 2023 · da91257 · da91257
1 parent 53272a9
commit da91257
Show file tree

Hide file tree

Showing 5 changed files with 174 additions and 11 deletions.
diff --git a/libraries/AP_HAL_ESP32/RCOutput.cpp b/libraries/AP_HAL_ESP32/RCOutput.cpp
@@ -25,6 +25,21 @@
 
 #if AP_SBUSOUTPUT_ENABLED
 #include <AP_SBusOut/AP_SBusOut.h>
+#include "driver/rmt.h"
+
+typedef struct {
+  rmt_item32_t items[6];
+  int item_index;
+} uart_tx_buffer_t;
+
+
+typedef struct {
+  rmt_config_t channel_config;
+} uart_tx_config_t;
+
+uart_tx_config_t* config = new uart_tx_config_t;
+uint32_t baudrate;
+
 #endif
 
 #include "driver/rtc_io.h"
@@ -64,6 +79,25 @@ void RCOutput::init()
     printf("RCOutput::init() - channels available: %d \n",(int)MAX_CHANNELS);
     printf("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo\n");
 
+    #if AP_SBUSOUTPUT_ENABLED
+    int clk_div = 80; //APB_CLK_FREQ/50/baudrate;
+    config->channel_config.gpio_num = SBUS_OUT_PIN;
+    config->channel_config.rmt_mode = RMT_MODE_TX;
+    config->channel_config.clk_div = clk_div;
+    config->channel_config.channel = RMT_CHANNEL_2;
+    config->channel_config.mem_block_num = 1;
+    //config->channel_config.flags = RMT_CHANNEL_FLAGS_INVERT_SIG;
+    config->channel_config.tx_config.idle_level = rmt_idle_level_t(1);
+    config->channel_config.tx_config.carrier_en = 0;
+    //config->channel_config.tx_config.loop_count = 0;
+    config->channel_config.tx_config.idle_output_en = 1;
+    config->channel_config.tx_config.loop_en = 0;
+    rmt_config(&config->channel_config);
+    rmt_driver_install(config->channel_config.channel, 0, 0);
+
+
+
+    #else
     static const mcpwm_io_signals_t signals[] = {
         MCPWM0A,
         MCPWM0B,
@@ -113,6 +147,7 @@ void RCOutput::init()
         mcpwm_init(unit, timer, &pwm_config);
         mcpwm_start(unit, timer);
     }
+    #endif
     _initialized = true;
 }
 
@@ -218,6 +253,129 @@ void RCOutput::push()
 
     bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED;
 
+    #if AP_SBUSOUTPUT_ENABLED
+    AP_SBusOut sbus;
+    uint32_t* data = sbus.update();
+    uart_tx_buffer_t *buffer = new uart_tx_buffer_t;
+
+    int delay = 8000; //(APB_CLK_FREQ/(APB_CLK_FREQ/50/baudrate))/baudrate;
+    int parity_counter = 0;
+    buffer->items[0].level0 = 0;
+    buffer->items[0].duration0 = delay;
+    //bit 0
+    if ((data & 0b00000001) == 0b00000001) {
+      buffer->items[0].level1 = 1;
+      buffer->items[0].duration1 = delay;
+      parity_counter++;
+      ESP_LOGI("bits","1");
+    }
+    else {
+      buffer->items[0].level1 = 0;
+      buffer->items[0].duration1 = delay;
+      ESP_LOGI("bits","0");
+    }
+    //bit 1
+    if ((data & 0b00000010) == 0b00000010) {
+      buffer->items[1].level0 = 1;
+      buffer->items[1].duration0 = delay;
+      parity_counter++;
+      ESP_LOGI("bits","1");
+    }
+    else {
+      buffer->items[1].level0 = 0;
+      buffer->items[1].duration0 = delay;
+      ESP_LOGI("bits","0");
+    }
+    //bit 2
+    if ((data & 0b00000100) == 0b00000100) {
+      buffer->items[1].level1 = 1;
+      buffer->items[1].duration1 = delay;
+      parity_counter++;
+      ESP_LOGI("bits","1");
+    }
+    else {
+      buffer->items[1].level1 = 0;
+      buffer->items[1].duration1 = delay;
+      ESP_LOGI("bits","0");
+    }
+    //bit 3
+    if ((data & 0b00001000) == 0b00001000) {
+      buffer->items[2].level0 = 1;
+      buffer->items[2].duration0 = delay;
+      parity_counter++;
+      ESP_LOGI("bits","1");
+    }
+    else {
+      buffer->items[2].level0 = 0;
+      buffer->items[2].duration0 = delay;
+      ESP_LOGI("bits","0");
+    }
+    //bit 4
+    if ((data & 0b00010000) == 0b00010000) {
+      buffer->items[2].level1 = 1;
+      buffer->items[2].duration1 = delay;
+      parity_counter++;
+      ESP_LOGI("bits","1");
+    }
+    else {
+      buffer->items[2].level1 = 0;
+      buffer->items[2].duration1 = delay;
+      ESP_LOGI("bits","0");
+    }
+    //bit 5
+    if ((data & 0b00100000) == 0b00100000) {
+      buffer->items[3].level0 = 1;
+      buffer->items[3].duration0 = delay;
+      parity_counter++;
+      ESP_LOGI("bits","1");
+    }
+    else {
+      buffer->items[3].level0 = 0;
+      buffer->items[3].duration0 = delay;
+      ESP_LOGI("bits","0");
+    }
+    //bit 6
+    if ((data & 0b01000000) == 0b01000000) {
+      buffer->items[3].level1 = 1;
+      buffer->items[3].duration1 = delay;
+      parity_counter++;
+      ESP_LOGI("bits","1");
+    }
+    else {
+      buffer->items[3].level1 = 0;
+      buffer->items[3].duration1 = delay;
+      ESP_LOGI("bits","0");
+    }
+    //bit 7
+    if ((data & 0b10000000) == 0b10000000) {
+      buffer->items[4].level0 = 1;
+      buffer->items[4].duration0 = delay;
+      parity_counter++;
+      ESP_LOGI("bits","1");
+    }
+    else {
+      buffer->items[4].level0 = 0;
+      buffer->items[4].duration0 = delay;
+      ESP_LOGI("bits","0");
+    }
+    //parity
+    if (parity_counter%2 == 0) {
+      buffer->items[4].level1 = 1;
+      buffer->items[4].duration1 = delay;
+    }
+    else {
+      buffer->items[4].level1 = 0;
+      buffer->items[4].duration1 = delay;
+    }
+    buffer->items[5].level0 = 1;
+    buffer->items[5].duration0 = delay;
+    buffer->items[5].level1 = 1;
+    buffer->items[5].duration1 = delay;
+    for (int i = 0; i < 6; i++) {
+        rmt_write_items(config->channel_config.channel, &buffer->items[i], 1, 1);
+    }
+
+    #else
     for (uint8_t i = 0; i < MAX_CHANNELS; i++) {
         if ((1U<<i) & _pending_mask) {
             uint32_t period_us = _pending[i];
@@ -230,9 +388,8 @@ void RCOutput::push()
             write_int(i, period_us);
         }
     }
-#if AP_SBUSOUTPUT_ENABLED
-	sbus->update();
-#endif
+    #endif
+
     _corked = false;
 }
 
@@ -247,6 +404,7 @@ void RCOutput::write_int(uint8_t chan, uint16_t period_us)
         return;
     }
 
+
     bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED;
     if (safety_on && !(safety_mask & (1U<<(chan)))) {
         // safety is on, overwride pwm

diff --git a/libraries/AP_HAL_ESP32/RmtSigReader.cpp b/libraries/AP_HAL_ESP32/RmtSigReader.cpp
@@ -11,7 +11,7 @@ void RmtSigReader::init()
     config.rmt_mode = RMT_MODE_RX;
     config.channel = RMT_CHANNEL_0;
     config.clk_div = 80;   //80MHZ APB clock to the 1MHZ target frequency
-    config.gpio_num = HAL_ESP32_RCIN;
+    config.gpio_num = SBUS_OUT_PIN;
     config.mem_block_num = 2; //each block could store 64 pulses
     config.rx_config.filter_en = true;
     config.rx_config.filter_ticks_thresh = 8;

diff --git a/libraries/AP_HAL_ESP32/boards/esp32empty.h b/libraries/AP_HAL_ESP32/boards/esp32empty.h
@@ -153,6 +153,7 @@
 //RMT pin number
 #define HAL_ESP32_RMT_RX_PIN_NUMBER			-1
 
+
 //SD CARD
 // Do u want to use mmc or spi mode for the sd card, this is board specific ,
 //  as mmc uses specific pins but is quicker,
@@ -175,6 +176,6 @@
 #define HAL_LOGGING_BACKENDS_DEFAULT		1
 //#define AP_NOTIFY_TONEALARM_ENABLED			1			//this definition is for activating ToneAlarm
 //#define HAL_PWM_BUZZER_PIN						14			//pin of where your ToneAlarm buzzer is	
-//#define AP_SBUSOUTPUT_ENABLED					TRUE
-//#define SBUS_OUT_PIN								GPIO_NUM_21		
+#define AP_SBUSOUTPUT_ENABLED					TRUE
+#define SBUS_OUT_PIN								GPIO_NUM_21		
 
diff --git a/libraries/AP_SBusOut/AP_SBusOut.cpp b/libraries/AP_SBusOut/AP_SBusOut.cpp
@@ -124,23 +124,25 @@ void AP_SBusOut::sbus_format_frame(uint16_t *channels, uint8_t num_channels, uin
  * build and send sbus1 frame representing first 16 servo channels
  * input arg is pointer to uart
  */
-void
+uint8_t*
 AP_SBusOut::update()
 {
+    uint8_t buffer[SBUS_BSIZE];
+
     if (!initialised) {
         initialised = true;
         init();
     }
 
     if (sbus1_uart == nullptr) {
-        return;
+        return buffer;
     }
 
     // constrain output rate using sbus_frame_interval
     static uint32_t last_micros = 0;
     uint32_t now = AP_HAL::micros();
     if ((now - last_micros) <= sbus_frame_interval) {
-        return;
+        return buffer;
     }
 
     last_micros = now;
@@ -156,7 +158,7 @@ AP_SBusOut::update()
         }
         channels[i] = c->get_output_pwm();
     }
-    uint8_t buffer[SBUS_BSIZE];
+
     sbus_format_frame(channels, nchan, buffer);
 
 #if SBUS_DEBUG
@@ -170,6 +172,8 @@ AP_SBusOut::update()
     hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
     hal.gpio->write(55, 0);
 #endif
+
+    return buffer;
 }
 
 void AP_SBusOut::init() {

diff --git a/libraries/AP_SBusOut/AP_SBusOut.h b/libraries/AP_SBusOut/AP_SBusOut.h
@@ -23,7 +23,7 @@ class AP_SBusOut {
 
     static const struct AP_Param::GroupInfo var_info[];
 
-    void update();
+    uint8_t* update();
 
     // public format function for use by IOMCU
     static void sbus_format_frame(uint16_t *channels, uint8_t num_channels, uint8_t buffer[25]);