PadController/handelSerial.h

typedef struct{
  byte type = 1;
  byte command;
  float X;
  float Y;
  float Z;
} mpuSendData;

void handelSerial(){
  String serialin = Serial.readStringUntil('\n');
  if(rpiStatus == rpi_on){
    rpiSince = millis();
  }
  if(rpiStatus == rpi_starting){
    rpiStatus = rpi_on;
    rpiSince = millis();
  }
  if(!serialin.startsWith("<"))return;
  String command = serialin.substring(1,serialin.indexOf(">"));
  String goonin = serialin.substring(serialin.indexOf(">")+1);
  if(command == "ping"){
    Serial.println("<ok>");
    return;
  }
  if(command=="setBLE"){
    changeMode(mode_bluetooth);
    return;
  }
  if(command=="setNRF"){
    changeMode(mode_radio);
    //goonin == "0"  //idel
    if(goonin=="1"){ //scan

    }
    if(goonin=="2"){ //auto connect

    }
    return;
  }
  if(command=="readAnalogRaw"){
    Serial.println(readAnalog(hextonum(goonin),true));
  }
  if(command=="readAnalog"){
    Serial.println(readAnalog(hextonum(goonin)));
  }
  if(command=="setDEBUG"){
    changeMode(mode_debug);
    return;
  }
  if(command=="readMPU"){
    if(NRFisConnected==1){
      uint16_t buf = 1;
      uint16_t kind = goonin.toInt();
      if(kind>2){
        Serial.println("<readMPU>error");
        return;
      }
      buf |= kind<<8;
      if(sendRCdata(&buf,2)){
        Serial.println("<readMPU>send");
      }else{
        Serial.println("<readMPU>lost");
      }
    }else{
      Serial.println("<readMPU>noconn");
    }
    return;
  }
  if(command=="readCOMP"){
    if(NRFisConnected==1){
      uint16_t buf = 2;
      if(sendRCdata(&buf,2)){
        Serial.println("<readCOMP>send");
      }else{
        Serial.println("<readCOMP>lost");
      }
    }else{
      Serial.println("<readCOMP>noconn");
    }
    return;
  }
  if(command=="readGNSS"){
    if(NRFisConnected==1){
      uint16_t buf = 3;
      uint16_t kind = goonin.toInt();
      if(kind>7){
        Serial.println("<readGNSS>error");
        return;
      }
      buf |= kind<<8;
      if(sendRCdata(&buf,2)){
        Serial.println("<readGNSS>send");
      }else{
        Serial.println("<readGNSS>lost");
      }
    }else{
      Serial.println("<readGNSS>noconn");
    }
    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;
      uint16_t kind = goonin.toInt();
      if(kind>11){
        Serial.println("<readAnalogDesc>error");
        return;
      }
      buf |= kind<<8;
      if(sendRCdata(&buf,2)){
        Serial.println("<readAnalogDesc>send");
      }else{
        Serial.println("<readAnalogDesc>lost");
      }
    }else{
      Serial.println("<readAnalogDesc>noconn");
    }
    return;
  }
  if(command=="readButtonDesc"){
    if(NRFisConnected==1){
      uint16_t buf = 51;
      uint16_t kind = goonin.toInt();
      if(kind>31){
        Serial.println("<readButtonDesc>error");
        return;
      }
      buf |= kind<<8;
      if(sendRCdata(&buf,2)){
        Serial.println("<readButtonDesc>send");
      }else{
        Serial.println("<readButtonDesc>lost");
      }
    }else{
      Serial.println("<readButtonDesc>noconn");
    }
    return;
  }
  if(command=="getARC"){
    if(NRFisConnected==1){
      Serial.print("<getARC>");
      Serial.println(radio.getARC());
    }else{
      Serial.println("<getARC>noconn");
    }
    return;
  }
  if(command=="loadCalibrationData"){
    loadCalibrationData(goonin);
  }
  if(command=="loadLEDProgram"){
    loadLEDProgram(goonin);
  }
  if(command=="putBLEProgram0"){
    BLEProgramCache[0] = goonin;
    preferences.putString("BLEPchache_0",goonin);
  }
  if(command=="putBLEProgram1"){
    BLEProgramCache[1] = goonin;
    preferences.putString("BLEPchache_1",goonin);
  }
  if(command=="putBLEProgram2"){
    BLEProgramCache[2] = goonin;
    preferences.putString("BLEPchache_2",goonin);
  }
  if(command=="putBLEProgram3"){
    BLEProgramCache[3] = goonin;
    preferences.putString("BLEPchache_3",goonin);
  }
  if(command=="putBLEProgram4"){
    BLEProgramCache[4] = goonin;
    preferences.putString("BLEPchache_4",goonin);
  }
  if(command=="BLEnextProgram"){
    BLEnextProgram();
  }
  if(command=="BLEgetProgramId"){
    Serial.print("<BLEgetProgramId>");
    Serial.println(BLEProgramActive);
  }
  if(command=="BLEsetProgramId"){
    Serial.print("<BLEgetProgramId>");
    BLEProgramActive = goonin.toInt()-1;
    BLEnextProgram();

  }
  if(command=="reset"){
    preferences.clear();
    ESP.restart();
  }
}