Update rcjoystick.c

general/package/rcjoystick/src/rcjoystick.c

@@ -2,16 +2,14 @@
 #include <stdio.h>
 #include <stdbool.h>
 #include <unistd.h>
+#include "string.h"
 #include <linux/joystick.h>
 #include <sys/socket.h>
-#include "string.h"
 #include <errno.h>
 #include <sys/time.h>
 #include <netinet/in.h>
 #include <arpa/inet.h>
 #include "common/mavlink.h"
-#include <time.h>
-#include <stdlib.h> // 用于 atoi 函数
 
 #define BUFFER_LENGTH 2041
 
@@ -114,6 +112,7 @@ int main(int argc, char *argv[])
   
   //time checker
   long long time_check = millis();
+  long long time_check_ping = millis();
   uint16_t send_time = 50;
   struct timespec tw = {0,10};
   struct timespec tr;
@@ -128,9 +127,9 @@ int main(int argc, char *argv[])
   int8_t chan_rxpkts = 0; //default disabled
   int16_t rxpkts_prev = 0, rxpkts = 0, rxpkts_per_second = 0;
   long long rxpkts_time = millis();
+  char wlan_rxpkts[10] = "wlan0";
   
-  
+  while ((opt = getopt(argc, argv, "vd:a:p:t:x:r:i:h")) != -1) {
-  while ((opt = getopt(argc, argv, "vd:a:p:t:x:r:h")) != -1) {
         switch (opt) {
         case 'v':
             verbose = true;
@@ -153,12 +152,19 @@ int main(int argc, char *argv[])
         case 'r':
             chan_rxpkts = atoi(optarg);
             break;
+        case 'i':
+            strcpy(wlan_rxpkts, optarg);
+            break;
         case 'h':
-            printf("rcjoystick by whoim@mail.ru\ncapture usb-hid joystic state and share to mavlink reciever as RC_CHANNELS_OVERRIDE packets\nUsage:\n [-v] verbose;\n [-d device] default '/dev/input/js0';\n [-a addr] ip address send to, default 127.0.0.1;\n [-p port] udp port send to, default 14650;\n [-t time] update RC_CHANNEL_OVERRIDE time in ms, default 50;\n [-x axes_count] 2..9 axes, default 5, other channels mapping to js buttons from button 0;\n");
+            printf("rcjoystick by whoim@mail.ru\ncapture usb-hid joystic state and share to mavlink reciever as RC_CHANNELS_OVERRIDE packets\nUsage:\n [-v] verbose;\n [-d device] default '/dev/input/js0';\n [-a addr] ip address send to, default 127.0.0.1;\n [-p port] udp port send to, default 14650;\n [-t time] update RC_CHANNEL_OVERRIDE time in ms, default 50;\n [-x axes_count] 2..9 axes, default 5, other channels mapping to js buttons from button 0;\n [-r rssi_channel] store rx packets per second value to this channel, default 0 (disabled);\n [-i interface] wlan interface for rx packets statistics, default wlan0;\n");
             return 0;
         }
   }
   
+  char rxpkts_file[100];
+  snprintf(rxpkts_file, sizeof(rxpkts_file), "/sys/class/net/%s/statistics/rx_packets", wlan_rxpkts);
+
+
   while (true) { //loop
     js = open(device, O_RDONLY | O_NONBLOCK);
 
@@ -175,7 +181,7 @@ int main(int argc, char *argv[])
     printf("Update time: %dms\n", send_time);
     printf("UDP: %s:%d\n", inet_ntoa(sin_out.sin_addr), ntohs(sin_out.sin_port));
     printf("Used axes: %d, other channels as buttons\n", axes_count);
-    if(chan_rxpkts > 0) printf("Store rxpkts to channel %d\n", chan_rxpkts);
+    if(chan_rxpkts > 0) printf("Store %s rxpkts to channel %d\n", wlan_rxpkts, chan_rxpkts);
     printf("Started\n");
     
     
@@ -203,30 +209,36 @@ int main(int argc, char *argv[])
         //check rxpkts
         if(chan_rxpkts > 4) {
           if( (long long)rxpkts_time + 1000 < millis() ){
-              FILE* rxpkts_ptr = fopen("/sys/class/net/wlan0/statistics/rx_packets", "r");
+              FILE* rxpkts_ptr = fopen(rxpkts_file, "r");
-              if (rxpkts_ptr == NULL) {
+              if (rxpkts_ptr != NULL) {
-                  printf("error open net statistics");
-                  //return 0;
-              }
                   fscanf(rxpkts_ptr, "%hd", &rxpkts);
                   fclose(rxpkts_ptr);
                   rxpkts_per_second = rxpkts - rxpkts_prev;
-              rxpkts_time = millis();
                   rxpkts_prev = rxpkts;
                   if (verbose) printf("current rx per sec is %d \n", rxpkts_per_second);
+              } else {
+                  printf("Unable to find interface %s\n", wlan_rxpkts);
               }
+              rxpkts_time = millis();
+          }
+        }
+        //send heartbeat every 1 sec
+        if( (long long)time_check_ping + 1000 < millis() ){
+            mavlink_msg_heartbeat_pack(255, 0, &msg, 1, 1, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0, 0);
+            len = mavlink_msg_to_send_buffer(buf, &msg);
+            bytes_sent = sendto(out_sock, buf, len, 0, (struct sockaddr *)&sin_out, sizeof(sin_out));
+            if (verbose) printf("HB Sent %d bytes\n", bytes_sent);
+            time_check_ping = millis();
         }
         //send to udp
         if( (long long)time_check + send_time < millis() ){
-    uint8_t chancount = 18;  // 设置通道数为18
+            uint8_t btnidx = 0;
-    uint8_t rssi = 100;  // 设置RSSI为100
+            mavlink_msg_rc_channels_override_pack(255, 0, &msg, 1, 1,
-    int btnidx = 0; // 初始化为0
-    mavlink_msg_rc_channels_pack(
-        255, 190, &msg, 0, chancount,
                     axes_to_ch(axes[0].x), //ch1
                     axes_to_ch(axes[0].y), //ch2
                     axes_to_ch(axes[1].x), //ch3
                     axes_to_ch(axes[1].y), //ch4
+                    
                     (chan_rxpkts == 5) ? rxpkts_per_second : (axes_count > 2) ? axes_to_ch(axes[2].x) : buttons[btnidx],   //ch5
                     (chan_rxpkts == 6) ? rxpkts_per_second : (axes_count > 2) ? axes_to_ch(axes[2].y) : buttons[btnidx++], //ch6
                     (chan_rxpkts == 7) ? rxpkts_per_second : (axes_count > 3) ? axes_to_ch(axes[3].x) : buttons[btnidx++], //ch7
@@ -240,16 +252,12 @@ int main(int argc, char *argv[])
                     (chan_rxpkts == 15) ? rxpkts_per_second : (axes_count > 7) ? axes_to_ch(axes[7].x) : buttons[btnidx++], //ch15
                     (chan_rxpkts == 16) ? rxpkts_per_second : (axes_count > 7) ? axes_to_ch(axes[7].y) : buttons[btnidx++], //ch16
                     (chan_rxpkts == 17) ? rxpkts_per_second : (axes_count > 8) ? axes_to_ch(axes[8].x) : buttons[btnidx++], //ch17
-        (chan_rxpkts == 18) ? rxpkts_per_second : (axes_count > 8) ? axes_to_ch(axes[8].y) : buttons[btnidx++], // ch18
+                    (chan_rxpkts == 18) ? rxpkts_per_second : (axes_count > 8) ? axes_to_ch(axes[8].y) : buttons[btnidx++]  //ch18
-        rssi // 设置 RSSI
-    );
-    // 更新btnidx，确保按顺序递增
-    btnidx++;
                     
-    if (btnidx >= 14) btnidx = 0; // 确保 btnidx 不越界
+             );
             len = mavlink_msg_to_send_buffer(buf, &msg);
             bytes_sent = sendto(out_sock, buf, len, 0, (struct sockaddr *)&sin_out, sizeof(sin_out));
-    if (verbose) printf("Sent %d bytes\n", bytes_sent);
+            if (verbose) printf("RC Sent %d bytes\n", bytes_sent);
             time_check = millis();
         }
         if (verbose) fflush(stdout);