15 Compatible With ESPilot APP

一、ESPilot APP通信协议

ESPlane	Direction	ESPilot App	function
192.168.43.42::2390/2392	UDP双向	2399	typr

byte	function
1	head
2-12	data

head	function
0x04	PID Received from App Store in EEPROM
0x08	App requested for PID
else	Control Packet Received

/*Control Packet Received*/
          rch   = (-1.0)*(float)((((uint16_t)packetBuffer[1]<<8)+(uint16_t)packetBuffer[2])-296);
          pch   = (-1.0)*(float)((((uint16_t)packetBuffer[3]<<8)+(uint16_t)packetBuffer[4])-296);
          tch   = (float)((((uint16_t)packetBuffer[5]<<8)+(uint16_t)packetBuffer[6])+200);
          ych   = (float)((((uint16_t)packetBuffer[7]<<8)+(uint16_t)packetBuffer[8])-296);

/*PID Received from App Store in EEPROM*/
          pid_p_gain_roll = (float)packetBuffer[1]/10; pid_i_gain_roll = (float)packetBuffer[2]/100; pid_d_gain_roll = (float)packetBuffer[3]/10;              
          pid_p_gain_pitch = (float)packetBuffer[4]/10; pid_i_gain_pitch = (float)packetBuffer[5]/100; pid_d_gain_pitch = (float)packetBuffer[6]/10;
          pid_p_gain_yaw = (float)packetBuffer[7]/10; pid_i_gain_yaw = (float)packetBuffer[8]/100; pid_d_gain_yaw = (float)packetBuffer[9]/10;

/*App requested for PID*/

          packetBuffer[1] = (uint8_t)(pid_p_gain_roll*10); packetBuffer[2] = (uint8_t)(pid_i_gain_roll*100); packetBuffer[3] = (uint8_t)(pid_d_gain_roll*10);
          packetBuffer[4] = (uint8_t)(pid_p_gain_pitch*10); packetBuffer[5] = (uint8_t)(pid_i_gain_pitch*100); packetBuffer[6] = (uint8_t)(pid_d_gain_pitch*10);
          packetBuffer[7] = (uint8_t)(pid_p_gain_yaw*10); packetBuffer[8] = (uint8_t)(pid_i_gain_yaw*100); packetBuffer[9] = (uint8_t)(pid_d_gain_yaw*10);
          packetBuffer[0]=0x08;
          V_bat = ((((float)analogRead(BATV_PIN)*3.17/4095.0) + BATV_OFFSET)*2);  // Measure Battery Voltage
          packetBuffer[10] = (unsigned char)(120);//(abs(WiFi.RSSI()));
          packetBuffer[11] = (unsigned char)(V_bat*10); //

二、飞机端代码移植

1. 检测数据包是否来自旧版本ESPilot APP

static bool detectOldVersionApp(UDPPacket *in){

  if ((in->data)[0]!=0x00&&in->size == 11)//12-1
  {
    if ((in->data)[0]==0x80&&(in->data)[9]==0x00&&(in->data)[10]==0x00&&(in->data)[11]==0x00)
    {
      isOldVersionApp = true;
      return true;
    }

  }
  isOldVersionApp = false;
  return false;
}

2. 将ESPilot 协议包转成CRTP协议包

if(detectOldVersionApp(&wifiIn)){

      rch  = (1.0)*(float)(((((uint16_t)wifiIn.data[1]<<8)+(uint16_t)wifiIn.data[2])-296)*15.0/150.0); //-15~+15
      pch  = (-1.0)*(float)(((((uint16_t)wifiIn.data[3]<<8)+(uint16_t)wifiIn.data[4])-296)*15.0/150.0);//-15~+15
      tch  = (((uint16_t)wifiIn.data[5]<<8)+(uint16_t)wifiIn.data[6])*59000.0/600.0;
      ych  = (float)(((((uint16_t)wifiIn.data[7]<<8)+(uint16_t)wifiIn.data[8])-296)*15.0/150.0);//-15~+15
      p.size = wifiIn.size + 1 ; //把cksum字节加上
      p.header = CRTP_HEADER(CRTP_PORT_SETPOINT, 0x00); //重新定义头

      memcpy(&p.data[0], &rch, 4);
      memcpy(&p.data[4], &pch, 4);  
      memcpy(&p.data[8], &ych, 4); 
      memcpy(&p.data[12],&tch, 2);   
      ESP_LOGD(TAG,"OldVersionApp"); 

    }else
    {
      p.size = wifiIn.size - 1;//CRTP中的size是去掉head的大小
      memcpy(&p.raw, wifiIn.data, wifiIn.size);  
    }