fixes, debug, radio loop

JuReceiver.ino

@@ -1,5 +1,4 @@
 
-
 // RC Config
 #define myId 0x01
 #define myChannel 1
@@ -14,6 +13,20 @@
 // RC lib
 #include "juRCReceiver.h"
 
+TaskHandle_t RadioTaskHandle;
+
+void RadioTask(void* parameter) {
+    for (;;) {
+        jRadioLoop();
+        if (rc_serial_available()) {
+            String got = rc_serial_read();
+            Serial.println("received serial: " + got);
+            rc_serial_write("got: " + got);
+        }
+        taskYIELD();
+    }
+}
+
 void onConnect() {}
 void onDisconnect(bool expected) {}
 
@@ -24,17 +37,16 @@ void setup() {
     Serial.println("setup");
     delay(1000);
     jSetup();
+    xTaskCreatePinnedToCore(RadioTask,   /* Function to implement the task */
+                            "RadioTask", /* Name of the task */
+                            10000,       /* Stack size in words */
+                            NULL,        /* Task input parameter */
+                            0,           /* Priority of the task */
+                            &RadioTaskHandle, /* Task handle. */
+                            0); /* Core where the task should run */
 }
 
 void loop() {
     jSensorLoop();
-
     // fly
 }
-/*void loop1() {
-    jRadioLoop();
-    if (rc_serial_available()) {
-        String got = rc_serial_read();
-        rc_serial_write("got: " + got);
-    }
-}*/

juRCReceiver.h

@@ -12,12 +12,15 @@
 #define debug false
 #endif
 
+#ifndef debugReceive
+#define debugReceive false
+#endif
+
 #include <Preferences.h>
 #include <SPI.h>
 #include <Wire.h>
-#include "pthread.h"
-#include <jqueue.h>
 #include <jmutex.h>
+#include <jqueue.h>
 
 //
 
@@ -91,12 +94,27 @@ void jSensorLoop() {
     last = now;
     avg = avg * 0.9 + delta * 0.1;
 #if debug
-    Serial.println("Sensor Loop, avg = " + String(avg));
+    if ((dcounterxy) % 100 == 0)
+        Serial.println("Sensor Loop, avg = " + String(avg));
 #endif
     MPULoop();
     GNSSLoop();
     COMPLoop();
 }
-void jRadioLoop() { handleRC(); }
+void jRadioLoop() {
+#if debug
+    unsigned long now = millis();
+    static long unsigned last = 0;
+    static long unsigned delta = 0;
+    static long unsigned radioLoopCount = 0;
+    static float avg = 2.;
+    delta = now - last;
+    last = now;
+    avg = avg * 0.9 + delta * 0.1;
+    if ((radioLoopCount++) % 200 == 0)
+        Serial.println("Radio Loop, avg = " + String(avg));
+#endif
+    handleRC();
+}
 
 #endif

rcserial.h

@@ -2,10 +2,11 @@
 #define _RC_SERIAL_H
 #define jType_RC_SERIAL 1
 
+#include <jmutex.h>
+#include <jqueue.h>
+
 #include "JuSDK.h"  //https://jusax.de/git/jusax23/JuSDK
 #include "juRCReceiver.h"
