From 3f7ea70f752ca05f0778181e6470afcababdceb9 Mon Sep 17 00:00:00 2001 From: jusax23 Date: Sun, 17 Sep 2023 13:57:28 +0200 Subject: [PATCH] fixes, debug, radio loop --- JuReceiver.ino | 30 +++++++++++++++++++++--------- juRCReceiver.h | 26 ++++++++++++++++++++++---- rcserial.h | 10 ++++++---- system_general/multicast.h | 8 ++++---- system_general/rc_handler.h | 33 ++++++++++++++++++++++++--------- system_sensors/mpu.h | 7 +++++-- 6 files changed, 82 insertions(+), 32 deletions(-) diff --git a/JuReceiver.ino b/JuReceiver.ino index 4b94ae6..e4117cd 100644 --- a/JuReceiver.ino +++ b/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); - } -}*/ diff --git a/juRCReceiver.h b/juRCReceiver.h index 22b889e..334853c 100644 --- a/juRCReceiver.h +++ b/juRCReceiver.h @@ -12,12 +12,15 @@ #define debug false #endif +#ifndef debugReceive +#define debugReceive false +#endif + #include #include #include -#include "pthread.h" -#include #include +#include // @@ -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 \ No newline at end of file diff --git a/rcserial.h b/rcserial.h index da7b04f..fd9fad3 100644 --- a/rcserial.h +++ b/rcserial.h @@ -2,10 +2,11 @@ #define _RC_SERIAL_H #define jType_RC_SERIAL 1 +#include +#include + #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("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); } diff --git a/system_general/multicast.h b/system_general/multicast.h index 9830b44..4a0b4b1 100644 --- a/system_general/multicast.h +++ b/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(); } diff --git a/system_general/rc_handler.h b/system_general/rc_handler.h index 1a24eeb..bfc51ae 100644 --- a/system_general/rc_handler.h +++ b/system_general/rc_handler.h @@ -3,11 +3,11 @@ #define maxLastRcPackage 2500 #include +#include +#include #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 } diff --git a/system_sensors/mpu.h b/system_sensors/mpu.h index 6167ecd..6f8d70a 100644 --- a/system_sensors/mpu.h +++ b/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");