SHARE
TWEET

droneid_esp8266

a guest Dec 9th, 2019 123 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <ESP8266WiFi.h>
  2. #include <string.h>
  3. #include <math.h>
  4.  
  5. #define DEG_TO_RAD(DEG) ((DEG) * M_PI / 180.0f)
  6. #define RAD_TO_DEG(RAD) ((RAD) * 180.0f / M_PI)
  7.  
  8. uint8_t wifi_beacon[] =
  9. {
  10.   0x80,                                                   // Version Type/Subtype
  11.   0x00,                                                   // Flags
  12.   0x00, 0x00,                                             // Duration
  13.   0xff, 0xff, 0xff, 0xff, 0xff, 0xff,                     // Destination address
  14.   0x60, 0x60, 0x1f, 0x00, 0x00, 0x00,                     // Source address
  15.   0x60, 0x60, 0x1f, 0x00, 0x00, 0x00,                     // BSS Id
  16.   0x00, 0x00,                                             // Fragment Sequence
  17.   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,         // Timestamp
  18.   0x64, 0x00,                                             // Beacon Interval
  19.   0x31, 0x04,                                             // Capabilities
  20.   // SSID
  21.   0x00, 0x0c, 0x4d, 0x61, 0x76, 0x69, 0x63, 0x2d, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30,
  22.   // Supported Rates
  23.   0x01, 0x08, 0x82, 0x84, 0x8b, 0x0c, 0x12, 0x96, 0x18, 0x24,
  24.   // Current Channel
  25.   0x03, 0x01, 0x01,
  26.   // Traffic Indication Map
  27.   0x05, 0x04, 0x00, 0x01, 0x00, 0x00,
  28.   // Country Information
  29.   0x07, 0x06, 0x55, 0x53, 0x00, 0x01, 0x0b, 0x1e,
  30.   // ERP Information
  31.   0x2a, 0x01, 0x00,
  32.   // Extended Supported Rates
  33.   0x32, 0x04, 0x30, 0x48, 0x60, 0x6c,
  34.   // HT Capabilities
  35.   0x2d, 0x1a, 0xac, 0x01, 0x02, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  36.   // HT Information
  37.   0x3d, 0x16, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  38.   // RSN Information
  39.   0x30, 0x14, 0x01, 0x00, 0x00, 0x0f, 0xac, 0x04, 0x01, 0x00, 0x00, 0x0f, 0xac, 0x04, 0x01, 0x00, 0x00, 0x0f, 0xac, 0x02, 0x0c, 0x00,
  40.   // Vendor Specific: Microsoft Corp.: WMM/WME: Parameter Element
  41.   0xdd, 0x18, 0x00, 0x50, 0xf2, 0x02, 0x01, 0x01, 0x00, 0x00, 0x03, 0xa4, 0x00, 0x00, 0x27, 0xa4, 0x00, 0x00, 0x42, 0x43, 0x5e, 0x00, 0x62, 0x32, 0x2f, 0x00,
  42. };
  43.  
  44. struct droneid_header
  45. {
  46.   uint8_t tag_number;
  47.   uint8_t tag_length;
  48.  
  49.   uint8_t oui[3];
  50.  
  51.   uint8_t vendor_type;
  52.   uint8_t unk1;
  53.   uint8_t unk2;
  54. } __attribute__((packed));
  55.  
  56. struct droneid_flight_reg
  57. {
  58.   droneid_header header;
  59.  
  60.   uint8_t sub_cmd;
  61.   uint8_t ver;
  62.   uint16_t seq;
  63.   uint16_t state_info;
  64.   char sn[16];
  65.   int32_t longitude;
  66.   int32_t latitude;
  67.   int16_t altitude;
  68.   int16_t height;
  69.   int16_t v_north;
  70.   int16_t v_east;
  71.   int16_t v_up;
  72.   int16_t pitch;
  73.   int16_t roll;
  74.   int16_t yaw;
  75.   int32_t longitude_home;
  76.   int32_t latitude_home;
  77.   uint8_t product_type;
  78.   uint8_t uuid_len;
  79.   char uuid[20];
  80. } __attribute__((packed));
  81.  
  82. struct droneid_flight_purpose
  83. {
  84.   droneid_header header;
  85.  
  86.   uint8_t sub_cmd;
  87.   char sn[16];
  88.   uint8_t drone_id_len;
  89.   char drone_id[10];
  90.   uint8_t purpose_len;
  91.   char purpose[100];
  92. } __attribute__((packed));
  93.  
  94. #define SOURCE_ADDRESS 10
  95. #define BSSID_ADDRESS 16
  96. #define SSID 38
  97. #define CURRENT_CHANNEL 62
  98. #define HT_INFO_CHANNEL 116
  99.  
  100. int count = 1;
  101.  
  102. uint64_t macs[] = {0x0100001f6060, 0x0200001f6060, 0x0300001f6060, 0x0400001f6060};
  103. const char *ssids[] = {"Mavic-000001", "Mavic-000002", "Spark-000003", "Spark-000004"};
  104. uint8_t channels[] = {1, 2, 3, 4};
  105.  
  106. uint16_t seq = 0;
  107.  
  108. void update_wifi_beacon(uint64_t mac, const char *ssid, uint8_t channel)
  109. {
  110.   memcpy(&wifi_beacon[SOURCE_ADDRESS], &mac, 6);
  111.   memcpy(&wifi_beacon[BSSID_ADDRESS], &mac, 6);
  112.  
  113.   memcpy(&wifi_beacon[SSID], ssid, 12);
  114.  
  115.   wifi_beacon[CURRENT_CHANNEL] = channel;
  116.   wifi_beacon[HT_INFO_CHANNEL] = channel;
  117. }
  118.  
  119. void update_droneid_header(droneid_header *header, uint8_t length)
  120. {
  121.   header->tag_number = 0xdd;
  122.   header->tag_length = length - 2;
  123.  
  124.   header->oui[0] = 0x26;
  125.   header->oui[1] = 0x37;
  126.   header->oui[2] = 0x12;
  127.  
  128.   header->vendor_type = 0x58;
  129.   header->unk1 = 0x62;
  130.   header->unk2 = 0x13;
  131. }
  132.  
  133. int wifi_send_beacon(uint64_t mac, const char *ssid, uint8_t channel)
  134. {
  135.   wifi_set_channel(channel);
  136.  
  137.   update_wifi_beacon(mac, ssid, channel);
  138.  
  139.   return wifi_send_pkt_freedom(wifi_beacon, sizeof(wifi_beacon), true);
  140. }
  141.  
  142. int wifi_send_droneid(uint64_t mac, const char *ssid, uint8_t channel, void *packet, uint8_t length)
  143. {
  144.   wifi_set_channel(channel);
  145.  
  146.   update_wifi_beacon(mac, ssid, channel);
  147.   update_droneid_header((droneid_header*) packet, length);
  148.  
  149.   uint8_t wifi_droneid[512];
  150.  
  151.   memcpy(wifi_droneid, wifi_beacon, sizeof(wifi_beacon));
  152.   memcpy(wifi_droneid + sizeof(wifi_beacon), packet, length);
  153.  
  154.   return wifi_send_pkt_freedom(wifi_droneid, sizeof(wifi_beacon) + length, true);
  155. }
  156.  
  157. void setup()
  158. {
  159.   Serial.begin(115200);
  160.  
  161.   delay(500);
  162.    
  163.   wifi_set_opmode(STATION_MODE);
  164.   wifi_promiscuous_enable(1);
  165. }
  166.  
  167. void loop()
  168. {
  169.   for (int i = 0; i < count; ++i)
  170.   {
  171.     int result = 0;
  172.    
  173.     droneid_flight_reg flight_reg = {0};
  174.  
  175.     flight_reg.sub_cmd = 0x10;
  176.     flight_reg.ver = 0x01;
  177.     flight_reg.seq = seq;
  178.     flight_reg.state_info = 0x0fff;
  179.     strncpy(flight_reg.sn, "0123456789ABCD", 14);
  180.     flight_reg.longitude = (int32_t) (DEG_TO_RAD(180.0f) * 10000000.0f);
  181.     flight_reg.latitude = (int32_t) (DEG_TO_RAD(90.0f) * 10000000.0f);
  182.     flight_reg.altitude = 200;
  183.     flight_reg.height = 100;
  184.     flight_reg.v_north = 10;
  185.     flight_reg.v_east = 20;
  186.     flight_reg.v_up = 30;
  187.     flight_reg.pitch = (int16_t) (40.0f * 100.0f);
  188.     flight_reg.roll = (int16_t) (50.0f * 100.0f);
  189.     flight_reg.yaw = (int16_t) (60.0f * 100.0f);
  190.     flight_reg.longitude_home = (int32_t) (DEG_TO_RAD(90.0f) * 10000000.0f);
  191.     flight_reg.latitude_home = (int32_t) (DEG_TO_RAD(45.0f) * 10000000.0f);
  192.     flight_reg.product_type = 0x10;
  193.     flight_reg.uuid_len = 18;
  194.     strncpy(flight_reg.uuid, "123456789123456789", 18);
  195.    
  196.     result = wifi_send_droneid(macs[i], ssids[i], channels[i], &flight_reg, sizeof(flight_reg));
  197.  
  198.     Serial.print(i);
  199.     Serial.print("> wifi_send_droneid_flight_reg = ");
  200.     Serial.println(result);
  201.  
  202.     delay(10);
  203.    
  204.     /*
  205.     droneid_flight_purpose flight_purpose = {0};
  206.    
  207.     flight_purpose.sub_cmd = 0x11;
  208.     strncpy(flight_purpose.sn, "0123456789ABCD", 14);
  209.     //flight_purpose.drone_id_len = 10;
  210.     //strncpy(flight_purpose.drone_id, "0123456789", 10);
  211.     //flight_purpose.purpose_len = 4;
  212.     //strncpy(flight_purpose.purpose, "TEST", 4);
  213.    
  214.     result = wifi_send_droneid(macs[i], ssids[i], channels[i], &flight_purpose, sizeof(flight_purpose));
  215.      
  216.     Serial.print(i);
  217.     Serial.print("> wifi_send_droneid_flight_purpose = ");
  218.     Serial.println(result);
  219.  
  220.     delay(10);
  221.     */
  222.   }
  223.  
  224.   ++seq;
  225.  
  226.   delay(100);
  227. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top