// WizFi360 Micro Drone UDP Receiver Code
// Developed By Ravi Butani (ravi.butani03@gmail.com)
// Released under Creative Commons CC-BY-SA license 
// Used AT Commands for Configure WizFi360 as AP and communicate with Android App Over UDP

// the setup routine runs once when you press reset:
uint8_t batv = 10;
uint32_t premillis = 0;
uint16_t ch1=1500, ch2=1500, ch3=1000, ch4=1500, ch5=1000, ch6=1000; 

void setup() {
  // initialize serial communication at 9600 bits per second:
  Serial.begin(115200);
  setup_wizfi360_wifi_ap();
  delay(10);
}

// the loop routine runs over and over again forever:
void loop() {
  if(read_wizfi360_command(ch1,ch2,ch3,ch4,ch5,ch6)>0)
  {
    Serial.print(ch1);         Serial.print('\t');
    Serial.print(ch2);         Serial.print('\t');
    Serial.print(ch3);         Serial.print('\t');
    Serial.print(ch4);         Serial.print('\t');
    Serial.print(ch5);         Serial.print('\t');
    Serial.print(2000);        Serial.print('\t');
    Serial.println(1000);      
  }
  if((millis()-premillis)>2000)
  {
    premillis = millis();
    send_wizfi360_telemetry(batv);
    batv++;
  }
}

void serial2_Flush(void)
{
  while(Serial2.available()>0)
  {
    char dummy = Serial2.read();
    Serial.write(dummy);
  }
}

void setup_wizfi360_wifi_ap(void)
{
  Serial2.setTX(4);
  Serial2.setRX(5);
  Serial2.begin(115200);

  delay(1000);
  Serial2.println("AT+RST");
  delay(1000);
  serial2_Flush();
  Serial2.println("ATE0");
  delay(400);
  serial2_Flush();
  
  Serial2.println("AT+CWMODE=2");
  delay(400);
  serial2_Flush();
  Serial2.println("AT+CWSAP_CUR=\"Wizfi360Drone\",\"12345678\",5,3,1,0");
  delay(2000);
  serial2_Flush();
  Serial2.println("AT+CIFSR");
  delay(500);
  serial2_Flush();
  Serial2.println("AT+CIPSTART=\"UDP\",\"192.168.36.2\",3333,2390,0");//2
  delay(1000);
  serial2_Flush();
}

void send_wizfi360_telemetry(uint8_t bat)
{
  Serial2.println("AT+CIPSEND=3,\"192.168.36.2\",3333");
  delay(10);
  Serial2.write(0x01);
  Serial2.write(0x78);
  Serial2.write(bat);
}

unsigned int read_wizfi360_command(uint16_t& rch, uint16_t& pch, uint16_t& tch, uint16_t& ych, uint16_t& auxch1, uint16_t& auxch2)
{
  if((Serial2.available()>0) && (Serial2.read() == '+'))
  {
    unsigned int len = 0;
    char packetBuffer[40];
    delayMicroseconds(1500);
    while((Serial2.available()>0)&&(len<40))
    {
      packetBuffer[len]=Serial2.read();
      len++;
    }
    if(len>15)
    {
      auxch1 = packetBuffer[0+7];
      auxch2 = auxch1;
      rch   = ((((uint16_t)packetBuffer[1+7]<<8)+(uint16_t)packetBuffer[2+7]));
      pch   = ((((uint16_t)packetBuffer[3+7]<<8)+(uint16_t)packetBuffer[4+7]));
      tch   = ((((uint16_t)packetBuffer[5+7]<<8)+(uint16_t)packetBuffer[6+7]));
      ych   = ((((uint16_t)packetBuffer[7+7]<<8)+(uint16_t)packetBuffer[8+7]));
      rch   = map(rch,0,592,1000,2000);
      pch   = map(pch,0,592,1000,2000);
      tch   = map(tch,0,592,1000,2000);
      ych   = map(ych,552,40,1000,2000);
      if(auxch1 == 128) auxch1 = 2000;
      else auxch1 = 1000;
      if(auxch2 == 128)auxch2 = 2000;
      else auxch2 = 1000;
    }
    //Serial.println(len);
    serial2_Flush();
    return 1;
  }
  else
  {
    serial2_Flush();
    return 0;
  }
  
}