int main (int argc, char **argv)
{
int fd;
struct js_event js;
unsigned int t, i;
if ((fd = open(JS_DEV, O_RDONLY)) < 0) {
perror("jstest");
exit(1);
}
/* non-blocking mode
*/
fcntl(fd, F_SETFL, O_NONBLOCK);
while (1) {
/* simulate work
*/
mon_delay_ms(300);
t = mon_time_ms();
/* check up on JS
*/
while (read(fd, &js, sizeof(struct js_event)) ==
sizeof(struct js_event)) {
/* register data
*/
// fprintf(stderr,".");
switch(js.type & ~JS_EVENT_INIT) {
case JS_EVENT_BUTTON:
button[js.number] = js.value;
break;
case JS_EVENT_AXIS:
axis[js.number] = js.value;
break;
}
}
if (errno != EAGAIN) {
perror("\njs: error reading (EAGAIN)");
exit (1);
}
printf("\n");
printf("%5d ",t);
for (i = 0; i < 6; i++) {
printf("%6d ",axis[i]);
}
printf(" | ");
for (i = 0; i < 12; i++) {
printf("%d ",button[i]);
}
if (button[0])
break;
}
printf("\n<exit>\n");
}
int set_js_command(int fd)
{
struct js_event js;
unsigned int t, i, throttle_total = 0, throttle_on_scale = 0, command_set = 0;
/* simulate work ???*/
mon_delay_ms(50);
t = mon_time_ms();
/*************** READ JOYSTICK ****************************/
while (read(fd, &js, sizeof(struct js_event)) == sizeof(struct js_event))
{
/* register data */
switch(js.type & ~JS_EVENT_INIT)
{
case JS_EVENT_BUTTON:
button[js.number] = js.value;
break;
case JS_EVENT_AXIS:
axis[js.number] = js.value;
break;
}
}
if (errno != EAGAIN)
{
perror("\njs: error reading (EAGAIN)");
exit(1);
}
//exit program *********** SHOULD GO IN PANIC MODE?
if (button[0])
return 1;
/*************** ENABLE JOYSTICK AFTER EVERYTHING IS SET TO ZERO *****************/
//when all axis are 0, then it is safe to increase the init_2_op counter
if(js_init_mode && axis[0] == 0 && axis[1] == 0 && axis[2] == 0)
js_init_2_op_counter++;
// when a level of init_2_op of 5 is reached the joystick is considered out of biased init_mode
if(js_init_2_op_counter > JS_OPERATION_MODE_THRESHOLD)
js_init_mode = 0, js_operation_mode = 1;
if(js_init_mode)
printf("init_mode\n");
if(js_operation_mode)
printf("op mode\n");
/*************** PREPARE PACKET TO BE SENT *************************************/
//pitch
if(axis[1] < -JS_MIN_VALUE || axis[1] > JS_MIN_VALUE)
{
set_throttle_command(SET_PITCH, axis[1], JS_STEP_DIVISION_SMALL);
command_set = 1;
}
else
{
set_throttle_command(SET_PITCH, 0, JS_STEP_DIVISION_SMALL);
command_set = 1;
}
//roll
if(axis[0] < -JS_MIN_VALUE || axis[0] > JS_MIN_VALUE)
{
set_throttle_command(SET_ROLL, axis[0], JS_STEP_DIVISION_SMALL);
command_set = 1;
}
else
{
set_throttle_command(SET_ROLL, 0, JS_STEP_DIVISION_SMALL);
command_set = 1;
}
//yaw
if(axis[2] < -JS_MIN_VALUE || axis[2] > JS_MIN_VALUE){
set_throttle_command(SET_YAWRATE, axis[2], JS_STEP_DIVISION_SMALL);
command_set = 1;
}else{
set_throttle_command(SET_YAWRATE, 0, JS_STEP_DIVISION_SMALL);
command_set = 1;
}
//throttle
throttle_total = 65534 - (axis[3] + 32767);
if(throttle_total > JS_MIN_VALUE){
set_throttle_command(SET_LIFT, throttle_total, JS_STEP_DIVISION_BIG);
command_set = 1;
}else{
set_throttle_command(SET_LIFT, 0, JS_STEP_DIVISION_BIG);
command_set = 1;
}
if(command_set == 0){
push_packet_t(EMPTY, EMPTY);
}
return 0;
}
