Ejemplo n.º 1
0
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");

}
Ejemplo n.º 2
0
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;
}