close #19 close #13

crossbow.ino

@@ -12,6 +12,7 @@
 #include <LoRa.h>
 // #include <PinChangeInterrupt.h>
 #include "variables.h"
+#include "sbus.h"
 #include "qsp.h"
 
 // LoRa32u4 ports
@@ -19,7 +20,7 @@
 #define LORA32U4_RST_PIN    4
 #define LORA32U4_DI0_PIN    7
 
-int ppm[PPM_OUTPUT_CHANNEL_COUNT] = {0};
+int ppm[16] = {0};
 
 /*
  * Main defines for device working in TX mode
@@ -40,7 +41,8 @@ PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true);
  * Main defines for device working in RX mode
  */
 #ifdef DEVICE_MODE_RX
-
+    uint32_t sbusTime = 0;
+    uint8_t sbusPacket[SBUS_PACKET_LENGTH] = {0};
 #endif
 
 /*
@@ -136,6 +138,10 @@ display.display();
 
 void setup(void)
 {
+#ifdef DEBUG_SERIAL
+    Serial.begin(115200);
+#endif
+
     qsp.hardwareWriteFunction = writeToRadio;
 
 #ifdef DEVICE_MODE_RX
@@ -149,7 +155,6 @@ void setup(void)
 #endif
 
 #ifdef LORA_HARDWARE_SPI
-    Serial.begin(115200);
 
 #ifdef WAIT_FOR_SERIAL
     while (!Serial) {
@@ -193,28 +198,16 @@ void setup(void)
     // display.display();
 
     //initiallize default ppm values
-    for (int i = 0; i < PPM_OUTPUT_CHANNEL_COUNT; i++)
+    for (int i = 0; i < 16; i++)
     {
         ppm[i] = PPM_CHANNEL_DEFAULT_VALUE;
     }
 
-    pinMode(PPM_OUTPUT_PIN, OUTPUT);
-    digitalWrite(PPM_OUTPUT_PIN, !PPM_SIGNAL_POSITIVE_STATE); //set the PPM signal pin to the default state (off)
-
-    cli();
-    TCCR1A = 0; // set entire TCCR1 register to 0
-    TCCR1B = 0;
-
-    OCR1A = 100;             // compare match register, change this
-    TCCR1B |= (1 << WGM12);  // turn on CTC mode
-    TCCR1B |= (1 << CS11);   // 8 prescaler: 0,5 microseconds at 16mhz
-    TIMSK1 |= (1 << OCIE1A); // enable timer compare interrupt
-    sei();
-
     pinMode(RX_ADC_PIN_1, INPUT);
     pinMode(RX_ADC_PIN_2, INPUT);
     pinMode(RX_ADC_PIN_3, INPUT);
 
+    Serial1.begin(100000, SERIAL_8E2);
 #endif
 
 #ifdef DEVICE_MODE_TX
@@ -238,54 +231,11 @@ void setup(void)
 #ifdef DEBUG_LED
     qsp.debugConfig |= DEBUG_FLAG_LED;
 #endif
+
 }
 
 #ifdef DEVICE_MODE_RX
 
-void writePpmOutput(uint8_t val) {
-    if (qsp.deviceState == DEVICE_STATE_OK) {
-        digitalWrite(PPM_OUTPUT_PIN, val);
-    } else {
-        //This is failsafe state, we pull output low so FC can decide about failsafe 
-        digitalWrite(PPM_OUTPUT_PIN, LOW);
-    }
-}
-
-ISR(TIMER1_COMPA_vect) { //leave this alone
-    static boolean state = true;
-
-    TCNT1 = 0;
-
-    if (state)
-    { //start pulse
-        writePpmOutput(PPM_SIGNAL_POSITIVE_STATE);
-        OCR1A = PPM_PULSE_LENGTH * PPM_OUTPUT_MULTIPLIER;
-        state = false;
-    }
-    else
-    { //end pulse and calculate when to start the next pulse
-        static byte cur_chan_numb;
-        static unsigned int calc_rest;
-
-        writePpmOutput(!PPM_SIGNAL_POSITIVE_STATE);
-        state = true;
-
-        if (cur_chan_numb >= PPM_OUTPUT_CHANNEL_COUNT)
-        {
-            cur_chan_numb = 0;
-            calc_rest = calc_rest + PPM_PULSE_LENGTH; //
-            OCR1A = (PPM_FRAME_LENGTH - calc_rest) * PPM_OUTPUT_MULTIPLIER;
-            calc_rest = 0;
-        }
-        else
-        {
-            OCR1A = (ppm[cur_chan_numb] - PPM_PULSE_LENGTH) * PPM_OUTPUT_MULTIPLIER;
-            calc_rest = calc_rest + ppm[cur_chan_numb];
-            cur_chan_numb++;
-        }
-    }
-}
-
 void updateRxDeviceState(RxDeviceState_t *rxDeviceState) {
     rxDeviceState->rxVoltage = map(analogRead(RX_ADC_PIN_1), 0, 1024, 0, 255);
     rxDeviceState->a1Voltage = map(analogRead(RX_ADC_PIN_2), 0, 1024, 0, 255);
@@ -306,7 +256,7 @@ void loop(void)
      */
     if (
         qsp.protocolState != QSP_STATE_IDLE && 
-        abs(millis() - qsp.frameDecodingStartedAt) > QSP_MAX_FRAME_DECODE_TIME 
+        abs(currentMillis - qsp.frameDecodingStartedAt) > QSP_MAX_FRAME_DECODE_TIME 
     ) {
         qsp.protocolState = QSP_STATE_IDLE;
     }