-#include "jqueue.h"
-#include "jmutex.h"
 
 typedef struct {
     byte type = 0x01;
@@ -59,7 +60,7 @@ String rc_serial_read() {
 bool rc_serial_write(String inn) {
     mutex_enter_blocking(&serial_mut);
     size_t len = inn.length();
-    if (len + 1 < serialRFBufSize - queue_get_level(&serial_write_queue)) {
+    if (len + 1 > serialRFBufSize - queue_get_level(&serial_write_queue)) {
 #if defined(debug) && debug
         Serial.println("<rfSerial>BufOverflow");
 #endif
@@ -94,7 +95,8 @@ void _handle_serial_send() {
                                   rcSerial.data + (rcSerial.command++));
         }
     }
-    if (jSendNRF(&rcSerial, sizeof(rcSerial))) rcSerial.command = 0;
+    if (rcSerial.command > 0)
+        if (jSendNRF(&rcSerial, sizeof(rcSerial))) rcSerial.command = 0;
     mutex_exit(&serial_mut);
 }
 

system_general/multicast.h

@@ -8,7 +8,7 @@ typedef struct {
 
 void RCMulticastTask() {
     if (random(25) == 1) {
-#ifdef debug
+#if debug
         Serial.println("Multicast!");
 #endif
         radio.stopListening();
@@ -17,11 +17,11 @@ void RCMulticastTask() {
         nrfMulticast data;
         data.pipe = myPipe;
         data.channel = myChannel;
-#ifdef jNAME
-        char nameMult[16] = jNAME;
+#ifdef jName
+        char nameMult[16] = jName;
         strncpy(data.name, nameMult, 16);
 #endif
-        radio.write(&data, sizeof(data), true);
+        radio.write(&data, sizeof(data));
         radio.setChannel(myChannel);
         radio.startListening();
     }

system_general/rc_handler.h

@@ -3,11 +3,11 @@
 #define maxLastRcPackage 2500
 
 #include <SPI.h>
+#include <jmutex.h>
+#include <jqueue.h>
 
 #include "../structsAndDefines.h"
 #include "RF24.h"
-#include "jqueue.h"
-#include "jmutex.h"
 #include "nRF24L01.h"
 
 mutex_t radioAccess;
@@ -27,8 +27,13 @@ void initRC() {
         }
     }
     radio.enableDynamicPayloads();
+    radio.setPALevel(RF24_PA_HIGH);
+    radio.setDataRate(RF24_1MBPS);
     radio.setRetries(5, 15);
     radio.setChannel(myChannel);
+    radio.enableDynamicPayloads();
+    radio.disableAckPayload();
+    radio.setAutoAck(false);
     radio.openReadingPipe(0, myPipe);
     radio.startListening();
     mutex_exit(&radioAccess);
@@ -50,16 +55,16 @@ bool jSendNRF(const void* buf, uint8_t length) {
 #include "multicast.h"
 #include "rc_data.h"
 
-#ifdef debug
+#if debugReceive
 const String b16 = "0123456789abcdef";
 
 String SensorDataBuffer = "";
 #endif
 
-void receiveRC() {
+bool receiveRC() {
     mutex_enter_blocking(&radioAccess);
     if (radio.available()) {
-#ifdef debug
+#if debugReceive
         Serial.println("Radio available!");
 #endif
         uint8_t payloadSize = radio.getDynamicPayloadSize();
@@ -67,7 +72,7 @@ void receiveRC() {
             jLastRCData = millis();
             radio.read(&jRCbuff, payloadSize);
             mutex_exit(&radioAccess);
-#ifdef debug
+#if debugReceive
             SensorDataBuffer = "data: ";
             for (size_t i = 0; i < payloadSize; i++) {
                 SensorDataBuffer += b16[(jRCbuff[i] >> 4) & 0xf];
@@ -92,26 +97,36 @@ void receiveRC() {
                     COMPradioTask();
                 } break;
             }
-        } else
+            return true;
+        } else {
             mutex_exit(&radioAccess);
-    } else
+            return false;
+        }
+    } else {
         mutex_exit(&radioAccess);
+        return false;
+    }
 }
 
 void handleRC() {
-    receiveRC();
+    while (receiveRC())
+        ;
     if (millis() - jLastRCData > maxLastRcPackage) {  // no connection
         if (jLastStatus == 1) {
             if (_j_on_disconnect != nullptr) _j_on_disconnect(true);
             jLastStatus = 3;
+            radio.setAutoAck(false);
         }
+        RCMulticastTask();
     } else {  // connection
         if (jLastStatus != 1) {
             if (_j_on_connect != nullptr) _j_on_connect();
             radio.openWritingPipe(myPipe);
+            radio.setAutoAck(true);
             radio.startListening();
             jLastStatus = 1;
         }
+        _handle_serial_send();
     }
     // todo: multicast, send serial
 }

system_sensors/mpu.h

@@ -49,8 +49,11 @@ void initMPU() {
                           preferences.getFloat("mpuOffAccY", 0.0),
                           preferences.getFloat("mpuOffAccZ", 0.3));*/
 
-    //mpu6050.setGyroOffsets(-7.8, 2.32, -0.22);
-    //mpu6050.setAccXoffset(0.03, 0.0, 0.3);
+    mpu6050.setGyroOffsets(-7.8, 2.32, -0.22);
+#if debug
+    Serial.println("set mpu6050 gyro");
+#endif
+    mpu6050.setAccXoffset(0.03, 0.0, 0.3);
 
 #if debug
     Serial.println("set mpu6050 offsets");