mirror of https://github.com/OpenIPC/firmware.git
Update rcjoystick.c
parent
539e7844ee
commit
23a2f2aee1
|
@ -2,16 +2,14 @@
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
|
#include "string.h"
|
||||||
#include <linux/joystick.h>
|
#include <linux/joystick.h>
|
||||||
#include <sys/socket.h>
|
#include <sys/socket.h>
|
||||||
#include "string.h"
|
|
||||||
#include <errno.h>
|
#include <errno.h>
|
||||||
#include <sys/time.h>
|
#include <sys/time.h>
|
||||||
#include <netinet/in.h>
|
#include <netinet/in.h>
|
||||||
#include <arpa/inet.h>
|
#include <arpa/inet.h>
|
||||||
#include "common/mavlink.h"
|
#include "common/mavlink.h"
|
||||||
#include <time.h>
|
|
||||||
#include <stdlib.h> // 用于 atoi 函数
|
|
||||||
|
|
||||||
#define BUFFER_LENGTH 2041
|
#define BUFFER_LENGTH 2041
|
||||||
|
|
||||||
|
@ -114,6 +112,7 @@ int main(int argc, char *argv[])
|
||||||
|
|
||||||
//time checker
|
//time checker
|
||||||
long long time_check = millis();
|
long long time_check = millis();
|
||||||
|
long long time_check_ping = millis();
|
||||||
uint16_t send_time = 50;
|
uint16_t send_time = 50;
|
||||||
struct timespec tw = {0,10};
|
struct timespec tw = {0,10};
|
||||||
struct timespec tr;
|
struct timespec tr;
|
||||||
|
@ -128,9 +127,9 @@ int main(int argc, char *argv[])
|
||||||
int8_t chan_rxpkts = 0; //default disabled
|
int8_t chan_rxpkts = 0; //default disabled
|
||||||
int16_t rxpkts_prev = 0, rxpkts = 0, rxpkts_per_second = 0;
|
int16_t rxpkts_prev = 0, rxpkts = 0, rxpkts_per_second = 0;
|
||||||
long long rxpkts_time = millis();
|
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) {
|
switch (opt) {
|
||||||
case 'v':
|
case 'v':
|
||||||
verbose = true;
|
verbose = true;
|
||||||
|
@ -153,12 +152,19 @@ int main(int argc, char *argv[])
|
||||||
case 'r':
|
case 'r':
|
||||||
chan_rxpkts = atoi(optarg);
|
chan_rxpkts = atoi(optarg);
|
||||||
break;
|
break;
|
||||||
|
case 'i':
|
||||||
|
strcpy(wlan_rxpkts, optarg);
|
||||||
|
break;
|
||||||
case 'h':
|
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;
|
return 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
char rxpkts_file[100];
|
||||||
|
snprintf(rxpkts_file, sizeof(rxpkts_file), "/sys/class/net/%s/statistics/rx_packets", wlan_rxpkts);
|
||||||
|
|
||||||
|
|
||||||
while (true) { //loop
|
while (true) { //loop
|
||||||
js = open(device, O_RDONLY | O_NONBLOCK);
|
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("Update time: %dms\n", send_time);
|
||||||
printf("UDP: %s:%d\n", inet_ntoa(sin_out.sin_addr), ntohs(sin_out.sin_port));
|
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);
|
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");
|
printf("Started\n");
|
||||||
|
|
||||||
|
|
||||||
|
@ -203,30 +209,36 @@ int main(int argc, char *argv[])
|
||||||
//check rxpkts
|
//check rxpkts
|
||||||
if(chan_rxpkts > 4) {
|
if(chan_rxpkts > 4) {
|
||||||
if( (long long)rxpkts_time + 1000 < millis() ){
|
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);
|
fscanf(rxpkts_ptr, "%hd", &rxpkts);
|
||||||
fclose(rxpkts_ptr);
|
fclose(rxpkts_ptr);
|
||||||
rxpkts_per_second = rxpkts - rxpkts_prev;
|
rxpkts_per_second = rxpkts - rxpkts_prev;
|
||||||
rxpkts_time = millis();
|
|
||||||
rxpkts_prev = rxpkts;
|
rxpkts_prev = rxpkts;
|
||||||
if (verbose) printf("current rx per sec is %d \n", rxpkts_per_second);
|
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
|
//send to udp
|
||||||
if( (long long)time_check + send_time < millis() ){
|
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].x), //ch1
|
||||||
axes_to_ch(axes[0].y), //ch2
|
axes_to_ch(axes[0].y), //ch2
|
||||||
axes_to_ch(axes[1].x), //ch3
|
axes_to_ch(axes[1].x), //ch3
|
||||||
axes_to_ch(axes[1].y), //ch4
|
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 == 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 == 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 == 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 == 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 == 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 == 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);
|
len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||||
bytes_sent = sendto(out_sock, buf, len, 0, (struct sockaddr *)&sin_out, sizeof(sin_out));
|
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();
|
time_check = millis();
|
||||||
}
|
}
|
||||||
if (verbose) fflush(stdout);
|
if (verbose) fflush(stdout);
|
||||||
|
|
Loading…
Reference in New Issue