Added sbus and working prototype

2 years ago · 3434a75b1a
7 changed files with 375 additions and 25 deletions
--- a/platformio.ini
+++ b/platformio.ini
@ -12,3 +12,5 @@
 platform = espressif32
 board = esp32dev
 framework = arduino
+lib_deps =
+    plerup/EspSoftwareSerial@^6.16.1
--- a/src/.main.cpp.swp
+++ b/src/.main.cpp.swp
--- a/src/.main.h.swp
+++ b/src/.main.h.swp
--- a/src/main.cpp
+++ b/src/main.cpp
@ -43,6 +43,10 @@ void setup() {
    delay(100);
    initESP_NOW();

+    // Setting up SBUS
+    sbus.sbus_type = true;      // True: FAST, False: SLOW
+    sbus.begin();
+
    // Task Setup
    xTaskCreatePinnedToCore(
        taskButton,
@ -54,28 +58,115 @@ void setup() {
        CPU_0
    );

-    /**
    xTaskCreatePinnedToCore(
-        taskDemo,
-        "TaskDemo",           // Name of the process
+        taskCounter,
+        "TaskCounter",          // Name of the process
+        8192,                   // This stack size can be checked & adjusted by reading the Stack Highwater
+        NULL,
+        4,                      // Priority
+        NULL,
+        CPU_0
+    );
+
+    xTaskCreatePinnedToCore(
+        taskDisplay,
+        "TaskDisplay",          // Name of the process
+        8192,                   // This stack size can be checked & adjusted by reading the Stack Highwater
+        NULL,
+        4,                      // Priority
+        NULL,
+        CPU_1
+    );
+
+    xTaskCreatePinnedToCore(
+        taskSBUS,
+        "TaskSBUS",          // Name of the process
        8192,                   // This stack size can be checked & adjusted by reading the Stack Highwater
        NULL,
        4,                      // Priority
        NULL,
        CPU_1
    );
-     **/
 }

-void taskDemo(void *pvParameters) {
+
+void taskSBUS(void *pvParameters) {
    (void) pvParameters;
+
    for(;;) {
-        vTaskDelay(1000);
-        
-        // WiFi.softAPmacAddress(pairingData.mac_address);   
-        pairingData.channel = channel;
-        Serial.println("Task Demo - send response");
-        esp_now_send(broadcast_address, (uint8_t *) &pairingData, sizeof(pairingData));
+        vTaskDelay(1);
+        if(!sbus.signalNotOK) {
+            sbus.writePackets();
+        }
+    }
+}
+
+
+void taskDisplay(void *pvParameters) {
+    (void) pvParameters;
+    uint32_t last_display = 0;
+    uint32_t last_pulse = 0;
+    uint32_t pulse_ms = 0;
+    bool last_state = false;
+
+    for(;;) {
+        vTaskDelay(10);
+        if((uint32_t)(millis() - last_display) > 1000) {
+            if(counter > 500) {
+                pulse_ms = 25;
+            } else if(counter > 400) {
+                pulse_ms = 50;
+            } else if(counter > 300) {
+                pulse_ms = 75;
+            } else if(counter > 200) {
+                pulse_ms = 100;
+            } else if(counter > 100) {
+                pulse_ms = 125;
+            } else if(counter > 0) {
+                pulse_ms = 250;
+            }
+
+            char msg[64];
+            sprintf(msg, "Total Packets: %d Pulse: %d SBUS: %i", counter, pulse_ms, sbus.sbus_packets_sent);
+            Serial.println(msg);
+            last_display = millis();
+        }
+
+        if(!sync_mode) {
+            if(counter > 0) {
+                if((uint32_t)(millis() - last_pulse) > pulse_ms) {
+                    last_state = !last_state;
+                    digitalWrite(LED_INDICATOR, last_state);
+                    last_pulse = millis();
+                }
+            } else {
+                digitalWrite(LED_INDICATOR, LOW);
+            }
+        }
+    }
+}
+
+
+void taskCounter(void *pvParameters) {
+    (void) pvParameters;
+
+    uint32_t last_update = 0;
+
+    for(;;) {
+        vTaskDelay(1);
+        if((uint32_t)(millis() - last_update) > 1000) {
+            counter = counter_raw;
+            counter_raw = 0;
+
+            sbus.sbus_packets_sent = sbus.sbus_packets_sent_raw;
+            sbus.sbus_packets_sent_raw = 0;
+
+            if(counter == 0) {
+                sbus.signalNotOK = true;
+            }
+
+            last_update = millis();
+        }
    }
 }

@ -152,33 +243,63 @@ void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {

    if(sync_mode) {
        // Rebooting...
+        vTaskDelay(1000);
        ESP.restart();
    }
 }

 void OnDataRecv(const uint8_t * mac_addr, const uint8_t *incomingData, int len) { 
+    /***
    Serial.print(len);
    Serial.print(" bytes of data received from : ");
    printMAC(mac_addr);
    Serial.println();
-    String payload;
+     ***/
+    // Only allow address will be processed else ignore
+    bool allow_process = false;
+    if(
+        mac_addr[0] == allow_address[0] &&
+        mac_addr[1] == allow_address[1] &&
+        mac_addr[2] == allow_address[2] &&
+        mac_addr[3] == allow_address[3] &&
+        mac_addr[4] == allow_address[4] &&
+        mac_addr[5] == allow_address[5]
+    ) {
+        allow_process = true;
+    }
+
+    if(sync_mode) {
+        allow_process = true;
+    }

+    if(!allow_process)
+        return;
+    
    uint8_t type = incomingData[0];     // first message byte is the type of message 
    switch(type) {
        case DATA:                      // the message is data type
            memcpy(&incomingReadings, incomingData, sizeof(incomingReadings));
-            /***
-            // create a JSON document with received data and send it by event to the web page
-            root["id"] = incomingReadings.id;
-            root["temperature"] = incomingReadings.temp;
-            root["humidity"] = incomingReadings.hum;
-            root["readingId"] = String(incomingReadings.readingId);
-            serializeJson(root, payload);
-            Serial.print("event send :");
-            serializeJson(root, Serial);
-            events.send(payload.c_str(), "new_readings", millis());
-             ***/
-            Serial.println();
+            // Serial.println(incomingReadings.throttle);
+
+            sbus.rcCommand[THROTTLE] = incomingReadings.throttle;
+            sbus.rcCommand[YAW] = incomingReadings.yaw;
+            sbus.rcCommand[PITCH] = incomingReadings.pitch;
+            sbus.rcCommand[ROLL] = incomingReadings.roll;
+
+            sbus.rcCommand[AUX1] = incomingReadings.aux[0] ? 2000 : 1000;       // 1
+            sbus.rcCommand[AUX2] = incomingReadings.aux[1] ? 2000 : 1000;       // 1
+            sbus.rcCommand[AUX3] = incomingReadings.aux[2] ? 2000 : 1000;       // 1
+            sbus.rcCommand[AUX4] = incomingReadings.aux[3] ? 2000 : 1000;       // 1
+            sbus.rcCommand[AUX5] = incomingReadings.aux[4] ? 2000 : 1000;       // 1
+            sbus.rcCommand[AUX6] = incomingReadings.aux[5] ? 2000 : 1000;       // 1
+            sbus.rcCommand[AUX7] = incomingReadings.aux[6] ? 2000 : 1000;       // 1
+            sbus.rcCommand[AUX8] = incomingReadings.aux[7] ? 2000 : 1000;       // 1
+            sbus.rcCommand[AUX9] = incomingReadings.aux[8] ? 2000 : 1000;       // 1
+
+            // Set to false to make the sbus sending to FC or others.
+            sbus.signalNotOK = false;
+            counter_raw++;
+
            break;
  
        case PAIRING:                   // the message is a pairing request
@ -224,6 +345,10 @@ void OnDataRecv(const uint8_t * mac_addr, const uint8_t *incomingData, int len)
            sync_mode = false;
            digitalWrite(LED_INDICATOR, LOW);
            break; 
+
+        default:
+            Serial.println("Some error!");
+            break;
    }
 }

--- a/src/main.h
+++ b/src/main.h
@ -19,6 +19,7 @@
 #include "esp_wifi.h"
 #include <esp_now.h>
 #include <WiFi.h>
+#include "sbus.h"

 typedef struct struct_data {
    uint8_t message_type;
@ -60,11 +61,16 @@ uint8_t broadcast_address[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
 uint8_t allow_address[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
 esp_now_peer_info_t peer_info;

+int16_t counter_raw = 0;
+int16_t counter = 0;
+
 // Task Handler
 // void taskScreen(void *pvParameters);
 // void taskSpeak(void *pvParameters);
 void taskButton(void *pvParameters);
-void taskDemo(void *pvParameters);
+void taskCounter(void *pvParameters);
+void taskDisplay(void *pvParameters);
+void taskSBUS(void *pvParameters);

 // Functions
 void printMAC(const uint8_t * mac_addr);
--- a/src/sbus.cpp
+++ b/src/sbus.cpp
@ -0,0 +1,140 @@
+// REFERENCES:
+// https://www.circuitbasics.com/basics-uart-communication/
+// https://www.analog.com/en/analog-dialogue/articles/uart-a-hardware-communication-protocol.html
+// https://github.com/uzh-rpg/rpg_quadrotor_control/wiki/SBUS-Protocol
+// https://github.com/plerup/espsoftwareserial/blob/main/src/SoftwareSerial.cpp
+// https://lucidar.me/en/serialib/most-used-baud-rates-table/
+
+
+// SBUS data structure 
+#define RC_CHANS                18
+#define RC_CHANNEL_MIN          990
+#define RC_CHANNEL_MAX          2010
+
+
+#include "sbus.h"
+
+
+uint rcChannels[SBUS_CHANNEL_NUMBER];
+uint32_t sbusTime = 0;
+
+SoftwareSerial swSer1;
+
+SBUS::SBUS(void) {
+}
+
+void SBUS::begin(void) {
+    pinMode(SBUS_PORT, OUTPUT);
+
+    // ON it first as IDLE
+    //// GPOS = (1 << SBUS_PORT); 
+    GPIO.out_w1ts = (1 << SBUS_PORT);
+
+    // swSer1.begin(200000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT, false, 264);
+    // swSer1.begin(100000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT);
+
+    // Default to SBUS FAST protocol  
+    /****
+    if(sbus_type) {
+        swSer1.begin(200000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT);
+    } else {
+        swSer1.begin(100000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT);
+    }
+     ****/
+    swSer1.begin(100000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT);
+    swSer1.enableTx(true);
+    // swSer1.enableIntTx(true);
+
+    // Initialize to 1000
+    for(uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) {
+        rcCommand[i] = 1000;
+    }
+}
+
+void SBUS::run(void) {
+}
+
+uint32_t last_profile = 0;
+uint32_t total_high = 0;
+uint32_t total_low = 0;
+
+// Sending SBUS packets
+void SBUS::writePackets(void) {
+    if(signalNotOK) {
+        rcCommand[2] = 900;
+        rcCommand[3] = 900;
+        rcCommand[1] = 900;
+        rcCommand[0] = 900;
+    }
+
+    if((uint32_t)millis() - last_sent > SBUS_UPDATE_RATE) {
+        sbusPreparePacket(rcCommand, signalNotOK, false);
+        swSer1.write(sbusPacket, SBUS_PACKET_LENGTH);
+        sbus_packets_sent_raw++;
+        last_sent = millis();
+    }
+
+    // DEBUG ONLY
+    // if((uint32_t)(millis() - last_profile) > 1000) {
+    //     Serial.printf("Cmd: %d Signal: %d Sent: %d NRF: %d\r\n", rcCommand[2], signalNotOK, sbus_packets_sent, nrf_packets_received);
+    //     last_profile = millis();
+    // }
+
+}
+
+
+// Delay CPU Cycles
+void SBUS::delay_cycles(uint16_t cpucycles) {
+    uint32_t cpucycle_start = ESP.getCycleCount();
+    while((uint32_t)(ESP.getCycleCount() - cpucycle_start) < cpucycles) {}
+}
+
+
+// Preparing SBUS packets
+void SBUS::sbusPreparePacket(uint16_t channels[], bool isSignalLoss, bool isFailsafe){
+    static int output[SBUS_CHANNEL_NUMBER] = {0};
+
+    /*
+     * Map 1000-2000 with middle at 1500 chanel values to
+     * 173-1811 with middle at 992 S.BUS protocol requires
+     */
+    for(uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) {
+        output[i] = map(channels[i], RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET);
+    }
+
+    uint8_t stateByte = 0x00;
+    if (isSignalLoss) {
+        stateByte |= SBUS_STATE_SIGNALLOSS;
+    }
+    if (isFailsafe) {
+        stateByte |= SBUS_STATE_FAILSAFE;
+    }
+    sbusPacket[0]  = SBUS_FRAME_HEADER; //Header
+
+    sbusPacket[1]  = (uint8_t) (output[0] & 0x07FF);
+    sbusPacket[2]  = (uint8_t) ((output[0] & 0x07FF)>>8 | (output[1] & 0x07FF)<<3);
+    sbusPacket[3]  = (uint8_t) ((output[1] & 0x07FF)>>5 | (output[2] & 0x07FF)<<6);
+    sbusPacket[4]  = (uint8_t) ((output[2] & 0x07FF)>>2);
+    sbusPacket[5]  = (uint8_t) ((output[2] & 0x07FF)>>10 | (output[3] & 0x07FF)<<1);
+    sbusPacket[6]  = (uint8_t) ((output[3] & 0x07FF)>>7 | (output[4] & 0x07FF)<<4);
+    sbusPacket[7]  = (uint8_t) ((output[4] & 0x07FF)>>4 | (output[5] & 0x07FF)<<7);
+    sbusPacket[8]  = (uint8_t) ((output[5] & 0x07FF)>>1);
+    sbusPacket[9]  = (uint8_t) ((output[5] & 0x07FF)>>9 | (output[6] & 0x07FF)<<2);
+    sbusPacket[10] = (uint8_t) ((output[6] & 0x07FF)>>6 | (output[7] & 0x07FF)<<5);
+    sbusPacket[11] = (uint8_t) ((output[7] & 0x07FF)>>3);
+    sbusPacket[12] = (uint8_t) ((output[8] & 0x07FF));
+    sbusPacket[13] = (uint8_t) ((output[8] & 0x07FF)>>8 | (output[9] & 0x07FF)<<3);
+    sbusPacket[14] = (uint8_t) ((output[9] & 0x07FF)>>5 | (output[10] & 0x07FF)<<6);
+    sbusPacket[15] = (uint8_t) ((output[10] & 0x07FF)>>2);
+    sbusPacket[16] = (uint8_t) ((output[10] & 0x07FF)>>10 | (output[11] & 0x07FF)<<1);
+    sbusPacket[17] = (uint8_t) ((output[11] & 0x07FF)>>7 | (output[12] & 0x07FF)<<4);
+    sbusPacket[18] = (uint8_t) ((output[12] & 0x07FF)>>4 | (output[13] & 0x07FF)<<7);
+    sbusPacket[19] = (uint8_t) ((output[13] & 0x07FF)>>1);
+    sbusPacket[20] = (uint8_t) ((output[13] & 0x07FF)>>9 | (output[14] & 0x07FF)<<2);
+    sbusPacket[21] = (uint8_t) ((output[14] & 0x07FF)>>6 | (output[15] & 0x07FF)<<5);
+    sbusPacket[22] = (uint8_t) ((output[15] & 0x07FF)>>3);
+    sbusPacket[23] = stateByte; //Flags byte
+    sbusPacket[24] = SBUS_FRAME_FOOTER; //Footer
+}
+
+SBUS sbus;
--- a/src/sbus.h
+++ b/src/sbus.h
@ -0,0 +1,77 @@
+#ifndef SBUS_H_
+#define SBUS_H_
+
+// #define SBUS_MIN_OFFSET         173
+// #define SBUS_MID_OFFSET         992
+// #define SBUS_MAX_OFFSET         1811
+// #define SBUS_CHANNEL_NUMBER     16
+// #define SBUS_PACKET_LENGTH      25
+// #define SBUS_FRAME_HEADER       0x0f
+// #define SBUS_FRAME_FOOTER       0x00
+// #define SBUS_FRAME_FOOTER_V2    0x04
+// #define SBUS_STATE_FAILSAFE     0x08
+// #define SBUS_STATE_SIGNALLOSS   0x04
+// #define SBUS_UPDATE_RATE        15          // ms
+// #define SBUS_PORT               16          // Trigger pulse from the port
+
+// FAST SBUS TIMING (Theoritically 50% timing) 
+#define SBUS_MIN_OFFSET         173
+#define SBUS_MID_OFFSET         992
+#define SBUS_MAX_OFFSET         1811
+#define SBUS_CHANNEL_NUMBER     16
+#define SBUS_PACKET_LENGTH      25
+#define SBUS_FRAME_HEADER       0x0f
+#define SBUS_FRAME_FOOTER       0x00
+#define SBUS_FRAME_FOOTER_V2    0x04
+#define SBUS_STATE_FAILSAFE     0x08
+#define SBUS_STATE_SIGNALLOSS   0x04
+#define SBUS_UPDATE_RATE         6          // ms
+#define SBUS_PORT               16          // Trigger pulse from the port
+
+// Enumerators - User Defined
+enum rc { 
+    ROLL,
+    PITCH,
+    THROTTLE,
+    YAW, 
+    AUX1,
+    AUX2,
+    AUX3,
+    AUX4,
+    AUX5,
+    AUX6,
+    AUX7,
+    AUX8,
+    AUX9 
+};
+
+
+#include <Arduino.h>
+#include <SoftwareSerial.h>
+
+class SBUS {
+    public:
+        bool sbus_type = false;
+        bool signalNotOK = true;
+        bool invert;
+        uint16_t rcCommand[18];                     // Signal to ESC
+        uint8_t sbusPacket[SBUS_PACKET_LENGTH];
+        uint32_t last_sent = 0;
+
+        int16_t sbus_packets_sent_raw = 0;
+        int16_t sbus_packets_sent = 0;
+        
+        SBUS(void);
+        void begin(void);
+        void run(void);
+        void sbusPreparePacket(uint16_t channels[], bool isSignalLoss, bool isFailsafe);
+        void writePackets(void);
+
+        void delay_cycles(uint16_t cpucycles);
+ 
+    private:
+        uint32_t last_wifi_check, wifi_check_duration;
+};
+ 
+extern SBUS sbus;
+#endif