mirror of https://github.com/OpenIPC/firmware.git
Huang jinlong
GitHub
parent
9621b14438
commit
cf555d384b
|
|
@ -2,7 +2,6 @@
|
|||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
#include <unistd.h>
|
||||
#include "string.h"
|
||||
#include <linux/joystick.h>
|
||||
#include <sys/socket.h>
|
||||
#include <errno.h>
|
||||
|
|
@ -10,6 +9,8 @@
|
|||
#include <netinet/in.h>
|
||||
#include <arpa/inet.h>
|
||||
#include "common/mavlink.h"
|
||||
#include <time.h>
|
||||
#include <stdlib.h> // 用于 atoi 函数
|
||||
|
||||
#define BUFFER_LENGTH 2041
|
||||
|
||||
|
|
@ -112,7 +113,6 @@ 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;
|
||||
|
|
@ -127,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;
|
||||
|
|
@ -152,18 +152,11 @@ 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 [-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");
|
||||
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");
|
||||
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);
|
||||
|
|
@ -181,7 +174,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 %s rxpkts to channel %d\n", wlan_rxpkts, chan_rxpkts);
|
||||
if(chan_rxpkts > 0) printf("Store rxpkts to channel %d\n", chan_rxpkts);
|
||||
printf("Started\n");
|
||||
|
||||
|
||||
|
|
@ -209,59 +202,59 @@ int main(int argc, char *argv[])
|
|||
//check rxpkts
|
||||
if(chan_rxpkts > 4) {
|
||||
if( (long long)rxpkts_time + 1000 < millis() ){
|
||||
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);
|
||||
FILE* rxpkts_ptr = fopen("/sys/class/net/wlan0/statistics/rx_packets", "r");
|
||||
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);
|
||||
}
|
||||
}
|
||||
//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 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( (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 (verbose) fflush(stdout);
|
||||
nanosleep(&tw, &tr); //for low CPU ut
|
||||
} while (errsv == 11 || errsv == 38); //while not err
|
||||
} //while true
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue