Пример #1
0
/************************************************************************
* 	int main()
*	initializes hardware and configuration struct.
* 	starts the IMU interrupt routine
*	starts the orientation detection thread
*	prints out orientation to the screen
************************************************************************/
int main(){
	initialize_cape();
	
	// initialize the global conf struct with some values
	conf.sample_rate = 50;	// Hz
	conf.det_time = 0.3;		// seconds
	conf.det_poss_time = 0.1;	// seconds
	conf.det_tolerance = 30; 	// degrees
	
	// initialize the IMU in flat orientation so that
	// flat is with the cape facing up
	signed char imu_orientation[9] = ORIENTATION_FLAT; 
	initialize_imu(conf.sample_rate, imu_orientation);
	
	//start the interrupt handler
	set_imu_interrupt_func(&read_imu_data);
	
	// start the orientation detection thread in the background
	pthread_t orientation_thread;
	pthread_create(&orientation_thread, NULL, orientation_detector, (void*) NULL);
	
	// print a header
	printf("Move your BBB around and observe the changing orientation\n");
	printf("\n\ncounter  possible  certain   \n");
	
	//now just wait, print_imu_data will run
	while (get_state() != EXITING) {
		printf("\r");
		printf(" %3d    ", cstate.counter);
		print_orientation(cstate.poss_orientation);
		printf(" ");
		print_orientation(cstate.orientation);
		printf("               ");
		// its important we flush the output to make sure
		// the next printf's print onto a clean line
		fflush(stdout); 
		
		// sleep for 10ms
		usleep(10000);
	}
	cleanup_cape();
	return 0;
}
Пример #2
0
int main(){
	imu_data_t data; //struct to hold new data
	
	if(initialize_cape()){
		printf("ERROR: failed to initialize_cape\n");
		return -1;
	}
	
	// use defaults for now, except also enable magnetometer.
	imu_config_t conf = get_default_imu_config();
	conf.enable_magnetometer=1;
	
	if(initialize_imu(&data, conf)){
		printf("initialize_imu_failed\n");
		return -1;
	}
	
	// print a header
	printf("\n");
	printf("   Accel XYZ(m/s^2)  |");
	printf("   Gyro XYZ (deg/s)  |");
	printf("  Mag Field XYZ(uT)  |");
	printf(" Temp (C)");
	printf("\n");
	
	//now just wait, print_data will run
	while (get_state() != EXITING) {
		printf("\r");
		
		if(read_accel_data(&data)<0)
			printf("read accel data failed\n");
		else printf("%6.2f %6.2f %6.2f |",	data.accel[0],\
											data.accel[1],\
											data.accel[2]);
								
		if(read_gyro_data(&data)<0)
			printf("read gyro data failed\n");
		else printf("%6.1f %6.1f %6.1f |",	data.gyro[0],\
											data.gyro[1],\
											data.gyro[2]);
								 
		if(read_mag_data(&data)<0){
			printf("read mag data failed\n");
		}
		else printf("%6.1f %6.1f %6.1f |",	data.mag[0],\
											data.mag[1],\
											data.mag[2]);
								
		if(read_imu_temp(&data)<0){
			printf("read temp data failed\n");
		}
		else printf(" %4.1f ", data.temp);
														
		fflush(stdout);
		usleep(100000);
	}

	power_off_imu();
	cleanup_cape();
	return 0;
}
Пример #3
0
/***********************************************************************
*	main()
*	initialize the IMU, start all the threads, and wait still something 
*	triggers a shut down
***********************************************************************/
int main(int argc, char* argv[]){
	// initialize cape hardware
	if(initialize_cape()<0){
		blink_red();
		return -1;
	}
	setRED(HIGH);
	setGRN(LOW);
	set_state(UNINITIALIZED);
	
	// set up button handlers first
	// so user can exit by holding pause
	set_pause_pressed_func(&on_pause_press);
	set_mode_unpressed_func(&on_mode_release);
	
	// load data from disk.
	if(load_config(&config)==-1){
		printf("aborting, config file error\n");
		return -1;
	}
	
	// start a thread to slowly sample battery 
	pthread_t  battery_thread;
	pthread_create(&battery_thread, NULL, battery_checker, (void*) NULL);
	
	
	// start listening for RC control from dsm2 radio
	if(config.enable_dsm2){
		if(initialize_dsm2()<0){
				printf("failed to start DSM2\n");
		}
		else{
			pthread_t  dsm2_thread;
			pthread_create(&dsm2_thread, NULL, dsm2_watcher, (void*) NULL);
		}
	}
	
	// // start logging thread if enabled
	// if(config.enable_logging){
		// if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){
			// printf("failed to start log\n");
		// }
		// else{
			// // start new thread to write the file occationally
			// pthread_t  logging_thread;
			// pthread_create(&logging_thread, NULL, log_writer, (void*) NULL);
		// }
	// }
	
	// // first check for user options
	// if(parse_arguments(argc, argv)<0){
		// return -1;
	// }
	
	
	// // start logging thread if enabled
	// if(config.enable_logging){
		// if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){
			// printf("failed to start log\n");
		// }
		// else{
			// // start new thread to write the file occationally
			// pthread_t  logging_thread;
			// pthread_create(&logging_thread, NULL, log_writer, (void*) NULL);
		// }
	// }
	
	// // Start Safety checking thread
	// pthread_create(&safety_thread, NULL, safety_thread_func, (void*) NULL);
	
	
	// Finally start the real-time interrupt driven control thread
	// start IMU with equilibrium set with upright orientation 
	// for MiP with Ethernet pointing relatively up
	signed char orientation[9] = ORIENTATION_FLAT; 
	if(initialize_imu(SAMPLE_RATE_HZ, orientation)){
		// can't talk to IMU, all hope is lost
		// blink red until the user exits
		printf("IMU initialization failed, please reboot\n");
		blink_red();
		cleanup_cape();
		return -1;
	}
	// assigning the interrupt function and stack
	// should be the last step in initialization 
	// to make sure other setup functions don't interfere
	printf("starting core IMU interrupt\n");
	cstate.core_start_time_us = microsSinceEpoch();
	set_imu_interrupt_func(&flight_core);
	// start flight stack to control setpoints
	// this thread is in charge of arming and managing the core
	pthread_t  flight_stack_thread;
	pthread_create(&flight_stack_thread, NULL, flight_stack, (void*) NULL);
	
	printf("\nReady for arming sequence\n");
	set_state(RUNNING);
	
	// start printf_thread if running from a terminal
	// if it was started as a background process then don't bother
	if(isatty(fileno(stdout))){
		pthread_t  printf_thread;
		pthread_create(&printf_thread, NULL, printf_loop, (void*) NULL);
	}
	
	//chill until something exits the program
	while(get_state()!=EXITING){
		usleep(100000);
	}
	
	// cleanup before closing
	//close(sock); 	// mavlink UDP socket
	cleanup_cape();	// de-initialize cape hardware
	return 0;
}
Пример #4
0
// main() has a brief setup and starts one background thread.
// then it enters one big while loop for testing multiple capes.
int main(){
	int ret;
	float volt;
	imu_data_t data; // not really used, just necessary to test imu
	// use defaults for now, except also enable magnetometer.
	imu_config_t conf = get_default_imu_config();
	conf.enable_magnetometer=1;

	// counters for how many pass and fail
	num_passes = 0;
	num_fails = 0;

	// initialize_cape, this should never fail unless software is not set up
	// in which case a useful error message should be printed out.
	if(initialize_cape()<0){
		printf("initialize_cape() failed, this is a software issue,\n");
		printf("not a hardware issue. Try running install.sh and restart\n");
		return -1;
	}

	// set up the button handlers once
	set_pause_pressed_func(&on_pause_pressed);
	set_pause_released_func(&on_pause_released);
	set_mode_pressed_func(&on_mode_pressed);
	set_mode_released_func(&on_mode_released);

	// start blinking thread for 6V test
	pthread_create(&blinking_thread, NULL, blinking_function, (void*) NULL);

	// print welcome
	clear_screen();
	goto_line(0);
	printf("Welcome to the Robotics Cape tester!\n\n");
	printf("this will walk you through testing multiple capes and keep\n");
	printf("track of how many pass and fail.\n");
	printf("Closing the program erases the pass/fail count.\n\n");
	printf("Press enter to begin, anything else to quit.\n");
	if(continue_or_quit()<1){
		goto END;
	}

	/***************************************************************************
	* Begin main while loop
	***************************************************************************/
	while(get_state()!=EXITING){
		line = 0; // reset current printing line to top of terminal
		set_led(RED,OFF);
		set_led(GREEN,OFF);
		
		// clear screen and print pass/fail header
		clear_screen();
		goto_line(line);
		printf("passes: %d  fails: %d\n", num_passes, num_fails);
		line+=2;
		
		
		goto_line(INSTRUCTION_LINE-1);
		printf("*******************************************************************\n");
		printf("Place a new cape in the test jig but don't connect anything else.\n");
		printf("Press any key to start test.\n");
		
		// wait to start test
		if(continue_or_quit()<0){
			goto END;
		}

		/***********************************************************************
		* begin list of tests
		***********************************************************************/
		// make sure 12V DC supply is disconnected
CHECK_DC_DISCONNECT:
		volt = get_dc_jack_voltage();
		if(volt>2.0){
			clear_instruction_area();
			printf("Voltage detected on the DC jack input. This is supposed to be\n");
			printf("disconnected for this part of the test.\n");
			printf("Disconnect and hit ENTER to continue\n");
			printf("If the DC supply was disconnected, there may be a problem with resistors\n");
			printf("R1 or R14, press any key other than ENTER to FAIL this test.\n");
			ret = continue_or_quit();
			if(ret==1) goto CHECK_DC_DISCONNECT;
			else if(ret<0) goto END;
			else{
				goto_line(line);
				printf("FAILED	DC JACK VOLTAGE TEST\n");
				line++;
				fail_test();
				continue;
			}
		}

		// test imu
		ret = initialize_imu(&data, conf);
		power_off_imu();
		goto_line(line);
		line++;
		if(ret<0){
			printf("FAILED	MPU9250 IMU\n");
			fail_test();
			continue; // go to beginning to test next cape
		}
		printf("PASSED	MPU9250 IMU\n");

		// test barometer
		ret = initialize_barometer(BMP_OVERSAMPLE_16,BMP_FILTER_OFF);
		power_off_barometer();
		goto_line(line);
		line++;
		if(ret<0){
			printf("FAILED	BMP280 BAROMETER\n");
			fail_test();
			continue; // go to beginning to test next cape
		}
		printf("PASSED	BMP280 BAROMETER\n");

		// test buttons/LEDS
		clear_instruction_area();
		printf("Press the PAUSE button on cape, the RED led should light up.\n");
		printf("Press the MODE button on cape, the GREEN led should light up.\n");
		printf("Press ENTER to indicate the buttons/leds work\n");
		printf("Press any other key to indicate a failure\n");
		ret = continue_or_quit();
		goto_line(line);
		line++;
		if(ret==0){
			printf("FAILED	BUTTON/LED\n");
			fail_test();
			continue;
		}
		else if(ret<0) goto END;
		printf("PASSED	BUTTON/LED\n");

		// DC power jack ADC check
		clear_instruction_area();
		printf("Plug in the 12V power supply, GREEN CHG LED should turn on.\n");
		printf("Press ENTER if the GREEN CHG LED turns on\n");
		printf("Press any other key if not\n");
		ret = continue_or_quit();
		if(ret<0) goto END;
		goto_line(line);
		line++;
		if(ret==0){
			printf("FAILED	CHARGER\n");
			printf("CHG_IC may be bad.\n");
			fail_test();
			continue;
		}
		printf("PASSED	CHARGER\n");
		volt = get_dc_jack_voltage();
		if(volt<11.0 || volt>13.0){
			printf("FAILED	12V DC VOLTAGE\n");
			printf("measuring %0.2fV at DC jack, should be roughly 12V\n", volt);
			printf("Resistors R1 or R14 may be bad, shorted, or missing.\n");
			fail_test();
			continue;
		}
		line++;
		printf("PASSED	12V DC VOLTAGE\n");

		// 5V regulator test
		clear_instruction_area();
		printf("Plug in the 4-pin dongle to PWR socket\n");
		printf("Press ENTER if the dongle LED lights up, any other key if not.\n");
		ret = continue_or_quit();
		goto_line(line);
		line++;
		if(ret==0){
			printf("FAILED	5V REGULATOR\n");
			printf("Diode D3 or IC 5VREG may be bad\n");
			fail_test();
			continue;
		}
		else if(ret<0) goto END;
		printf("PASSED	5V REGULATOR\n");

		// battery ADC check
		clear_instruction_area();
		printf("Plug in 2-cell battery and press any key to continue\n");
		ret = continue_or_quit();
		if(ret<0) goto END;
		volt = get_battery_voltage();
		goto_line(line);
		line++;
		if(volt<5.0 || volt>9.0){
			printf("FAILED	BATTERY VOLTAGE\n");
			printf("measuring %0.2fV at battery, should be between 6 and 8.4\n", volt);
			printf("Resistors R19 or R25 may be bad, shorted, or missing.\n");
			fail_test();
			continue;
		}
		printf("PASSED	BATTERY VOLTAGE\n");

		// battery discharge check
		clear_instruction_area();
		printf("Disconnect the 12V DC power supply.\n");
		printf("If 4-pin dongle LED is still lit, press ENTER\n");
		printf("Otherwise press any other key.\n");
		ret = continue_or_quit();
		if(ret<0) goto END;
		goto_line(line);
		line++;
		if(ret==0){
			printf("FAILED	BATTERY DISCHARGE\n");
			printf("Diode D2, or mosfet Q3 are bad.\n");
			fail_test();
			continue;
		}
		printf("PASSED	BATTERY DISCHARGE\n");

		// 6V regulator check
		clear_instruction_area();
		printf("Plug in the 3-pin dongle into any of the 8 servo channels.\n");
		printf("If the dongle LED is blinking press ENTER.\n");
		printf("Otherwise press any other key.\n");
		ret = continue_or_quit();
		if(ret<0) goto END;
		goto_line(line);
		line++;
		if(ret==0){
			printf("FAILED	6VREG\n");
			printf("AOZ1284PI 6VREG or supporting components are bad.\n");
			fail_test();
			continue;
		}
		printf("PASSED	6VREG CHECK\n");

		// END OF TESTING THIS CAPE, PASSED!!!
		num_passes++;
		printf("COMPLETE TEST PASSED\n");
		goto_line(0);
		printf("passes: %d  fails: %d\n", num_passes, num_fails);
		clear_instruction_area();
		printf("Press any key to continue with next cape\n");
		continue_or_quit();
		if(ret<0) goto END;
		// now loop back to test next cape

	} // end while(get_state()!= EXITING)

		
	// if we got here there was a critical error or user hit ctrl+c
END:
	pthread_join(blinking_thread, NULL);
	disable_servo_power_rail();
	cleanup_cape();
	clear_screen();
	return 0;
}
Пример #5
0
/******************************************************************
*	main()
*	initialize the IMU, start all the threads, and wait still
*	something triggers a shut down
*******************************************************************/
int main(){
	initialize_cape();
	set_led(RED,HIGH);
	set_led(GREEN,LOW);
	set_state(UNINITIALIZED);
	
	// set up button handlers first
	// so user can exit by holding pause
	set_pause_pressed_func(&on_pause_press);
	set_mode_unpressed_func(&on_mode_release);
	
	// load data from disk.
	if(load_config(&config)==-1){
		printf("aborting, config file error\n");
		return -1;
	}


	
	// start a thread to slowly sample battery 
	pthread_t  battery_thread;
	pthread_create(&battery_thread, NULL, battery_checker, (void*) NULL);
	
	// start printf_thread if running from a terminal
	// if it was started as a background process then don't bother
	if(isatty(fileno(stdout))){
		pthread_t  printf_thread;
		pthread_create(&printf_thread, NULL, printf_loop, (void*) NULL);
	}
	
	// start listening for RC control from dsm2 radio
	if(config.enable_dsm2){
		initialize_dsm2();
		pthread_t  dsm2_thread;
		pthread_create(&dsm2_thread, NULL, dsm2_listener, (void*) NULL);
	}
	
	// start mavlink if enabled
	if(config.enable_mavlink_listening || config.enable_mavlink_listening){
		char target_ip[16];
		strcpy(target_ip, DEFAULT_MAV_ADDRESS);
		// open a udp port for mavlink
		// sock and gcAddr are global variables needed to send and receive
		gcAddr = initialize_mavlink_udp(target_ip, udp_sock);
		
		if(udp_sock != NULL){ 
			printf("WARNING: continuing without mavlink enabled\n");
		}
		else {
			if(config.enable_mavlink_listening){
				// start a thread listening for incoming packets
				pthread_t  mav_listen_thread;
				pthread_create(&mav_listen_thread, NULL, mavlink_listener, (void*) NULL);
				printf("Listening for Packets\n");
			}
			if(config.enable_mavlink_transmitting){
				// Start thread sending heartbeat and IMU attitude packets
				pthread_t  mav_send_thread;
				pthread_create(&mav_send_thread, NULL, mavlink_sender, (void*) NULL);
				printf("Transmitting Heartbeat Packets\n");
			}
		}
	}
	
	// start logging thread if enabled
	if(config.enable_logging){
		if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){
			printf("failed to start log\n");
		}
		else{
			// start new thread to write the file occationally
			pthread_t  logging_thread;
			pthread_create(&logging_thread, NULL, log_writer, (void*) NULL);
		}
	}
	
	// Finally start the real-time interrupt driven control thread
	// start IMU with equilibrium set with upright orientation 
	// for MiP with Ethernet pointing relatively up
	signed char orientation[9] = ORIENTATION_UPRIGHT; 
	if(initialize_imu(SAMPLE_RATE_HZ, orientation)){
		// can't talk to IMU, all hope is lost
		// blink red until the user exits
		blink_red();
		return -1;
	}
	// this should be the last step in initialization 
	// to make sure other setup functions don't interfere
	printf("starting core IMU interrupt\n");
	core_start_time_us = microsSinceEpoch();
	set_imu_interrupt_func(&balance_core);
	
	// start balance stack to control setpoints
	pthread_t  balance_stack_thread;
	pthread_create(&balance_stack_thread, NULL, balance_stack, (void*) NULL);
	
	printf("\nHold your MIP upright to begin balancing\n");
	set_state(RUNNING);
	
	// chill until something exits the program
	while(get_state()!=EXITING){
		usleep(100000);
	}
	
	// close(*udp_sock); 	// close network socket
	cleanup_cape(); // always end with cleanup to shut down cleanly
	return 0;
}