2014-01-04 02:34:53 +01:00
# include <pthread.h>
# include <stdio.h>
# include <stdlib.h>
# include <string.h>
# include <unistd.h>
# include <errno.h>
# include <fcntl.h>
# include <dirent.h>
# include <linux/input.h>
# include <sys/types.h>
# include <sys/stat.h>
# include <sys/select.h>
# include <sys/time.h>
# include <termios.h>
# include <signal.h>
2014-01-05 16:34:00 +01:00
# include <sys/socket.h>
# include <netinet/in.h>
2014-01-06 00:20:30 +01:00
# include <arpa/inet.h>
# define __STDC_FORMAT_MACROS
# include <inttypes.h>
# include "henglong.h"
2014-02-16 04:36:01 +01:00
# include <linux/joystick.h>
2014-02-25 02:28:15 +01:00
# include "wansview.h"
2014-03-22 16:35:41 +01:00
# include "extern.h"
2014-03-27 00:31:38 +01:00
# include "checkvideo.h"
2014-03-30 20:29:30 +02:00
# include "checkkillswitch.h"
2014-01-04 02:34:53 +01:00
2014-02-05 06:43:14 +01:00
typedef struct outtty_t
{
int16_t motor_r ;
int16_t motor_l ;
int16_t servo_tilt ;
int16_t servo_pan ;
} outtty_t ;
2014-01-04 02:34:53 +01:00
typedef struct input_thread_t
{
char * filename ;
2014-01-13 14:53:48 +01:00
int frame ;
henglong_t hl ;
2014-02-05 06:43:14 +01:00
outtty_t outtty ;
2014-01-04 02:34:53 +01:00
} input_thread_t ;
2014-02-16 04:36:01 +01:00
void initouttty ( outtty_t * tty )
{
tty - > motor_l = 0 ;
tty - > motor_r = 0 ;
tty - > servo_pan = 0 ;
tty - > servo_tilt = 0 ;
}
2014-02-05 06:43:14 +01:00
typedef struct RCdatagram_t
{
uint16_t frame_nbr ;
uint64_t time_us ;
2014-02-16 04:36:01 +01:00
int32_t frame_recv ;
2014-02-05 06:43:14 +01:00
uint8_t clisel ;
uint8_t clinbr ;
uint8_t client_selected ;
2014-02-16 04:36:01 +01:00
uint8_t servoff ;
2014-02-05 06:43:14 +01:00
outtty_t outtty ;
2014-02-16 04:36:01 +01:00
} __attribute__ ( ( packed ) ) RCdatagram_t ;
2014-01-04 02:34:53 +01:00
2014-02-16 04:36:01 +01:00
void * keyboard_thread_fcn ( void * arg )
2014-01-04 02:34:53 +01:00
{
2014-01-06 00:20:30 +01:00
printf ( " pthread input started \n " ) ;
2014-01-04 02:34:53 +01:00
2014-01-08 22:54:24 +01:00
struct input_event ev ;
2014-01-04 02:34:53 +01:00
int fevdev ;
2014-02-16 04:36:01 +01:00
int size = sizeof ( ev ) ;
2014-01-04 02:34:53 +01:00
int rd ;
input_thread_t * args ;
args = ( input_thread_t * ) arg ;
fevdev = open ( args - > filename , O_RDONLY ) ;
if ( fevdev = = - 1 ) {
2014-02-16 04:36:01 +01:00
printf ( " Failed to open keyboard device. \n " ) ;
pthread_exit ( 0 ) ;
2014-01-04 02:34:53 +01:00
}
while ( 1 )
{
2014-01-08 22:54:24 +01:00
if ( ( rd = read ( fevdev , & ev , size ) ) < size ) {
2014-01-04 02:34:53 +01:00
break ;
}
2014-01-08 22:54:24 +01:00
if ( EV_KEY = = ev . type ) {
printf ( " %d %d \n " , ev . code , ev . value ) ;
2014-01-13 14:53:48 +01:00
args - > frame = data2frame ( event2data ( & args - > hl , ev ) ) ;
2014-02-05 06:43:14 +01:00
args - > outtty . motor_r = ( 2 * 0 b01111 - args - > hl . velocity - args - > hl . direction ) * 40 ;
args - > outtty . motor_l = ( - args - > hl . velocity + args - > hl . direction ) * 40 ;
2014-02-16 19:14:25 +01:00
2014-01-08 22:54:24 +01:00
}
2014-01-04 02:34:53 +01:00
// quit
2014-01-13 14:53:48 +01:00
if ( 16 = = ev . code & & 1 = = ev . value ) break ;
2014-01-04 02:34:53 +01:00
}
2014-02-16 04:36:01 +01:00
printf ( " Exiting keyboard thread. \n " ) ;
2014-01-06 00:20:30 +01:00
ioctl ( fevdev , EVIOCGRAB , 1 ) ;
2014-01-04 02:34:53 +01:00
close ( fevdev ) ;
pthread_exit ( 0 ) ;
}
2014-02-16 04:36:01 +01:00
void * joystick_thread_fcn ( void * arg )
{
printf ( " pthread input started \n " ) ;
struct js_event ev ;
struct JS_DATA_TYPE jsdata ;
int fevdev ;
int size = sizeof ( ev ) ;
int rd ;
outtty_t outttyloc ;
input_thread_t * args ;
args = ( input_thread_t * ) arg ;
fevdev = open ( args - > filename , O_RDONLY ) ;
if ( fevdev = = - 1 ) {
printf ( " Failed to open joystick device. \n " ) ;
pthread_exit ( 0 ) ;
}
jsdata . x = 0 ;
jsdata . y = 0 ;
jsdata . buttons = 0 ;
while ( 1 )
{
if ( ( rd = read ( fevdev , & ev , size ) ) < size ) {
break ;
}
if ( JS_EVENT_BUTTON = = ev . type ) {
//printf("%d %d %d\n", ev.type, ev.value, ev.number);
if ( 1 = = ev . value ) {
jsdata . buttons | = 1 < < ev . number ;
}
if ( 0 = = ev . value ) {
jsdata . buttons & = ~ ( 1 < < ev . number ) ;
}
}
if ( JS_EVENT_AXIS = = ev . type ) {
2014-02-16 19:14:25 +01:00
printf ( " %d %d %d \n " , ev . type , ev . value , ev . number ) ;
2014-02-16 04:36:01 +01:00
if ( 0 = = ev . number ) {
jsdata . x = ev . value ;
}
if ( 1 = = ev . number ) {
jsdata . y = - ev . value ;
if ( - 32768 = = ev . value ) {
jsdata . y = + 32767 ;
}
}
2014-02-16 19:14:25 +01:00
if ( 2 = = ev . number ) {
outttyloc . servo_tilt = 0 ;
if ( - 30000 > ev . value ) {
outttyloc . servo_tilt = - 1 ;
args - > hl . tilt_down = 1 ;
args - > hl . tilt_up = 0 ;
} else {
args - > hl . tilt_down = 0 ;
if ( + 5000 > ev . value ) {
args - > hl . tilt_up = 1 ;
outttyloc . servo_tilt = + 1 ;
} else {
args - > hl . tilt_up = 0 ;
}
}
}
if ( 3 = = ev . number ) {
outttyloc . servo_pan = 0 ;
if ( - 30000 > ev . value ) {
args - > hl . pan_left = 1 ;
args - > hl . pan_right = 0 ;
outttyloc . servo_pan = - 1 ;
} else {
args - > hl . pan_left = 0 ;
if ( + 5000 > ev . value ) {
args - > hl . pan_right = 1 ;
outttyloc . servo_pan = + 1 ;
} else {
args - > hl . pan_right = 0 ;
}
}
}
2014-02-16 04:36:01 +01:00
}
outttyloc . motor_r = jsdata . y / 30 - jsdata . x / 30 ;
outttyloc . motor_l = jsdata . y / 30 + jsdata . x / 30 ;
if ( 1022 < outttyloc . motor_r ) outttyloc . motor_r = 1023 ;
if ( - 1022 > outttyloc . motor_r ) outttyloc . motor_r = - 1023 ;
if ( 1022 < outttyloc . motor_l ) outttyloc . motor_l = 1023 ;
if ( - 1022 > outttyloc . motor_l ) outttyloc . motor_l = - 1023 ;
args - > outtty = outttyloc ;
2014-02-16 19:14:25 +01:00
args - > hl . ignation = ( 1 & ( jsdata . buttons > > 2 ) ) ;
2014-03-23 00:12:24 +01:00
args - > hl . fire = ( 1 & ( jsdata . buttons > > 0 ) ) ;
2014-02-16 04:36:01 +01:00
printf ( " %6d %6d %4x \n " , jsdata . x , jsdata . y , jsdata . buttons ) ;
}
printf ( " Exiting joystick thread. \n " ) ;
close ( fevdev ) ;
pthread_exit ( 0 ) ;
}
2014-01-05 16:34:00 +01:00
typedef struct refl_thread_args_t
{
int sockfd ;
uint64_t timestamps [ 256 ] ;
uint16_t frame_nbr_send ;
2014-01-06 00:20:30 +01:00
uint8_t timeout ;
2014-01-05 16:34:00 +01:00
} refl_thread_args_t ;
uint64_t get_us ( void )
{
struct timeval tv ;
gettimeofday ( & tv , NULL ) ;
return ( uint64_t ) tv . tv_usec + 1000000 * ( uint64_t ) tv . tv_sec ;
2014-01-04 02:34:53 +01:00
}
2014-01-05 16:34:00 +01:00
void * refl_thread_fcn ( void * arg )
{
2014-02-05 06:43:14 +01:00
int n ;
2014-01-05 16:34:00 +01:00
int frame_refl ;
uint64_t time_us_refl ;
uint16_t frame_nbr_refl ;
refl_thread_args_t * refl_thread_args_ptr ;
2014-01-06 21:38:22 +01:00
uint8_t clinbr , clisel ;
unsigned char servoff ;
2014-02-05 06:43:14 +01:00
RCdatagram_t refldata ;
2014-01-07 05:25:14 +01:00
2014-01-05 16:34:00 +01:00
printf ( " pthread refl started \n " ) ;
2014-01-04 02:34:53 +01:00
2014-01-05 16:34:00 +01:00
refl_thread_args_ptr = ( refl_thread_args_t * ) arg ;
2014-01-04 02:34:53 +01:00
2014-01-05 16:34:00 +01:00
while ( 1 ) {
2014-02-16 04:36:01 +01:00
n = recvfrom ( refl_thread_args_ptr - > sockfd , & refldata , sizeof ( refldata ) , 0 , NULL , NULL ) ;
2014-02-05 06:43:14 +01:00
frame_nbr_refl = refldata . frame_nbr ;
time_us_refl = refldata . time_us ;
frame_refl = refldata . frame_recv ;
clinbr = refldata . clinbr ;
clisel = refldata . clisel ;
servoff = refldata . servoff ;
2014-01-06 21:38:22 +01:00
printf ( " REFL FRAME -- FRM_NBR: %5d, RTT: %7 " PRIu64 " , BYTES recv: %3d, REFL_FRM: %#x, CLINBR: %d, CLISEL: %d, SERVOFF: %d \n " , frame_nbr_refl , get_us ( ) - time_us_refl , n , frame_refl , clinbr , clisel , servoff ) ;
2014-01-06 00:20:30 +01:00
refl_thread_args_ptr - > timestamps [ frame_nbr_refl % refl_thread_args_ptr - > timeout ] = 0 ;
2014-01-05 16:34:00 +01:00
}
pthread_exit ( 0 ) ;
}
2014-01-04 02:34:53 +01:00
2014-01-06 00:20:30 +01:00
typedef struct henglongconf_t
{
uint32_t frame_us ;
2014-02-16 04:36:01 +01:00
char keyboarddevname [ 256 ] ;
char joystickdevname [ 256 ] ;
2014-01-06 00:20:30 +01:00
in_addr_t ip ; // v4 only
2014-02-25 02:28:15 +01:00
char cam [ 64 ] ;
2014-03-27 00:31:38 +01:00
char video [ 16 ] ;
2014-03-30 20:29:30 +02:00
char killsw [ 16 ] ;
char killurl [ 256 ] ;
2014-03-25 17:30:20 +01:00
char user [ 64 ] ;
char pwd [ 64 ] ;
2014-02-25 02:28:15 +01:00
uint32_t caminterval ;
2014-03-30 20:29:30 +02:00
uint16_t port , videoport , killport ;
2014-01-06 00:20:30 +01:00
uint8_t timeout ;
2014-01-06 21:38:22 +01:00
uint8_t clinbr ;
2014-04-02 20:47:46 +02:00
uint32_t killswtimeoutus ;
2014-04-03 01:13:46 +02:00
uint32_t killswint ;
2014-01-06 00:20:30 +01:00
} henglongconf_t ;
henglongconf_t getconfig ( char * conffilename )
{
FILE * configFile ;
char line [ 256 ] ;
char parameter [ 16 ] , value [ 256 ] ;
char ip [ 16 ] ;
henglongconf_t conf ;
configFile = fopen ( conffilename , " r " ) ;
// defaults
conf . timeout = 16 ;
conf . frame_us = 100000 ;
conf . port = 32000 ;
conf . ip = inet_addr ( " 127.0.0.1 " ) ;
2014-02-16 04:36:01 +01:00
conf . keyboarddevname [ 0 ] = 0 ;
conf . joystickdevname [ 0 ] = 0 ;
2014-02-25 02:28:15 +01:00
conf . cam [ 0 ] = 0 ;
conf . caminterval = 100000 ;
2014-03-25 17:30:20 +01:00
conf . user [ 0 ] = 0 ;
conf . pwd [ 0 ] = 0 ;
2014-03-27 00:31:38 +01:00
conf . video [ 0 ] = 0 ;
conf . videoport = 0 ;
2014-03-30 20:29:30 +02:00
conf . killsw [ 0 ] = 0 ;
conf . killport = 0 ;
conf . killurl [ 0 ] = 0 ;
2014-04-02 20:47:46 +02:00
conf . killswtimeoutus = 500000 ;
2014-04-03 01:13:46 +02:00
conf . killswint = 100000 ;
2014-01-06 00:20:30 +01:00
while ( fgets ( line , 256 , configFile ) ) {
sscanf ( line , " %16s %256s " , parameter , value ) ;
2014-02-16 04:36:01 +01:00
if ( 0 = = strcmp ( parameter , " KEYBOARD " ) ) {
sscanf ( value , " %256s " , conf . keyboarddevname ) ;
}
if ( 0 = = strcmp ( parameter , " JOYSTICK " ) ) {
sscanf ( value , " %256s " , conf . joystickdevname ) ;
2014-01-06 00:20:30 +01:00
}
2014-02-25 02:28:15 +01:00
if ( 0 = = strcmp ( parameter , " CAM " ) ) {
sscanf ( value , " %64s " , conf . cam ) ;
}
2014-03-25 17:30:20 +01:00
if ( 0 = = strcmp ( parameter , " USER " ) ) {
sscanf ( value , " %64s " , conf . user ) ;
}
if ( 0 = = strcmp ( parameter , " PWD " ) ) {
sscanf ( value , " %64s " , conf . pwd ) ;
}
2014-02-25 02:28:15 +01:00
if ( 0 = = strcmp ( parameter , " CAMINTERVAL " ) ) {
sscanf ( value , " % " SCNu32 , & conf . caminterval ) ;
}
2014-01-06 00:20:30 +01:00
if ( 0 = = strcmp ( parameter , " FRAME_US " ) ) {
sscanf ( value , " % " SCNu32 , & conf . frame_us ) ;
}
if ( 0 = = strcmp ( parameter , " SERVER " ) ) {
sscanf ( value , " %16[^:]:% " SCNu16 , ip , & conf . port ) ;
conf . ip = inet_addr ( ip ) ;
}
if ( 0 = = strcmp ( parameter , " TIMEOUT " ) ) {
sscanf ( value , " % " SCNu8 , & conf . timeout ) ;
}
2014-01-06 21:38:22 +01:00
if ( 0 = = strcmp ( parameter , " CLINBR " ) ) {
sscanf ( value , " % " SCNu8 , & conf . clinbr ) ;
}
2014-03-27 00:31:38 +01:00
if ( 0 = = strcmp ( parameter , " VIDEO " ) ) {
sscanf ( value , " %16[^:]:% " SCNu16 , conf . video , & conf . videoport ) ;
}
2014-03-30 20:29:30 +02:00
if ( 0 = = strcmp ( parameter , " KILLSW " ) ) {
sscanf ( value , " %16[^:]:% " SCNu16 " %256s " , conf . killsw , & conf . killport , conf . killurl ) ;
}
2014-04-02 20:47:46 +02:00
if ( 0 = = strcmp ( parameter , " KILLSWTO_US " ) ) {
sscanf ( value , " % " SCNu32 , & conf . killswtimeoutus ) ;
}
2014-04-03 01:13:46 +02:00
if ( 0 = = strcmp ( parameter , " KILLSWINT " ) ) {
sscanf ( value , " % " SCNu32 , & conf . killswint ) ;
}
2014-01-06 00:20:30 +01:00
}
return conf ;
}
2014-01-05 16:34:00 +01:00
2014-02-25 02:28:15 +01:00
typedef struct cam_ctrl_thread_t
{
char * ip ;
2014-03-25 17:30:20 +01:00
char * username ;
char * password ;
2014-03-06 00:35:51 +01:00
int up , down , cw , ccw , nul , end ;
2014-02-25 02:28:15 +01:00
uint32_t caminterval ;
} cam_ctrl_thread_t ;
void * cam_ctrl_thread_fcn ( void * arg )
{
printf ( " pthread cam_ctrl started \n " ) ;
cam_ctrl_thread_t * args ;
args = ( cam_ctrl_thread_t * ) arg ;
while ( ! args - > end ) {
usleep ( args - > caminterval ) ;
2014-03-25 17:30:20 +01:00
if ( args - > up ) cam_up ( args - > ip , args - > username , args - > password ) ;
if ( args - > down ) cam_down ( args - > ip , args - > username , args - > password ) ;
if ( args - > cw ) cam_cw ( args - > ip , args - > username , args - > password ) ;
if ( args - > ccw ) cam_ccw ( args - > ip , args - > username , args - > password ) ;
2014-03-06 00:35:51 +01:00
if ( args - > nul ) {
2014-03-25 17:30:20 +01:00
cam_nul ( args - > ip , args - > username , args - > password ) ;
2014-03-06 00:35:51 +01:00
args - > nul = 0 ;
}
2014-02-25 02:28:15 +01:00
}
pthread_exit ( 0 ) ;
}
2014-04-02 20:47:46 +02:00
typedef struct killsw_thread_t
{
char * ip ;
uint16_t port ;
char * url ;
uint32_t timeout_us ;
2014-04-03 01:13:46 +02:00
uint32_t killswint ;
2014-04-02 20:47:46 +02:00
int state ;
} killsw_thread_t ;
void * killsw_thread_fcn ( void * arg )
{
printf ( " pthread killsw started \n " ) ;
killsw_thread_t * args ;
args = ( killsw_thread_t * ) arg ;
args - > state = 0 ;
while ( 1 ) {
2014-04-03 01:13:46 +02:00
usleep ( args - > killswint ) ;
args - > state = checkkillswitch ( args - > ip , args - > port , args - > url , args - > timeout_us ) ;
2014-04-02 20:47:46 +02:00
}
pthread_exit ( 0 ) ;
}
2014-01-05 16:34:00 +01:00
int main ( int argc , char * argv [ ] )
2014-01-04 02:34:53 +01:00
{
2014-03-27 00:31:38 +01:00
2014-03-30 20:29:30 +02:00
2014-04-02 20:47:46 +02:00
pthread_t keybthread , joythread , refl_thread , cam_ctrl_thread , killsw_thread ;
2014-02-16 04:36:01 +01:00
input_thread_t keyboard_thread_args ;
input_thread_t joystick_thread_args ;
2014-01-05 16:34:00 +01:00
refl_thread_args_t refl_thread_args ;
2014-04-02 20:47:46 +02:00
killsw_thread_t killsw_thread_args ;
2014-02-25 02:28:15 +01:00
cam_ctrl_thread_t cam_ctrl_thread_args ;
2014-01-13 14:53:48 +01:00
int frame = 0 ;
2014-01-05 16:34:00 +01:00
uint16_t frame_nbr ;
2014-01-06 00:20:30 +01:00
int sockfd , n_send ;
struct sockaddr_in servaddr ;
uint64_t time_us ;
henglongconf_t conf ;
2014-02-05 06:43:14 +01:00
RCdatagram_t senddata ;
2014-02-25 02:28:15 +01:00
int updown , cwccw ;
2014-03-22 16:35:41 +01:00
int fire_old ;
2014-01-05 16:34:00 +01:00
2014-01-06 21:38:22 +01:00
if ( 2 ! = argc ) {
printf ( " \n This program is intented to be run on the PC as client to control the server on the heng long tank. \n \n USAGE: UDPclient client.config \n \n Copyright (C) 2014 Stefan Helmert <stefan.helmert@gmx.net> \n \n " ) ;
return 0 ;
}
2014-02-16 04:36:01 +01:00
inithenglong ( & keyboard_thread_args . hl ) ;
initouttty ( & keyboard_thread_args . outtty ) ;
inithenglong ( & joystick_thread_args . hl ) ;
initouttty ( & joystick_thread_args . outtty ) ;
2014-01-06 00:20:30 +01:00
conf = getconfig ( argv [ 1 ] ) ;
2014-02-16 04:36:01 +01:00
joystick_thread_args . filename = conf . joystickdevname ;
keyboard_thread_args . filename = conf . keyboarddevname ;
2014-01-05 16:34:00 +01:00
2014-02-16 04:36:01 +01:00
if ( keyboard_thread_args . filename [ 0 ] ) {
if ( pthread_create ( & keybthread , NULL , keyboard_thread_fcn , ( void * ) & keyboard_thread_args ) ) printf ( " failed to create keyboard thread \n " ) ;
} else {
printf ( " no keyboard! \n " ) ;
}
if ( joystick_thread_args . filename [ 0 ] ) {
if ( pthread_create ( & joythread , NULL , joystick_thread_fcn , ( void * ) & joystick_thread_args ) ) printf ( " failed to create joystick thread \n " ) ;
} else {
printf ( " no joystick! \n " ) ;
}
2014-02-25 02:28:15 +01:00
if ( conf . cam [ 0 ] ) {
cam_ctrl_thread_args . down = 0 ;
cam_ctrl_thread_args . up = 0 ;
cam_ctrl_thread_args . cw = 0 ;
cam_ctrl_thread_args . ccw = 0 ;
cam_ctrl_thread_args . end = 0 ;
cam_ctrl_thread_args . ip = conf . cam ;
cam_ctrl_thread_args . caminterval = conf . caminterval ;
2014-03-25 17:30:20 +01:00
cam_ctrl_thread_args . username = conf . user ;
cam_ctrl_thread_args . password = conf . pwd ;
2014-02-25 02:28:15 +01:00
if ( pthread_create ( & cam_ctrl_thread , NULL , cam_ctrl_thread_fcn , ( void * ) & cam_ctrl_thread_args ) ) printf ( " failed to create cam_ctrl thread \n " ) ;
} else {
printf ( " no cam config! \n " ) ;
}
2014-04-02 20:47:46 +02:00
killsw_thread_args . state = 1 ; // default, wenn kein Killswitch konfiguriert - immer an
if ( conf . killsw [ 0 ] ) {
killsw_thread_args . ip = conf . killsw ;
killsw_thread_args . port = conf . killport ;
killsw_thread_args . url = conf . killurl ;
killsw_thread_args . state = 0 ;
killsw_thread_args . timeout_us = conf . killswtimeoutus ;
2014-04-03 01:13:46 +02:00
killsw_thread_args . killswint = conf . killswint ;
2014-04-02 20:47:46 +02:00
if ( pthread_create ( & killsw_thread , NULL , killsw_thread_fcn , ( void * ) & killsw_thread_args ) ) printf ( " failed to create killsw thread \n " ) ;
} else {
printf ( " no killswitch! \n " ) ;
}
2014-01-05 16:34:00 +01:00
sockfd = socket ( AF_INET , SOCK_DGRAM , 0 ) ;
2014-01-04 02:34:53 +01:00
2014-01-05 16:34:00 +01:00
refl_thread_args . sockfd = sockfd ;
2014-01-06 00:20:30 +01:00
refl_thread_args . timeout = conf . timeout ;
2014-01-04 02:34:53 +01:00
2014-01-05 16:34:00 +01:00
bzero ( & servaddr , sizeof ( servaddr ) ) ;
servaddr . sin_family = AF_INET ;
2014-01-06 00:20:30 +01:00
servaddr . sin_addr . s_addr = conf . ip ;
servaddr . sin_port = htons ( conf . port ) ;
2014-01-04 02:34:53 +01:00
2014-01-07 00:15:04 +01:00
if ( pthread_create ( & refl_thread , NULL , refl_thread_fcn , ( void * ) & refl_thread_args ) ) printf ( " failed to create thread \n " ) ;
2014-01-05 16:34:00 +01:00
2014-01-06 00:20:30 +01:00
memset ( refl_thread_args . timestamps , 0 , 256 * sizeof ( uint64_t ) ) ;
2014-01-05 16:34:00 +01:00
frame_nbr = 0 ;
2014-02-16 19:14:25 +01:00
senddata . outtty . servo_pan = 0 ;
senddata . outtty . servo_tilt = 0 ;
2014-03-06 00:35:51 +01:00
cam_ctrl_thread_args . nul = 1 ;
2014-03-22 16:35:41 +01:00
fire_old = 0 ;
2014-01-04 02:34:53 +01:00
while ( 1 ) {
2014-01-06 00:20:30 +01:00
usleep ( conf . frame_us ) ;
2014-01-05 16:34:00 +01:00
2014-03-27 00:31:38 +01:00
// do not send control commands if live video is not watched
if ( 0 = = checkvideo ( conf . video , conf . videoport ) ) {
printf ( " No video stream connection from %s:%u, remote control locked! \n " , conf . video , conf . videoport ) ;
continue ;
}
2014-04-02 20:47:46 +02:00
if ( 0 = = killsw_thread_args . state ) {
2014-03-30 20:29:30 +02:00
printf ( " Killswitch pressed or no connection.! \n " ) ;
continue ;
}
2014-02-16 04:36:01 +01:00
frame = 0xc0ffee ;
2014-01-13 14:53:48 +01:00
2014-01-05 16:34:00 +01:00
time_us = get_us ( ) ;
frame_nbr + + ;
refl_thread_args . frame_nbr_send = frame_nbr ;
2014-01-06 00:20:30 +01:00
if ( refl_thread_args . timestamps [ frame_nbr % conf . timeout ] ) printf ( " *** Frameloss detected! Lost frame from local time: % " PRIu64 " \n " , refl_thread_args . timestamps [ frame_nbr % conf . timeout ] ) ;
refl_thread_args . timestamps [ frame_nbr % conf . timeout ] = time_us ;
2014-01-05 16:34:00 +01:00
2014-02-05 06:43:14 +01:00
senddata . frame_nbr = frame_nbr ;
senddata . time_us = time_us ;
senddata . frame_recv = frame ;
senddata . clinbr = conf . clinbr ;
2014-02-16 04:36:01 +01:00
senddata . clisel = keyboard_thread_args . hl . clisel ;
senddata . servoff = keyboard_thread_args . hl . servoff ;
senddata . outtty . motor_l = keyboard_thread_args . outtty . motor_l + joystick_thread_args . outtty . motor_l ;
senddata . outtty . motor_r = keyboard_thread_args . outtty . motor_r + joystick_thread_args . outtty . motor_r ;
2014-02-25 02:28:15 +01:00
cwccw = keyboard_thread_args . hl . pan_right - keyboard_thread_args . hl . pan_left + joystick_thread_args . hl . pan_right - joystick_thread_args . hl . pan_left ;
updown = keyboard_thread_args . hl . tilt_up - keyboard_thread_args . hl . tilt_down + joystick_thread_args . hl . tilt_up - joystick_thread_args . hl . tilt_down ;
senddata . outtty . servo_pan + = cwccw ;
senddata . outtty . servo_tilt + = updown ;
if ( + 1 < = cwccw ) {
cam_ctrl_thread_args . ccw = 1 ;
cam_ctrl_thread_args . cw = 0 ;
} else if ( - 1 > = cwccw ) {
cam_ctrl_thread_args . ccw = 0 ;
cam_ctrl_thread_args . cw = 1 ;
} else {
cam_ctrl_thread_args . ccw = 0 ;
cam_ctrl_thread_args . cw = 0 ;
}
if ( + 1 < = updown ) {
cam_ctrl_thread_args . up = 1 ;
cam_ctrl_thread_args . down = 0 ;
} else if ( - 1 > = updown ) {
cam_ctrl_thread_args . up = 0 ;
cam_ctrl_thread_args . down = 1 ;
} else {
cam_ctrl_thread_args . up = 0 ;
cam_ctrl_thread_args . down = 0 ;
}
2014-02-16 19:14:25 +01:00
if ( keyboard_thread_args . hl . ignation | joystick_thread_args . hl . ignation ) {
senddata . outtty . servo_pan = 0 ;
senddata . outtty . servo_tilt = 0 ;
2014-03-06 00:35:51 +01:00
cam_ctrl_thread_args . nul = 1 ;
2014-02-16 19:14:25 +01:00
}
2014-02-16 04:36:01 +01:00
if ( 1022 < senddata . outtty . motor_l ) senddata . outtty . motor_l = 1023 ;
if ( - 1022 > senddata . outtty . motor_l ) senddata . outtty . motor_l = - 1023 ;
if ( 1022 < senddata . outtty . motor_r ) senddata . outtty . motor_r = 1023 ;
if ( - 1022 > senddata . outtty . motor_r ) senddata . outtty . motor_r = - 1023 ;
2014-01-05 16:34:00 +01:00
2014-02-16 19:14:25 +01:00
if ( senddata . outtty . servo_pan > 50 ) senddata . outtty . servo_pan = 50 ;
if ( senddata . outtty . servo_pan < - 50 ) senddata . outtty . servo_pan = - 50 ;
if ( senddata . outtty . servo_tilt > 50 ) senddata . outtty . servo_tilt = 50 ;
if ( senddata . outtty . servo_tilt < - 50 ) senddata . outtty . servo_tilt = - 50 ;
2014-03-22 16:35:41 +01:00
// Return-Button on Keyboard via Joystick-Fire for screenshot in Browser
2014-03-27 00:31:38 +01:00
if ( ( 0 = = fire_old ) & ( 1 = = joystick_thread_args . hl . fire ) ) {
2014-03-22 16:35:41 +01:00
fire ( ) ;
}
fire_old = joystick_thread_args . hl . fire ;
2014-02-16 19:14:25 +01:00
2014-02-05 06:43:14 +01:00
n_send = sendto ( sockfd , & senddata , sizeof ( senddata ) , 0 , ( struct sockaddr * ) & servaddr , sizeof ( servaddr ) ) ;
2014-01-05 16:34:00 +01:00
2014-02-16 04:36:01 +01:00
printf ( " SEND FRAME -- FRM_NBR: %5d, BYTES send: %3d, SEND_FRM: %#x, CLINBR: %d, CLISEL: %d, SERVOFF: %d \n " , frame_nbr , n_send , frame , senddata . clinbr , senddata . clisel , senddata . servoff ) ;
2014-04-02 03:04:16 +02:00
if ( keyboard_thread_args . filename [ 0 ] ) if ( pthread_kill ( keybthread , 0 ) ) break ;
2014-01-04 02:34:53 +01:00
}
2014-01-05 16:34:00 +01:00
return 0 ;
2014-01-04 02:34:53 +01:00
}
2014-01-05 16:34:00 +01:00