diff --git a/src/main.cpp b/src/main.cpp index 078795f..64d61ae 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -23,14 +23,16 @@ int16_t sbus_packets_sent_raw = 0; int16_t nrf_packets_received = 0; int16_t nrf_packets_received_raw = 0; +bool sbus_busy = false; + Scheduler scheduler; RXMessage rxMessage; -// Every 7 miliseconds -Task task_sbus_generator(7, TASK_FOREVER, &send_sbus); +// Every 5 miliseconds +Task task_sbus_generator(4, TASK_FOREVER, &send_sbus); -// Every 8 millseconds -Task task_rx(8, TASK_FOREVER, &nrf_rx); +// Every 10 millseconds +// Task task_rx(10, TASK_FOREVER, &nrf_rx); // Every 200 miliseconds Task task_web(200, TASK_FOREVER, &web_handler); @@ -80,7 +82,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); @@ -88,7 +90,7 @@ void setup() { // Enabling Tasks task_sbus_generator.enable(); - task_rx.enable(); + ///// task_rx.enable(); task_heartbeat.enable(); task_web.enable(); task_ota.enable(); @@ -125,8 +127,10 @@ void status_heartbeat(void) { // Keep sending SBUS void send_sbus(void) { + sbus_busy = true; sbus.writePackets(); - sbus_packets_sent_raw++; + nrf_rx(); + sbus_busy = false; } @@ -160,9 +164,12 @@ void nrf_rx(void) { Serial.println("NRF READY TO LISTEN..."); } - if((uint32_t)(millis() - last_receive) > 1000) { + if(nrf_packets_received == 0) { sbus.signalNotOK = true; - } + } + + // Keep looping tille sbus_busy = false... + ////while(sbus_busy); if(rx.available()) { // Getting signals from Receiver diff --git a/src/main.h b/src/main.h index c099aeb..da0d6ed 100644 --- a/src/main.h +++ b/src/main.h @@ -32,6 +32,7 @@ void ota_handler(void); void uptime_tracker(void); extern int16_t sbus_packets_sent; +extern int16_t sbus_packets_sent_raw; extern int16_t nrf_packets_received; diff --git a/src/sbus.cpp b/src/sbus.cpp index bc7e733..1c94bfa 100644 --- a/src/sbus.cpp +++ b/src/sbus.cpp @@ -32,7 +32,7 @@ void SBUS::begin(void) { // swSer1.begin(100000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT); swSer1.begin(200000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT); swSer1.enableTx(true); - swSer1.enableIntTx(true); + // swSer1.enableIntTx(true); // Initialize to 1000 for(uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) { @@ -49,133 +49,26 @@ uint32_t total_low = 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] = 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); + 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); - // Serial2.write(sbusPacket, SBUS_PACKET_LENGTH); 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(); + } -// // 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 = 0; -// - if((uint32_t)(millis() - last_profile) > 500) { - Serial.printf("Cmd: %d Signal: %d Sent: %d NRF: %d\r\n", rcCommand[2], signalNotOK, sbus_packets_sent, nrf_packets_received); - last_profile = millis(); - } -// -// total_high = 0; -// total_low = 0; -// -// // bool tx_state = true; -// uint8_t parity_count = 0; -// int8_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); -// delay_cycles(PULSE_BIT_TIME); -// -// GPOS = (1 << SBUS_PORT); -// delay_cycles(PULSE_GAP); -// -// -// // DATA FRAME -// // Reading the data sending it byte by byte -// ///// for(data_bits = 7; data_bits >= 0; data_bits--) { -// for(data_bits = 0; data_bits < 8; data_bits++) { -// current_bit = (sbusPacket[pos] >> data_bits) & 0x01; -// -// // Sending it out... -// if(current_bit == 0x01) { -// parity_count++; -// -// total_high++; -// -// // HIGH -// GPOS = (1 << SBUS_PORT); -// delay_cycles(PULSE_BIT_TIME); -// -// } else { -// -// // LOW -// total_low++; -// -// GPOC = (1 << SBUS_PORT); -// delay_cycles(PULSE_BIT_TIME); -// } -// -// // GAP -// GPOS = (1 << SBUS_PORT); -// delay_cycles(PULSE_GAP); -// } -// -// // PARITY BITS -// // Parity Calculation -// if((parity_count & 0x01) == 0x01) { -// // ODD -// // Need to make it EVEN.... -// // HIGH -// GPOS = (1 << SBUS_PORT); -// delay_cycles(PULSE_BIT_TIME); -// -// } else { -// // EVEN -// // Nothing needs to be set -// // LOW -// GPOC = (1 << SBUS_PORT); -// delay_cycles(PULSE_BIT_TIME); -// } -// -// // GAP -// GPOS = (1 << SBUS_PORT); -// delay_cycles(PULSE_GAP); -// -// // STOP BITS -// // LOW -// GPOC = (1 << SBUS_PORT); -// delay_cycles(PULSE_BIT_TIME); -// -// // GAP -// GPOS = (1 << SBUS_PORT); -// delay_cycles(PULSE_GAP); -// -// // LOW -// GPOC = (1 << SBUS_PORT); -// delay_cycles(PULSE_BIT_TIME); -// -// -// // GAP -// GPOS = (1 << SBUS_PORT); -// delay_cycles(PULSE_GAP); -// } -// - - // Wait for next round - // sbusTime = millis(); - // } } diff --git a/src/sbus.h b/src/sbus.h index 46b9afa..9313c88 100644 --- a/src/sbus.h +++ b/src/sbus.h @@ -25,7 +25,7 @@ #define SBUS_FRAME_FOOTER_V2 0x04 #define SBUS_STATE_FAILSAFE 0x08 #define SBUS_STATE_SIGNALLOSS 0x04 -#define SBUS_UPDATE_RATE 15 // ms +#define SBUS_UPDATE_RATE 6 // ms #define SBUS_PORT 16 // Trigger pulse from the port @@ -39,6 +39,7 @@ class SBUS { bool invert; uint16_t rcCommand[18]; // Signal to ESC uint8_t sbusPacket[SBUS_PACKET_LENGTH]; + uint32_t last_sent = 0; SBUS(void); void begin(void);