commit b3ea8f47583d3be862ba05938a71977838250031
Author: jusax23 <jusax23@gmail.com>
Date:   Sun Sep 10 15:33:34 2023 +0200

    try to fix

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..600d2d3
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+.vscode
\ No newline at end of file
diff --git a/JuReceiver.ino b/JuReceiver.ino
new file mode 100644
index 0000000..4b94ae6
--- /dev/null
+++ b/JuReceiver.ino
@@ -0,0 +1,40 @@
+
+
+// RC Config
+#define myId 0x01
+#define myChannel 1
+#define jName "DefaultReceiver"
+
+// activate System-Features
+#define jMPU6050
+#define jCOMP
+#define jGNSS
+#define debug true
+
+// RC lib
+#include "juRCReceiver.h"
+
+void onConnect() {}
+void onDisconnect(bool expected) {}
+
+void setup() {
+    // jSetOnConnect(onConnect);
+    // jSetOnDisconnect(onDisconnect);
+    Serial.begin(115200);
+    Serial.println("setup");
+    delay(1000);
+    jSetup();
+}
+
+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
new file mode 100644
index 0000000..22b889e
--- /dev/null
+++ b/juRCReceiver.h
@@ -0,0 +1,102 @@
+#ifndef _juRCReceiver_h_
+#define _juRCReceiver_h_
+
+#ifndef myId
+#error "myId is not defined"
+#endif
+#ifndef myChannel
+#error "myChannel is not defined"
+#endif
+
+#ifndef debug
+#define debug false
+#endif
+
+#include <Preferences.h>
+#include <SPI.h>
+#include <Wire.h>
+#include "pthread.h"
+#include <jqueue.h>
+#include <jmutex.h>
+
+//
+
+#include "structsAndDefines.h"
+
+on_connect _j_on_connect = nullptr;
+on_disconnect _j_on_disconnect = nullptr;
+on_data _j_on_data = nullptr;
+
+void jSetOnConnect(on_connect conn) { _j_on_connect = conn; }
+void jSetOnDisconnect(on_disconnect conn) { _j_on_disconnect = conn; }
+void jSetOnData(on_data conn) { _j_on_data = conn; }
+
+// 3 (CE)   0  //4 (CSN)  4  //5 (SCK)  18  //6 (MOSI) 23  //7 (MISO) 19
+//         CE CSN
+
+Preferences preferences;
+
+unsigned long last = 0;
+unsigned long delta = 0;
+float avg = 10;
+
+#include "system_general/rc_handler.h"
+
+//
+
+#include "rcserial.h"
+#include "system_sensors/compass.h"
+#include "system_sensors/gnss.h"
+#include "system_sensors/mpu.h"
+
+void jSetup() {
+    Serial.begin(115200);
+#if debug
+    Serial.println("start up");
+#endif
+#if debug
+    bool prefsucc =
+#endif
+        preferences.begin("data", false);
+#if debug
+    if (!prefsucc) Serial.println("prefs false");
+#endif
+    initRC();
+#if debug
+    Serial.println("inited rc");
+#endif
+    Wire.begin();
+#if debug
+    Serial.println("inited wire");
+#endif
+    initMPU();
+#if debug
+    Serial.println("inited mpu");
+#endif
+    initGNSS();
+#if debug
+    Serial.println("inited gnss");
+#endif
+    initCOMP();
+#if debug
+    Serial.println("inited comp");
+#endif
+    _rc_serial_init();
+
+    last = millis();
+}
+void jSensorLoop() {
+    unsigned long now = millis();
+    delta = now - last;
+    last = now;
+    avg = avg * 0.9 + delta * 0.1;
+#if debug
+    Serial.println("Sensor Loop, avg = " + String(avg));
+#endif
+    MPULoop();
+    GNSSLoop();
+    COMPLoop();
+}
+void jRadioLoop() { handleRC(); }
+
+#endif
\ No newline at end of file
diff --git a/rcserial.h b/rcserial.h
new file mode 100644
index 0000000..da7b04f
--- /dev/null
+++ b/rcserial.h
@@ -0,0 +1,101 @@
+#ifndef _RC_SERIAL_H
+#define _RC_SERIAL_H
+#define jType_RC_SERIAL 1
+
+#include "JuSDK.h"  //https://jusax.de/git/jusax23/JuSDK
+#include "juRCReceiver.h"
+#include "jqueue.h"
+#include "jmutex.h"
+
+typedef struct {
+    byte type = 0x01;
+    byte command = 0;
+    char data[30];
+} rcSerialData;
+
+#define serialRFBufSize 2048
+mutex_t serial_mut;
+queue_t serial_rec_queue;
+int serial_lb = 0;
+queue_t serial_write_queue;
+rcSerialData rcSerial;
+
+void _rc_serial_init() {
+    mutex_init(&serial_mut);
+    mutex_enter_blocking(&serial_mut);
+    queue_init(&serial_rec_queue, 1, serialRFBufSize);
+    queue_init(&serial_write_queue, 1, serialRFBufSize);
+    mutex_exit(&serial_mut);
+}
+
+bool rc_serial_available() {
+    mutex_enter_blocking(&serial_mut);
+    bool emp_queue = queue_is_empty(&serial_rec_queue);
+    bool out = serial_lb > 0;
+    if (out && emp_queue) serial_lb = 0;
+    mutex_exit(&serial_mut);
+    return out && !emp_queue;
+}
+
+String rc_serial_read() {
+    mutex_enter_blocking(&serial_mut);
+
+    String out = "";
+
+    if (serial_lb > 0) {
+        serial_lb--;
+        char curr = 0;
+        queue_remove_blocking(&serial_rec_queue, &curr);
+        while (curr != '\n') {
+            out += curr;
+            if (!queue_try_remove(&serial_rec_queue, &curr)) break;
+        }
+    }
+
+    mutex_exit(&serial_mut);
+
+    return out;
+}
+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 defined(debug) && debug
+        Serial.println("<rfSerial>BufOverflow");
+#endif
+        mutex_exit(&serial_mut);
+        return false;
+    } else {
+        const char* data = inn.c_str();
+        for (int i = 0; i < len; i++) {
+            queue_try_add(&serial_write_queue, data + i);
+        }
+        const char lb = '\n';
+        queue_try_add(&serial_write_queue, &lb);
+        mutex_exit(&serial_mut);
+        return true;
+    }
+}
+
+void _rc_serial_add(const char* data, size_t len) {
+    mutex_enter_blocking(&serial_mut);
+    for (size_t i = 0; i < len && i < 30; i++) {
+        queue_try_add(&serial_rec_queue, data + i);
+        if ((char)*(data + i) == '\n') serial_lb++;
+    }
+    mutex_exit(&serial_mut);
+}
+
+void _handle_serial_send() {
+    mutex_enter_blocking(&serial_mut);
+    if (rcSerial.command == 0) {
+        while (rcSerial.command < 30 && !queue_is_empty(&serial_write_queue)) {
+            queue_remove_blocking(&serial_write_queue,
+                                  rcSerial.data + (rcSerial.command++));
+        }
+    }
+    if (jSendNRF(&rcSerial, sizeof(rcSerial))) rcSerial.command = 0;
+    mutex_exit(&serial_mut);
+}
+
+#endif
\ No newline at end of file
diff --git a/readme.md b/readme.md
new file mode 100644
index 0000000..e3d0c33
--- /dev/null
+++ b/readme.md
@@ -0,0 +1,18 @@
+# ju Receiver
+
+## Notes to rf Protocol
+### Types / Commands
+- `0x00 - 0x1F`: General System
+    - `0x00`: Send Data
+    - `0x01`: Serial Data (command represents length: 0-30)
+- `0x20 - 0x3F`: System Sensors
+    - `0x21`: [MPU](#mpu) 
+    - `0x22`: [COMP](#comp) 
+    - `0x23`: [GNSS](#gnss) 
+- `0x40 - 0x5F`: Custom Sensors
+- `0x60 - 0x7F`: Custom
+- `0x80 - 0x9F`: Not Assigned
+- `0xA0 - 0xBF`: Not Assigned
+- `0xC0 - 0xDF`: Not Assigned
+- `0xE0 - 0xFF`: Not connection specific
+    - `0xFF`: Multicast (Connect request from receiver)
\ No newline at end of file
diff --git a/structsAndDefines.h b/structsAndDefines.h
new file mode 100644
index 0000000..6b7cf39
--- /dev/null
+++ b/structsAndDefines.h
@@ -0,0 +1,30 @@
+#ifndef _structsAndDefines_h_
+#define _structsAndDefines_h_
+
+enum jChannel {
+    jChannel_0 = 13,
+    jChannel_1 = 12,
+    jChannel_2 = 14,
+    jChannel_3 = 27,
+    jChannel_4 = 26,
+    jChannel_5 = 25,
+    jChannel_6 = 33,
+    jChannel_7 = 32,
+    jChannel_8 = 15,
+};
+
+enum jType {
+    jServo = 1,
+    jhBridge = 2,
+    jESC = 3,
+    jDigital = 4,
+};
+
+typedef void (*on_connect)();
+typedef void (*on_disconnect)(bool);
+typedef void (*on_data)();
+
+#define multicastChannel 124
+#define multicastAddress 0xF0F0F0F0FF
+
+#endif
\ No newline at end of file
diff --git a/system_general/multicast.h b/system_general/multicast.h
new file mode 100644
index 0000000..9830b44
--- /dev/null
+++ b/system_general/multicast.h
@@ -0,0 +1,28 @@
+
+typedef struct {
+    byte type = 0xff;
+    byte channel;
+    uint64_t pipe;
+    char name[16] = "[UNNAMED]";
+} nrfMulticast;
+
+void RCMulticastTask() {
+    if (random(25) == 1) {
+#ifdef debug
+        Serial.println("Multicast!");
+#endif
+        radio.stopListening();
+        radio.setChannel(multicastChannel);
+        radio.openWritingPipe(multicastAddress);
+        nrfMulticast data;
+        data.pipe = myPipe;
+        data.channel = myChannel;
+#ifdef jNAME
+        char nameMult[16] = jNAME;
+        strncpy(data.name, nameMult, 16);
+#endif
+        radio.write(&data, sizeof(data), true);
+        radio.setChannel(myChannel);
+        radio.startListening();
+    }
+}
\ No newline at end of file
diff --git a/system_general/rc_data.h b/system_general/rc_data.h
new file mode 100644
index 0000000..ddc79f1
--- /dev/null
+++ b/system_general/rc_data.h
@@ -0,0 +1,23 @@
+#define jType_RC 0
+
+#ifndef _J_RC_DATA_H
+#define _J_RC_DATA_H
+
+typedef struct {
+    byte type = 0;
+    byte command;
+    uint16_t btns;
+    int16_t analog[14];
+} nrfRC;
+
+nrfRC RCdata;
+
+void RCdataradioTask() {
+    memmove(&RCdata, &jRCbuff, sizeof(RCdata));
+    if (_j_on_data != nullptr) _j_on_data();
+}
+
+bool RCgetBTN(byte i) { return (RCdata.btns >> min(16 - 1, (int)i)) & 1; }
+int16_t RCgetAnalog(byte i) { return RCdata.analog[min(14 - 1, (int)i)]; }
+
+#endif
\ No newline at end of file
diff --git a/system_general/rc_handler.h b/system_general/rc_handler.h
new file mode 100644
index 0000000..1a24eeb
--- /dev/null
+++ b/system_general/rc_handler.h
@@ -0,0 +1,119 @@
+#ifndef _RC_HANDLER_H
+#define _RC_HANDLER_H
+#define maxLastRcPackage 2500
+
+#include <SPI.h>
+
+#include "../structsAndDefines.h"
+#include "RF24.h"
+#include "jqueue.h"
+#include "jmutex.h"
+#include "nRF24L01.h"
+
+mutex_t radioAccess;
+
+RF24 radio(0, 4);
+const uint64_t myPipe = 0xF0F0F0F000 | myId;
+byte jRCbuff[32];
+unsigned long jLastRCData = 0;
+byte jLastStatus = 0;
+
+void initRC() {
+    mutex_init(&radioAccess);
+    mutex_enter_blocking(&radioAccess);
+    if (!radio.begin()) {
+        Serial.println("radio hardware is not responding!!");
+        while (1) {
+        }
+    }
+    radio.enableDynamicPayloads();
+    radio.setRetries(5, 15);
+    radio.setChannel(myChannel);
+    radio.openReadingPipe(0, myPipe);
+    radio.startListening();
+    mutex_exit(&radioAccess);
+}
+
+bool jSendNRF(const void* buf, uint8_t length) {
+    mutex_enter_blocking(&radioAccess);
+    radio.stopListening();
+    bool succ = radio.write(buf, length);
+    radio.startListening();
+    mutex_exit(&radioAccess);
+    return succ;
+}
+
+#include "../rcserial.h"
+#include "../system_sensors/compass.h"
+#include "../system_sensors/gnss.h"
+#include "../system_sensors/mpu.h"
+#include "multicast.h"
+#include "rc_data.h"
+
+#ifdef debug
+const String b16 = "0123456789abcdef";
+
+String SensorDataBuffer = "";
+#endif
+
+void receiveRC() {
+    mutex_enter_blocking(&radioAccess);
+    if (radio.available()) {
+#ifdef debug
+        Serial.println("Radio available!");
+#endif
+        uint8_t payloadSize = radio.getDynamicPayloadSize();
+        if (payloadSize >= 1) {
+            jLastRCData = millis();
+            radio.read(&jRCbuff, payloadSize);
+            mutex_exit(&radioAccess);
+#ifdef debug
+            SensorDataBuffer = "data: ";
+            for (size_t i = 0; i < payloadSize; i++) {
+                SensorDataBuffer += b16[(jRCbuff[i] >> 4) & 0xf];
+                SensorDataBuffer += b16[jRCbuff[i] & 0xf];
+            }
+            Serial.println(SensorDataBuffer);
+#endif
+            switch (jRCbuff[0]) {
+                case jType_RC: {
+                    RCdataradioTask();
+                } break;
+                case jType_RC_SERIAL: {
+                    _rc_serial_add((char*)(jRCbuff + 2), jRCbuff[1]);
+                } break;
+                case jType_S_MPU: {
+                    MPUradioTask();
+                } break;
+                case jType_S_GNSS: {
+                    GNSSradioTask();
+                } break;
+                case jType_S_COMP: {
+                    COMPradioTask();
+                } break;
+            }
+        } else
+            mutex_exit(&radioAccess);
+    } else
+        mutex_exit(&radioAccess);
+}
+
+void handleRC() {
+    receiveRC();
+    if (millis() - jLastRCData > maxLastRcPackage) {  // no connection
+        if (jLastStatus == 1) {
+            if (_j_on_disconnect != nullptr) _j_on_disconnect(true);
+            jLastStatus = 3;
+        }
+    } else {  // connection
+        if (jLastStatus != 1) {
+            if (_j_on_connect != nullptr) _j_on_connect();
+            radio.openWritingPipe(myPipe);
+            radio.startListening();
+            jLastStatus = 1;
+        }
+    }
+    // todo: multicast, send serial
+}
+
+#endif
\ No newline at end of file
diff --git a/system_sensors/compass.h b/system_sensors/compass.h
new file mode 100644
index 0000000..a33d5a7
--- /dev/null
+++ b/system_sensors/compass.h
@@ -0,0 +1,56 @@
+#ifndef _J_SYS_SENS_COMPASS_H
+#define _J_SYS_SENS_COMPASS_H
+
+#define jType_S_COMP 0x22
+#define jCommand_COMP_fail 0xff
+// #define jCommand_Compass_Raw 0
+
+typedef struct {
+    byte type = jType_S_COMP;
+    byte command = 0;
+    int X;
+    int Y;
+    int Z;
+    int angle;
+} compData;
+
+#ifdef jCOMP
+
+#include <QMC5883LCompass.h>
+QMC5883LCompass compass;
+
+void initCOMP() { compass.init(); }
+
+void COMPLoop() { compass.read(); }
+
+/*float Altitude = mpu6050.getAngleX()/180*PI;
+float Roll = mpu6050.getAngleY()/180*PI;
+float xh=compass.getX()*cos(Altitude)+compass.getZ()*sin(Altitude);
+float
+yh=compass.getX()*sin(Roll)*sin(Altitude)+compass.getY()*cos(Roll)-compass.getZ()*sin(Roll)*cos(Altitude);
+float heading = atan2(yh, xh);*/
+
+void COMPradioTask() {
+    compData data;
+    data.X = compass.getX();
+    data.Y = compass.getY();
+    data.Z = compass.getZ();
+    data.angle = compass.getAzimuth();
+    jSendNRF(&data, sizeof(data));
+}
+
+#else
+
+void initCOMP() {}
+
+void COMPLoop() {}
+
+void MPUradioTask() {
+    compData data;
+    data.command = jCommand_COMP_fail;
+    jSendNRF(&data, sizeof(data));
+}
+
+#endif
+
+#endif
\ No newline at end of file
diff --git a/system_sensors/gnss.h b/system_sensors/gnss.h
new file mode 100644
index 0000000..5c191b6
--- /dev/null
+++ b/system_sensors/gnss.h
@@ -0,0 +1,200 @@
+#ifndef _J_SYS_SENS_GNSS_H
+#define _J_SYS_SENS_GNSS_H
+
+#define jType_S_GNSS 0x23
+#define jCommand_GNSS_location 0
+#define jCommand_GNSS_date 1
+#define jCommand_GNSS_time 2
+#define jCommand_GNSS_speed 3
+#define jCommand_GNSS_course 4
+#define jCommand_GNSS_altitude 5
+#define jCommand_GNSS_satellites 6
+#define jCommand_GNSS_hdop 7
+
+#define jCommand_GNSS_fail 0xff
+
+typedef struct {
+    byte type = jType_S_GNSS;
+    byte command = jCommand_GNSS_location;
+
+    byte isValid;
+
+    double lat;
+    double lng;
+} gnssLocData;
+typedef struct {
+    byte type = jType_S_GNSS;
+    byte command = jCommand_GNSS_date;
+
+    byte isValid;
+
+    uint32_t value;
+    uint16_t year;
+    uint8_t month;
+    uint8_t day;
+} gnssDateData;
+typedef struct {
+    byte type = jType_S_GNSS;
+    byte command = jCommand_GNSS_time;
+
+    byte isValid;
+
+    uint32_t value;
+    uint8_t hour;
+    uint8_t minute;
+    uint8_t second;
+    uint8_t centisecond;
+} gnssTimeData;
+typedef struct {
+    byte type = jType_S_GNSS;
+    byte command = jCommand_GNSS_speed;
+
+    byte isValid;
+
+    double mps;
+} gnssSpeedData;
+typedef struct {
+    byte type = jType_S_GNSS;
+    byte command = jCommand_GNSS_course;
+
+    byte isValid;
+
+    double deg;
+} gnssCourseData;
+typedef struct {
+    byte type = jType_S_GNSS;
+    byte command = jCommand_GNSS_altitude;
+
+    byte isValid;
+
+    double m;
+} gnssAltData;
+typedef struct {
+    byte type = jType_S_GNSS;
+    byte command = jCommand_GNSS_satellites;
+
+    byte isValid;
+
+    uint32_t n;
+} gnssSatData;
+typedef struct {
+    byte type = jType_S_GNSS;
+    byte command = jCommand_GNSS_hdop;
+
+    byte isValid;
+
+    double hdop;
+} gnssHdopData;
+typedef struct {
+    byte type = jType_S_GNSS;
+    byte command = jCommand_GNSS_fail;
+} gnssFailData;
+
+#ifdef jGNSS
+
+#include <TinyGPS++.h>  //TinyGPSPlus
+
+TinyGPSPlus gps;
+
+void initGNSS() { Serial2.begin(9600, SERIAL_8N1, 16, 17); }
+
+void GNSSLoop() {
+    while (Serial2.available() > 0) {
+        int readd = Serial2.read();
+        gps.encode(readd);
+    }
+}
+
+void GNSSradioTask() {
+    switch (jRCbuff[1]) {
+        case jCommand_GNSS_location: {
+            gnssLocData data;
+
+            data.isValid =
+                gps.location.isValid() | (gps.location.isUpdated() << 1);
+            data.lat = gps.location.lat();
+            data.lng = gps.location.lng();
+
+            jSendNRF(&data, sizeof(data));
+        } break;
+        case jCommand_GNSS_date: {
+            gnssDateData data;
+
+            data.isValid = gps.date.isValid() | (gps.date.isUpdated() << 1);
+            data.value = gps.date.value();
+            data.year = gps.date.year();
+            data.month = gps.date.month();
+            data.day = gps.date.day();
+
+            jSendNRF(&data, sizeof(data));
+        } break;
+        case jCommand_GNSS_time: {
+            gnssTimeData data;
+
+            data.isValid = gps.time.isValid() | (gps.time.isUpdated() << 1);
+            data.value = gps.time.value();
+            data.hour = gps.time.hour();
+            data.minute = gps.time.minute();
+            data.second = gps.time.second();
+            data.centisecond = gps.time.centisecond();
+
+            jSendNRF(&data, sizeof(data));
+        } break;
+        case jCommand_GNSS_speed: {
+            gnssSpeedData data;
+
+            data.isValid = gps.speed.isValid() | (gps.speed.isUpdated() << 1);
+            data.mps = gps.speed.mps();
+
+            jSendNRF(&data, sizeof(data));
+        } break;
+        case jCommand_GNSS_course: {
+            gnssCourseData data;
+
+            data.isValid = gps.course.isValid() | (gps.course.isUpdated() << 1);
+            data.deg = gps.course.deg();
+
+            jSendNRF(&data, sizeof(data));
+        } break;
+        case jCommand_GNSS_altitude: {
+            gnssAltData data;
+
+            data.isValid =
+                gps.altitude.isValid() | (gps.altitude.isUpdated() << 1);
+            data.m = gps.altitude.meters();
+
+            jSendNRF(&data, sizeof(data));
+        } break;
+        case jCommand_GNSS_satellites: {
+            gnssSatData data;
+
+            data.isValid =
+                gps.satellites.isValid() | (gps.satellites.isUpdated() << 1);
+            data.n = gps.satellites.value();
+
+            jSendNRF(&data, sizeof(data));
+        } break;
+        case jCommand_GNSS_hdop: {
+            gnssHdopData data;
+
+            data.isValid = gps.hdop.isValid() | (gps.hdop.isUpdated() << 1);
+            data.hdop = gps.hdop.hdop();
+
+            jSendNRF(&data, sizeof(data));
+        } break;
+    }
+}
+
+#else
+
+void initGNSS() {}
+
+void GNSSLoop() {}
+
+void GNSSradioTask() {
+    gnssFailData data;
+    jSendNRF(&data, sizeof(data));
+}
+
+#endif
+#endif
diff --git a/system_sensors/mpu.h b/system_sensors/mpu.h
new file mode 100644
index 0000000..6167ecd
--- /dev/null
+++ b/system_sensors/mpu.h
@@ -0,0 +1,209 @@
+#ifndef _J_SYS_SENS_MPU_H
+#define _J_SYS_SENS_MPU_H
+
+#define jType_S_MPU 0x21
+#define jCommand_MPU_Acc 0
+#define jCommand_MPU_Gyro 1
+#define jCommand_MPU_Angle 2
+
+#define jCommand_MPU_reset_offset_calc 0x10
+
+#define jCommand_MPU_set_calc_offsets_acc 0x11
+#define jCommand_MPU_set_calc_offsets_gyro 0x12
+
+#define jCommand_MPU_get_calc_offsets_acc 0x13
+#define jCommand_MPU_get_calc_offsets_gyro 0x14
+
+#define jCommand_MPU_get_offsets_acc 0x15
+#define jCommand_MPU_get_offsets_gyro 0x16
+
+#define jCommand_MPU_set_offsets_acc 0x17
+#define jCommand_MPU_set_offsets_gyro 0x18
+
+#define jCommand_MPU_fail 0xff
+
+typedef struct {
+    byte type = jType_S_MPU;
+    byte command;
+    float X;
+    float Y;
+    float Z;
+} mpuSendData;
+
+#ifdef jMPU6050
+
+#include <MPU6050_jusax23.h>
+
+MPU6050 mpu6050(Wire, 0.001f, 0.999f);
+
+void initMPU() {
+    mpu6050.begin();
+#if debug
+    Serial.println("began mpu6050");
+#endif
+
+    /*mpu6050.setGyroOffsets(preferences.getFloat("mpuOffGyrX", -7.8),
+                           preferences.getFloat("mpuOffGyrY", 2.32),
+                           preferences.getFloat("mpuOffGyrZ", -0.22));
+    mpu6050.setAccXoffset(preferences.getFloat("mpuOffAccX", 0.03),
+                          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);
+
+#if debug
+    Serial.println("set mpu6050 offsets");
+#endif
+    // mpu6050.calcGyroOffsets(false,50,50);
+}
+
+#if debug
+int dcounterxy = 0;
+#endif
+
+void MPULoop() {
+    mpu6050.update();
+#if debug
+    if ((dcounterxy++) % 100 == 0) {
+        Serial.print("AccX: ");
+        Serial.print(mpu6050.getAccX());
+        Serial.print(" AccY: ");
+        Serial.print(mpu6050.getAccY());
+        Serial.print(" AccZ: ");
+        Serial.print(mpu6050.getAccZ());
+        Serial.println();
+        Serial.print("GyroX: ");
+        Serial.print(mpu6050.getGyroX());
+        Serial.print(" GyroY: ");
+        Serial.print(mpu6050.getGyroY());
+        Serial.print(" GyroZ: ");
+        Serial.print(mpu6050.getGyroZ());
+        Serial.println();
+        Serial.print("AngleX: ");
+        Serial.print(mpu6050.getAngleX());
+        Serial.print(" AngleY: ");
+        Serial.print(mpu6050.getAngleY());
+        Serial.print(" AngleZ: ");
+        Serial.print(mpu6050.getAngleZ());
+        Serial.println();
+        Serial.println();
+    }
+#endif
+}
+
+void MPUradioTask() {
+    switch (jRCbuff[1]) {
+        case jCommand_MPU_Acc: {
+            mpuSendData data;
+            data.command = jCommand_MPU_Acc;
+            data.X = mpu6050.getAccX();
+            data.Y = mpu6050.getAccY();
+            data.Z = mpu6050.getAccZ();
+            jSendNRF(&data, sizeof(data));
+        } break;
+        case jCommand_MPU_Gyro: {
+            mpuSendData data;
+            data.command = jCommand_MPU_Gyro;
+            data.X = mpu6050.getGyroX();
+            data.Y = mpu6050.getGyroY();
+            data.Z = mpu6050.getGyroZ();
+            jSendNRF(&data, sizeof(data));
+        } break;
+        case jCommand_MPU_Angle: {
+            mpuSendData data;
+            data.command = jCommand_MPU_Angle;
+            data.X = mpu6050.getAngleX();
+            data.Y = mpu6050.getAngleY();
+            data.Z = mpu6050.getAngleZ();
+            jSendNRF(&data, sizeof(data));
+        } break;
+
+        case jCommand_MPU_reset_offset_calc: {
+            mpu6050.restGyroOffsetGlide();
+            mpu6050.resetAccOffsetWatch();
+        } break;
+        case jCommand_MPU_set_calc_offsets_acc: {
+            float offX = mpu6050.calcAccXoffset();
+            float offY = mpu6050.calcAccYoffset();
+            float offZ = mpu6050.calcAccZoffset();
+            preferences.putFloat("mpuOffAccX", offX);
+            preferences.putFloat("mpuOffAccY", offY);
+            preferences.putFloat("mpuOffAccZ", offZ);
+            mpu6050.setAccXoffset(offX, offY, offZ);
+        } break;
+        case jCommand_MPU_set_calc_offsets_gyro: {
+            float offX = mpu6050.getGyroXoffsetGlide();
+            float offY = mpu6050.getGyroYoffsetGlide();
+            float offZ = mpu6050.getGyroZoffsetGlide();
+            preferences.putFloat("mpuOffGyrX", offX);
+            preferences.putFloat("mpuOffGyrY", offY);
+            preferences.putFloat("mpuOffGyrZ", offZ);
+            mpu6050.setGyroOffsets(offX, offY, offZ);
+        } break;
+        case jCommand_MPU_get_calc_offsets_acc: {
+            mpuSendData data;
+            data.command = jCommand_MPU_get_calc_offsets_acc;
+            data.X = mpu6050.calcAccXoffset();
+            data.Y = mpu6050.calcAccYoffset();
+            data.Z = mpu6050.calcAccZoffset();
+            jSendNRF(&data, sizeof(data));
+        } break;
+        case jCommand_MPU_get_calc_offsets_gyro: {
+            mpuSendData data;
+            data.command = jCommand_MPU_get_calc_offsets_gyro;
+            data.X = mpu6050.getGyroXoffsetGlide();
+            data.Y = mpu6050.getGyroYoffsetGlide();
+            data.Z = mpu6050.getGyroZoffsetGlide();
+            jSendNRF(&data, sizeof(data));
+        } break;
+        case jCommand_MPU_set_offsets_acc: {
+            mpuSendData data;
+            memmove(&data, &jRCbuff, sizeof(data));
+            preferences.putFloat("mpuOffGyrX", data.X);
+            preferences.putFloat("mpuOffGyrY", data.Y);
+            preferences.putFloat("mpuOffGyrZ", data.Z);
+            mpu6050.setGyroOffsets(data.X, data.Y, data.Z);
+        } break;
+        case jCommand_MPU_set_offsets_gyro: {
+            mpuSendData data;
+            memmove(&data, &jRCbuff, sizeof(data));
+            preferences.putFloat("mpuOffAccX", data.X);
+            preferences.putFloat("mpuOffAccY", data.Y);
+            preferences.putFloat("mpuOffAccZ", data.Z);
+            mpu6050.setAccXoffset(data.X, data.Y, data.Z);
+        } break;
+
+        case jCommand_MPU_get_offsets_acc: {
+            mpuSendData data;
+            data.command = jCommand_MPU_get_offsets_acc;
+            data.X = preferences.getFloat("mpuOffGyrX", -7.8);
+            data.Y = preferences.getFloat("mpuOffGyrY", 2.32);
+            data.Z = preferences.getFloat("mpuOffGyrZ", -0.22);
+            jSendNRF(&data, sizeof(data));
+        } break;
+        case jCommand_MPU_get_offsets_gyro: {
+            mpuSendData data;
+            data.command = jCommand_MPU_get_offsets_gyro;
+            data.X = preferences.getFloat("mpuOffAccX", 0.03);
+            data.Y = preferences.getFloat("mpuOffAccY", 0.0);
+            data.Z = preferences.getFloat("mpuOffAccZ", 0.3);
+            jSendNRF(&data, sizeof(data));
+        }
+    }
+}
+
+#else
+
+void initMPU() {}
+
+void MPULoop() {}
+
+void MPUradioTask() {
+    mpuSendData data;
+    data.command = jCommand_MPU_fail;
+    jSendNRF(&data, sizeof(data));
+}
+
+#endif
+#endif
\ No newline at end of file