diff --git a/general/package/rcjoystick/src/rcjoystick.c b/general/package/rcjoystick/src/rcjoystick.c index c6ad972d..4bb7946b 100644 --- a/general/package/rcjoystick/src/rcjoystick.c +++ b/general/package/rcjoystick/src/rcjoystick.c @@ -2,16 +2,14 @@ #include #include #include +#include "string.h" #include #include -#include "string.h" #include #include #include #include #include "common/mavlink.h" -#include -#include // 用于 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:h")) != -1) { + while ((opt = getopt(argc, argv, "vd:a:p:t:x:r:i:h")) != -1) { switch (opt) { case 'v': verbose = true; @@ -153,11 +152,18 @@ 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,55 +209,57 @@ 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"); - if (rxpkts_ptr == NULL) { - printf("error open net statistics"); - //return 0; + FILE* rxpkts_ptr = fopen(rxpkts_file, "r"); + if (rxpkts_ptr != NULL) { + fscanf(rxpkts_ptr, "%hd", &rxpkts); + fclose(rxpkts_ptr); + rxpkts_per_second = rxpkts - rxpkts_prev; + 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); } - 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); } } + //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 rssi = 100; // 设置RSSI为100 - 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 - (chan_rxpkts == 8) ? rxpkts_per_second : (axes_count > 3) ? axes_to_ch(axes[3].y) : buttons[btnidx++], // ch8 - (chan_rxpkts == 9) ? rxpkts_per_second : (axes_count > 4) ? axes_to_ch(axes[4].x) : buttons[btnidx++], // ch9 - (chan_rxpkts == 10) ? rxpkts_per_second : (axes_count > 4) ? axes_to_ch(axes[4].y) : buttons[btnidx++], // ch10 - (chan_rxpkts == 11) ? rxpkts_per_second : (axes_count > 5) ? axes_to_ch(axes[5].x) : buttons[btnidx++], // ch11 - (chan_rxpkts == 12) ? rxpkts_per_second : (axes_count > 5) ? axes_to_ch(axes[5].y) : buttons[btnidx++], // ch12 - (chan_rxpkts == 13) ? rxpkts_per_second : (axes_count > 6) ? axes_to_ch(axes[6].x) : buttons[btnidx++], // ch13 - (chan_rxpkts == 14) ? rxpkts_per_second : (axes_count > 6) ? axes_to_ch(axes[6].y) : buttons[btnidx++], // ch14 - (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 - 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); - time_check = millis(); -} + if( (long long)time_check + send_time < millis() ){ + uint8_t btnidx = 0; + mavlink_msg_rc_channels_override_pack(255, 0, &msg, 1, 1, + 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 + (chan_rxpkts == 8) ? rxpkts_per_second : (axes_count > 3) ? axes_to_ch(axes[3].y) : buttons[btnidx++], //ch8 + (chan_rxpkts == 9) ? rxpkts_per_second : (axes_count > 4) ? axes_to_ch(axes[4].x) : buttons[btnidx++], //ch9 + (chan_rxpkts == 10) ? rxpkts_per_second : (axes_count > 4) ? axes_to_ch(axes[4].y) : buttons[btnidx++], //ch10 + (chan_rxpkts == 11) ? rxpkts_per_second : (axes_count > 5) ? axes_to_ch(axes[5].x) : buttons[btnidx++], //ch11 + (chan_rxpkts == 12) ? rxpkts_per_second : (axes_count > 5) ? axes_to_ch(axes[5].y) : buttons[btnidx++], //ch12 + (chan_rxpkts == 13) ? rxpkts_per_second : (axes_count > 6) ? axes_to_ch(axes[6].x) : buttons[btnidx++], //ch13 + (chan_rxpkts == 14) ? rxpkts_per_second : (axes_count > 6) ? axes_to_ch(axes[6].y) : buttons[btnidx++], //ch14 + (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 + + ); + 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("RC Sent %d bytes\n", bytes_sent); + time_check = millis(); + } if (verbose) fflush(stdout); nanosleep(&tw, &tr); //for low CPU ut } while (errsv == 11 || errsv == 38); //while not err