diff --git a/crossbow.ino b/crossbow.ino index b63b31b..600c649 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -12,6 +12,7 @@ #include // #include #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 */ diff --git a/sbus.cpp b/sbus.cpp new file mode 100644 index 0000000..53ffe0c --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/sbus.h b/sbus.h new file mode 100644 index 0000000..4004277 --- /dev/null +++ b/sbus.h @@ -0,0 +1,3 @@ +#include "Arduino.h" + +void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe); \ No newline at end of file diff --git a/variables.h b/variables.h index 3084d8b..a9b4194 100644 --- a/variables.h +++ b/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