diff --git a/DRAFT.md b/DRAFT.md new file mode 100644 index 0000000..47cc613 --- /dev/null +++ b/DRAFT.md @@ -0,0 +1,25 @@ +.0000000062 + mmmuuunnnp + +6.2nS per cpu cycle + +100000 baud ? +1/100000 = .0000100000 + +.0000100000 + mmmuuunnn + +10uS per bit + +HIGH: +80% HIGH 20% LOW +LOW: +50% HIGH 50% LOW + +10uS ~ 1612.9032258064 cpu cycles +HIGH: +1290.3225806451 HIGH 322.5806451612 LOW +LOW: +806.4516129032 HIGH 806.4516129032 LOW + + diff --git a/platformio.ini b/platformio.ini index 979b747..d7e8886 100644 --- a/platformio.ini +++ b/platformio.ini @@ -16,4 +16,5 @@ framework = arduino upload_protocol = espota upload_port = 192.168.4.1 board_build.filesystem = littlefs +board_build.f_cpu = 160000000L lib_deps = arkhipenko/TaskScheduler@^3.6.0 diff --git a/src/main.cpp b/src/main.cpp index 08b1827..33f9819 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -18,7 +18,7 @@ #define LED_HEARTBEAT 2 bool led_heartbeat_state = false; -uint16_t rcCommand[18]; // Signal to ESC +//// uint16_t rcCommand[18]; // Signal to ESC Scheduler scheduler; RXMessage rxMessage; @@ -43,6 +43,9 @@ void setup() { // Delay a bit delay(100); + // Checking CPU Speed + Serial.printf("CPU: %d MHz\r\n", ESP.getCpuFreqMHz()); + // Setting up status led pinMode(LED_STATUS, OUTPUT); pinMode(LED_HEARTBEAT, OUTPUT); @@ -58,7 +61,7 @@ void setup() { // swSer1.begin(100000, SWSERIAL_8N2, D5, D5, false, 256); // Start up NRF24L01 - //////// rx.begin(); + rx.begin(); // Setup Receiver // setup_rx(); @@ -73,7 +76,7 @@ void setup() { Serial.println("Enabling Tasks..."); // Add task to scheduler scheduler.addTask(task_sbus_generator); - ////////// scheduler.addTask(task_rx); + scheduler.addTask(task_rx); scheduler.addTask(task_heartbeat); scheduler.addTask(task_web); scheduler.addTask(task_ota); @@ -81,7 +84,7 @@ void setup() { // Enabling Tasks task_sbus_generator.enable(); - //////////// task_rx.enable(); + task_rx.enable(); task_heartbeat.enable(); task_web.enable(); task_ota.enable(); @@ -146,6 +149,8 @@ void uptime_tracker(void) { // DEBUGGING... uint32_t printagain = 0; bool first_load = true; +uint16_t last_receive = 0; + // Processing received data void nrf_rx(void) { if(first_load == true) { @@ -159,13 +164,18 @@ void nrf_rx(void) { setup_rx(); Serial.println("NRF READY TO LISTEN..."); - } + } + + if((uint32_t)(millis() - last_receive) > 1000) { + sbus.signalNotOK = true; + } if(rx.available()) { // Getting signals from Receiver rx.read(&rxMessage, sizeof(RXMessage)); // Debugging only. Remove this once working. + /*** Serial.print(rxMessage.Byte00); Serial.print(" "); Serial.print(rxMessage.Byte01); @@ -178,6 +188,7 @@ void nrf_rx(void) { Serial.print(" "); Serial.print(rxMessage.Byte05); Serial.println(" "); + ***/ // static bool firstload = true; @@ -199,21 +210,23 @@ void nrf_rx(void) { rcCommand[AUX8] = ((rxMessage.Byte00 >> 0) & 0x01) ? 2000 : 1000; // 8 rcCommand[AUX9] = ((rxMessage.Byte01 >> 7) & 0x01) ? 2000 : 1000; // 9 ***/ - rcCommand[AUX1] = ((rxMessage.Byte00 >> 7) & 0x01) ? 1 : 0; // 1 - rcCommand[AUX2] = ((rxMessage.Byte00 >> 6) & 0x01) ? 1 : 0; // 2 - rcCommand[AUX3] = ((rxMessage.Byte00 >> 5) & 0x01) ? 1 : 0; // 3 - rcCommand[AUX4] = ((rxMessage.Byte00 >> 4) & 0x01) ? 1 : 0; // 4 - rcCommand[AUX5] = ((rxMessage.Byte00 >> 3) & 0x01) ? 1 : 0; // 5 - rcCommand[AUX6] = ((rxMessage.Byte00 >> 2) & 0x01) ? 1 : 0; // 6 - rcCommand[AUX7] = ((rxMessage.Byte00 >> 1) & 0x01) ? 1 : 0; // 7 - rcCommand[AUX8] = ((rxMessage.Byte00 >> 0) & 0x01) ? 1 : 0; // 8 - rcCommand[AUX9] = ((rxMessage.Byte01 >> 7) & 0x01) ? 1 : 0; // 9 - - rcCommand[THROTTLE] = ((rxMessage.Byte01 & 0X7F) << 4) | (rxMessage.Byte02 & 0xF0) >> 4; // Throttle - rcCommand[YAW] = ((rxMessage.Byte02 & 0x07) << 8) | rxMessage.Byte03; // Yaw - rcCommand[PITCH] = ((rxMessage.Byte04 & 0x7F) << 4) | (rxMessage.Byte05 & 0xF0) >> 4; // Pitch - rcCommand[ROLL] = ((rxMessage.Byte05 & 0x07) << 8) | rxMessage.Byte06; // Roll - + sbus.rcCommand[AUX1] = ((rxMessage.Byte00 >> 7) & 0x01) ? 2000 : 1000; // 1 + sbus.rcCommand[AUX2] = ((rxMessage.Byte00 >> 6) & 0x01) ? 2000 : 1000; // 2 + sbus.rcCommand[AUX3] = ((rxMessage.Byte00 >> 5) & 0x01) ? 2000 : 1000; // 3 + sbus.rcCommand[AUX4] = ((rxMessage.Byte00 >> 4) & 0x01) ? 2000 : 1000; // 4 + sbus.rcCommand[AUX5] = ((rxMessage.Byte00 >> 3) & 0x01) ? 2000 : 1000; // 5 + sbus.rcCommand[AUX6] = ((rxMessage.Byte00 >> 2) & 0x01) ? 2000 : 1000; // 6 + sbus.rcCommand[AUX7] = ((rxMessage.Byte00 >> 1) & 0x01) ? 2000 : 1000; // 7 + sbus.rcCommand[AUX8] = ((rxMessage.Byte00 >> 0) & 0x01) ? 2000 : 1000; // 8 + sbus.rcCommand[AUX9] = ((rxMessage.Byte01 >> 7) & 0x01) ? 2000 : 1000; // 9 + + sbus.rcCommand[THROTTLE] = ((rxMessage.Byte01 & 0X7F) << 4) | (rxMessage.Byte02 & 0xF0) >> 4; // Throttle + sbus.rcCommand[YAW] = ((rxMessage.Byte02 & 0x07) << 8) | rxMessage.Byte03; // Yaw + sbus.rcCommand[PITCH] = ((rxMessage.Byte04 & 0x7F) << 4) | (rxMessage.Byte05 & 0xF0) >> 4; // Pitch + sbus.rcCommand[ROLL] = ((rxMessage.Byte05 & 0x07) << 8) | rxMessage.Byte06; // Roll + + sbus.signalNotOK = false; + last_receive = millis(); } } diff --git a/src/nrf24l01.cpp b/src/nrf24l01.cpp index 846964f..6195d4f 100644 --- a/src/nrf24l01.cpp +++ b/src/nrf24l01.cpp @@ -441,7 +441,7 @@ bool NRF24L01::begin(void) { pinMode(ce_pin, OUTPUT); pinMode(csn_pin, OUTPUT); } - // _SPI.begin(); + _SPI.begin(); ce(LOW); csn(HIGH); diff --git a/src/sbus.cpp b/src/sbus.cpp index 00eb609..37cdd71 100644 --- a/src/sbus.cpp +++ b/src/sbus.cpp @@ -5,34 +5,23 @@ // https://github.com/plerup/espsoftwareserial/blob/main/src/SoftwareSerial.cpp // https://lucidar.me/en/serialib/most-used-baud-rates-table/ -#include "sbus.h" // SBUS data structure #define RC_CHANS 18 #define RC_CHANNEL_MIN 990 #define RC_CHANNEL_MAX 2010 -#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 13 // Trigger pulse from the port - -uint8_t sbusPacket[SBUS_PACKET_LENGTH]; -int rcChannels[SBUS_CHANNEL_NUMBER]; + +#include "sbus.h" + + +uint rcChannels[SBUS_CHANNEL_NUMBER]; uint32_t sbusTime = 0; -bool signalNotOK = false; uint32_t counter = 900; uint32_t last_count = 0; +SoftwareSerial swSer1; SBUS::SBUS(void) { } @@ -42,93 +31,150 @@ void SBUS::begin(void) { // ON it first as IDLE GPOS = (1 << SBUS_PORT); - // swSer1.begin(220000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT, false, 256); - // swSer1.enableIntTx(false); - // swSer1.enableTx(true); + // swSer1.begin(200000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT, false, 264); + // swSer1.begin(100000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT); + swSer1.begin(200000, 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; + // Sending SBUS packets void SBUS::writePackets(void) { if((uint32_t)(millis() - sbusTime) > SBUS_UPDATE_RATE) { if(signalNotOK) { // DEBUG ONLY...later put back to 900 - rcChannels[2] = 1800; - rcChannels[3] = 900; - rcChannels[1] = 900; - rcChannels[0] = 900; + // rcChannels[2] = 900; + // rcChannels[3] = 900; + // rcChannels[1] = 900; + // rcChannels[0] = 900; + rcCommand[2] = 900; + rcCommand[3] = 900; + rcCommand[1] = 900; + rcCommand[0] = 900; } - sbusPreparePacket(sbusPacket, rcChannels, signalNotOK, false); + // sbusPreparePacket(sbusPacket, rcChannels, signalNotOK, false); + sbusPreparePacket(rcCommand, signalNotOK, false); // Serial2.write(sbusPacket, SBUS_PACKET_LENGTH); - /// swSer1.write(sbusPacket, SBUS_PACKET_LENGTH); + swSer1.write(sbusPacket, SBUS_PACKET_LENGTH); + // Looping through the bytes - Generating Parity Bits // Is using SBUS_FAST (200000 Baud), 8E2 (Even Parity and 2 bits stop // For sending at 200000 baud, each bit used about 5uS // Profiling each bits timing before sending - uint32_t clock_cycles_per_micro = 0; - uint32_t clock_cycles = 0; - uint32_t clock_cycles_total_start = millis(); - - while((uint32_t) millis() - clock_cycles_total_start < 1) { - clock_cycles_per_micro++; - } - - // bool tx_state = true; - uint8_t parity_count = 0; - uint8_t data_bits = 0; - bool current_bit = 0; - - - // Now we roughly know how much per clock cycles per microseconds - for(uint8_t pos = 0; pos < SBUS_PACKET_LENGTH; pos++) { - // Now we are going to send the data from LSB and with EVEN parity on each byte with 2 bits stop. - // Looping through each bytes with: - // START BIT | DATA FRAME | PARITY BITS | STOP BITS - - // START BIT - For 1 clock cycle = 5uS = clock_per_micro - // Set to LOW as start bit - GPOC = (1 << SBUS_PORT); - for(clock_cycles = 0; clock_cycles < clock_cycles_per_micro; clock_cycles++); - - // DATA FRAME - // Reading the data sending it byte by byte - for(data_bits = 0; data_bits < 8; data_bits++) { - current_bit = (sbusPacket[pos] >> data_bits) & 0x01; - // Sending it out... - if(current_bit) { - parity_count++; - - // HIGH - GPOS = (1 << SBUS_PORT); - } else { - // LOW - GPOC = (1 << SBUS_PORT); - } - for(clock_cycles = 0; clock_cycles < clock_cycles_per_micro; clock_cycles++); - } - - // PARITY BITS - // Parity Calculation - if(parity_count & 0x01) { - // ODD - // Need to make it EVEN.... - GPOS = (1 << SBUS_PORT); - } else { - // EVEN - // Nothing needs to be set - GPOC = (1 << SBUS_PORT); - } - for(clock_cycles = 0; clock_cycles < clock_cycles_per_micro; clock_cycles++); - - // STOP BITS - GPOS = (1 << SBUS_PORT); - for(clock_cycles = 0; clock_cycles < clock_cycles_per_micro; clock_cycles++); - for(clock_cycles = 0; clock_cycles < clock_cycles_per_micro; clock_cycles++); - } +// uint32_t clock_cycles = 0; +// +// if((uint32_t)(millis() - last_profile) > 500) { +// Serial.printf("Cmd: %d Signal: %d\r\n", rcCommand[2], signalNotOK); +// last_profile = millis(); +// } +// +// // bool tx_state = true; +// uint8_t parity_count = 0; +// uint8_t data_bits = 0; +// bool current_bit = 0; +// +// // Now we roughly know how much per clock cycles per microseconds +// for(uint8_t pos = 0; pos < SBUS_PACKET_LENGTH; pos++) { +// // Now we are going to send the data from LSB and with EVEN parity on each byte with 2 bits stop. +// // Looping through each bytes with: +// // START BIT | DATA FRAME | PARITY BITS | STOP BITS +// +// // START BIT - For 1 clock cycle = 5uS = clock_per_micro +// // Set to LOW as start bit +// GPOS = (1 << SBUS_PORT); +// +// // Low for 50% duty cycle +// for(clock_cycles = 0; clock_cycles < PULSE_LOW_ON_TIME; clock_cycles++); +// +// GPOC = (1 << SBUS_PORT); +// +// // High for 50% duty cycle +// for(clock_cycles = 0; clock_cycles < PULSE_LOW_OFF_TIME; clock_cycles++); +// +// // DATA FRAME +// // Reading the data sending it byte by byte +// for(data_bits = 0; data_bits < 8; data_bits++) { +// current_bit = (sbusPacket[pos] >> data_bits) & 0x01; +// +// // Sending it out... +// if(current_bit) { +// parity_count++; +// +// // HIGH +// GPOS = (1 << SBUS_PORT); +// for(clock_cycles = 0; clock_cycles < PULSE_HIGH_ON_TIME; clock_cycles++); +// +// // LOW +// GPOC = (1 << SBUS_PORT); +// for(clock_cycles = 0; clock_cycles < PULSE_HIGH_OFF_TIME; clock_cycles++); +// +// +// } else { +// // HIGH +// GPOS = (1 << SBUS_PORT); +// for(clock_cycles = 0; clock_cycles < PULSE_LOW_ON_TIME; clock_cycles++); +// +// // LOW +// GPOC = (1 << SBUS_PORT); +// for(clock_cycles = 0; clock_cycles < PULSE_LOW_OFF_TIME; clock_cycles++); +// } +// } +// +// // PARITY BITS +// // Parity Calculation +// if(parity_count & 0x01) { +// // ODD +// // Need to make it EVEN.... +// // HIGH +// GPOS = (1 << SBUS_PORT); +// for(clock_cycles = 0; clock_cycles < PULSE_HIGH_ON_TIME; clock_cycles++); +// +// // LOW +// GPOC = (1 << SBUS_PORT); +// for(clock_cycles = 0; clock_cycles < PULSE_HIGH_OFF_TIME; clock_cycles++); +// } else { +// // EVEN +// // Nothing needs to be set +// // HIGH +// GPOS = (1 << SBUS_PORT); +// for(clock_cycles = 0; clock_cycles < PULSE_LOW_ON_TIME; clock_cycles++); +// +// // LOW +// GPOC = (1 << SBUS_PORT); +// for(clock_cycles = 0; clock_cycles < PULSE_LOW_OFF_TIME; clock_cycles++); +// } +// +// // STOP BITS +// // HIGH +// GPOS = (1 << SBUS_PORT); +// for(clock_cycles = 0; clock_cycles < PULSE_LOW_ON_TIME; clock_cycles++); +// +// // LOW +// GPOC = (1 << SBUS_PORT); +// for(clock_cycles = 0; clock_cycles < PULSE_LOW_OFF_TIME; clock_cycles++); +// +// // HIGH +// GPOS = (1 << SBUS_PORT); +// for(clock_cycles = 0; clock_cycles < PULSE_LOW_ON_TIME; clock_cycles++); +// +// // LOW +// GPOC = (1 << SBUS_PORT); +// for(clock_cycles = 0; clock_cycles < PULSE_LOW_OFF_TIME; clock_cycles++); +// +// } // Wait for next round @@ -137,7 +183,7 @@ void SBUS::writePackets(void) { } // Preparing SBUS packets -void SBUS::sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe){ +void SBUS::sbusPreparePacket(uint16_t channels[], bool isSignalLoss, bool isFailsafe){ if(millis() - last_count > 2000) { counter+= 100; @@ -153,10 +199,11 @@ void SBUS::sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss * 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); + 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); // DEBUG oNly - output[i] = counter; + // output[i] = 1200; + // output[i] = counter; } uint8_t stateByte = 0x00; @@ -166,32 +213,32 @@ void SBUS::sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss 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 + 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; diff --git a/src/sbus.h b/src/sbus.h index 658f8c0..740d519 100644 --- a/src/sbus.h +++ b/src/sbus.h @@ -1,17 +1,40 @@ #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 + +// For BAUD RATE: 100000 +#define PULSE_HIGH_ON_TIME 1290 +#define PULSE_HIGH_OFF_TIME 323 +#define PULSE_LOW_ON_TIME 807 +#define PULSE_LOW_OFF_TIME 807 +#define PULSE_TOTAL_TIME 1613 + #include -// #include "SoftwareSerial.h" +#include "SoftwareSerial.h" class SBUS { public: + bool signalNotOK = true; bool invert; + uint16_t rcCommand[18]; // Signal to ESC + uint8_t sbusPacket[SBUS_PACKET_LENGTH]; SBUS(void); void begin(void); void run(void); - void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe); + void sbusPreparePacket(uint16_t channels[], bool isSignalLoss, bool isFailsafe); void writePackets(void); private: