diff --git a/libraries/AP_HAL_ESP32/RCOutput.cpp b/libraries/AP_HAL_ESP32/RCOutput.cpp index fc1a9bd8ff70aa..ca2e6e136ac4dc 100644 --- a/libraries/AP_HAL_ESP32/RCOutput.cpp +++ b/libraries/AP_HAL_ESP32/RCOutput.cpp @@ -25,6 +25,21 @@ #if AP_SBUSOUTPUT_ENABLED #include +#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<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 index c7d73b4032e646..0da2d022d8ee1e 100644 --- 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 index 79f68e121467cb..af93638a0c5d9b 100644 --- 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 index 35d9d2e0445115..c4c304d560a13e 100644 --- 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 index a6ce95f236bbba..8c1d1af2e82d97 100644 --- 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]);