@@ -365,6 +315,14 @@ void loop(void)
 #endif
 
 #ifdef DEVICE_MODE_RX
+
+    if (currentMillis > sbusTime) {
+        sbusPreparePacket(sbusPacket, ppm, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE));
+        Serial1.write(sbusPacket, SBUS_PACKET_LENGTH);
+
+        sbusTime = currentMillis + SBUS_UPDATE_RATE;
+    }
+
     /*
      * RX_HEALTH QSP frame
      */

sbus.cpp

@@ -0,0 +1,61 @@
+#include "Arduino.h"
+#include "variables.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
+
+void sbusPreparePacket(uint8_t packet[], int 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;
+    }
+    packet[0] = SBUS_FRAME_HEADER; //Header
+    
+    packet[1] = (uint8_t) (output[0] & 0x07FF);
+    packet[2] = (uint8_t) ((output[0] & 0x07FF)>>8 | (output[1] & 0x07FF)<<3);
+    packet[3] = (uint8_t) ((output[1] & 0x07FF)>>5 | (output[2] & 0x07FF)<<6);
+    packet[4] = (uint8_t) ((output[2] & 0x07FF)>>2);
+    packet[5] = (uint8_t) ((output[2] & 0x07FF)>>10 | (output[3] & 0x07FF)<<1);
+    packet[6] = (uint8_t) ((output[3] & 0x07FF)>>7 | (output[4] & 0x07FF)<<4);
+    packet[7] = (uint8_t) ((output[4] & 0x07FF)>>4 | (output[5] & 0x07FF)<<7);
+    packet[8] = (uint8_t) ((output[5] & 0x07FF)>>1);
+    packet[9] = (uint8_t) ((output[5] & 0x07FF)>>9 | (output[6] & 0x07FF)<<2);
+    packet[10] = (uint8_t) ((output[6] & 0x07FF)>>6 | (output[7] & 0x07FF)<<5);
+    packet[11] = (uint8_t) ((output[7] & 0x07FF)>>3);
+    packet[12] = (uint8_t) ((output[8] & 0x07FF));
+    packet[13] = (uint8_t) ((output[8] & 0x07FF)>>8 | (output[9] & 0x07FF)<<3);
+    packet[14] = (uint8_t) ((output[9] & 0x07FF)>>5 | (output[10] & 0x07FF)<<6);  
+    packet[15] = (uint8_t) ((output[10] & 0x07FF)>>2);
+    packet[16] = (uint8_t) ((output[10] & 0x07FF)>>10 | (output[11] & 0x07FF)<<1);
+    packet[17] = (uint8_t) ((output[11] & 0x07FF)>>7 | (output[12] & 0x07FF)<<4);
+    packet[18] = (uint8_t) ((output[12] & 0x07FF)>>4 | (output[13] & 0x07FF)<<7);
+    packet[19] = (uint8_t) ((output[13] & 0x07FF)>>1);
+    packet[20] = (uint8_t) ((output[13] & 0x07FF)>>9 | (output[14] & 0x07FF)<<2);
+    packet[21] = (uint8_t) ((output[14] & 0x07FF)>>6 | (output[15] & 0x07FF)<<5);
+    packet[22] = (uint8_t) ((output[15] & 0x07FF)>>3);
+    
+    packet[23] = stateByte; //Flags byte
+    packet[24] = SBUS_FRAME_FOOTER; //Footer
+}

sbus.h

@@ -0,0 +1,3 @@
+#include "Arduino.h"
+
+void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe);

variables.h

@@ -1,11 +1,17 @@
 #pragma once
 
+#define SBUS_UPDATE_RATE 12 //ms
+#define SBUS_PACKET_LENGTH 25
+
+#define RC_CHANNEL_MIN 990
+#define RC_CHANNEL_MAX 2010
+
 //Only for UART connected radio modules
 #define UART_SPEED 57600
 #define E45_TTL_100_UART_DOWNTIME 30 
 
 #define RX_RX_HEALTH_FRAME_RATE 1000
-#define TX_RC_FRAME_RATE 50 //ms
+#define TX_RC_FRAME_RATE 500 //ms
 #define RX_FAILSAFE_DELAY (TX_RC_FRAME_RATE * 8)
 
 #define TX_PING_RATE 2000