From f1ed2b8cfc7cbe025f1d394abf10c19903f9044d Mon Sep 17 00:00:00 2001
From: jusax23 <jusax23@gmail.com>
Date: Mon, 22 Aug 2022 12:44:16 +0200
Subject: [PATCH] mpu offsets

---
 handelSerial.h | 92 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 92 insertions(+)

diff --git a/handelSerial.h b/handelSerial.h
index b721350..04c322a 100644
--- a/handelSerial.h
+++ b/handelSerial.h
@@ -1,3 +1,10 @@
+typedef struct{
+  byte type = 1;
+  byte command;
+  float X;
+  float Y;
+  float Z;
+} mpuSendData;
 
 void handelSerial(){
   String serialin = Serial.readStringUntil('\n');
@@ -91,6 +98,91 @@ void handelSerial(){
     }
     return;
   }
+
+  if(command=="MPUoffestSettings"){
+    if (NRFisConnected == 1)
+    {
+      uint16_t buf = 3;
+      uint16_t kind = goonin.toInt();
+      if (kind > 6)
+      {
+        Serial.println("<MPUoffestSett>error");
+        return;
+      }
+      buf |= (kind+0x10) << 8;
+      if (sendRCdata(&buf, 2))
+      {
+        Serial.println("<MPUoffestSett>send");
+      }
+      else
+      {
+        Serial.println("<MPUoffestSett>lost");
+      }
+    }
+    else
+    {
+      Serial.println("<MPUoffestSett>noconn");
+    }
+    return;
+  }
+
+  if(command=="MPUsetOffsetsAcc"){
+    if (NRFisConnected == 1)
+    {
+      mpuSendData buf;
+      buf.command = 0x17;
+
+      unsigned int cutpos = cutcheck(goonin.indexOf("|"), goonin.length());
+      String vX = goonin.substring(0, cutpos);
+      goonin = goonin.substring(cutpos + 1);
+      String vY = goonin.substring(0, cutpos);
+      String vZ = goonin.substring(cutpos + 1);
+
+      buf.X = vX.toFloat();
+      buf.Y = vY.toFloat();
+      buf.Z = vZ.toFloat();
+
+      if (sendRCdata(&buf, 2)){
+        Serial.println("<MPUsetOffsetsAcc>send");
+      }else{
+        Serial.println("<MPUsetOffsetsAcc>lost");
+      }
+    }
+    else
+    {
+      Serial.println("<MPUsetOffsetsAcc>noconn");
+    }
+    return;
+  }
+  if(command=="MPUsetOffsetsGyro"){
+    if (NRFisConnected == 1)
+    {
+      mpuSendData buf;
+      buf.command = 0x18;
+
+      unsigned int cutpos = cutcheck(goonin.indexOf("|"), goonin.length());
+      String vX = goonin.substring(0, cutpos);
+      goonin = goonin.substring(cutpos + 1);
+      String vY = goonin.substring(0, cutpos);
+      String vZ = goonin.substring(cutpos + 1);
+
+      buf.X = vX.toFloat();
+      buf.Y = vY.toFloat();
+      buf.Z = vZ.toFloat();
+
+      if (sendRCdata(&buf, 2)){
+        Serial.println("<MPUsetOffsetsGyro>send");
+      }else{
+        Serial.println("<MPUsetOffsetsGyro>lost");
+      }
+    }
+    else
+    {
+      Serial.println("<MPUsetOffsetsGyro>noconn");
+    }
+    return;
+  }
+
   if(command=="readAnalogDesc"){
     if(NRFisConnected==1){
       uint16_t buf = 50;