//// MPU9150 IMU ////
int initialize_imu(int sample_rate, signed char orientation[9]){
printf("Initializing IMU\n");
//set up gpio interrupt pin connected to imu
if(gpio_export(INTERRUPT_PIN)){
printf("can't export gpio %d \n", INTERRUPT_PIN);
return (-1);
}
gpio_set_dir(INTERRUPT_PIN, INPUT_PIN);
gpio_set_edge(INTERRUPT_PIN, "falling"); // Can be rising, falling or both
linux_set_i2c_bus(1);
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
mpu_set_sample_rate(sample_rate);
// compass run at 100hz max.
if(sample_rate > 100){
// best to sample at a fraction of the gyro/accel
mpu_set_compass_sample_rate(sample_rate/2);
}
else{
mpu_set_compass_sample_rate(sample_rate);
}
mpu_set_lpf(188); //as little filtering as possible
dmp_load_motion_driver_firmware(sample_rate);
dmp_set_orientation(inv_orientation_matrix_to_scalar(orientation));
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL);
dmp_set_fifo_rate(sample_rate);
if (mpu_set_dmp_state(1)) {
printf("\nmpu_set_dmp_state(1) failed\n");
return -1;
}
if(loadGyroCalibration()){
printf("\nGyro Calibration File Doesn't Exist Yet\n");
printf("Use calibrate_gyro example to create one\n");
printf("Using 0 offset for now\n");
};
// set the imu_interrupt_thread to highest priority since this is used
// for real-time control with time-sensitive IMU data
set_imu_interrupt_func(&null_func);
pthread_t imu_interrupt_thread;
struct sched_param params;
pthread_create(&imu_interrupt_thread, NULL, imu_interrupt_handler, (void*) NULL);
params.sched_priority = sched_get_priority_max(SCHED_FIFO);
pthread_setschedparam(imu_interrupt_thread, SCHED_FIFO, ¶ms);
return 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;
}
/***********************************************************************
* 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;
}
/******************************************************************
* 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;
}
/*******************************************************************************
* int main(int argc, char *argv[])
*
* main() serves to parse user options, initialize the imu and interrupt handler,
* and wait for the get_state()==EXITING condition before exiting cleanly.
* The imu_interrupt function print_data() is what actually prints new imu data
* to the screen after being set with set_imu_interrupt_func().
*******************************************************************************/
int main(int argc, char *argv[]){
int c, sample_rate, priority;
int show_something = 0; // set to 1 when any show data option is given.
// start with default config and modify based on options
imu_config_t conf = get_default_imu_config();
// parse arguments
opterr = 0;
while ((c=getopt(argc, argv, "s:magrqtcp:hwo"))!=-1 && argc>1){
switch (c){
case 's': // sample rate option
sample_rate = atoi(optarg);
if(sample_rate>200 || sample_rate<4){
printf("sample_rate must be between 4 & 200");
return -1;
}
conf.dmp_sample_rate = sample_rate;
break;
case 'p': // priority option
priority = atoi(optarg);
const int max_pri = sched_get_priority_max(SCHED_FIFO);
if(priority>max_pri || priority<0){
printf("priority must be between 0 & %d\n",max_pri);
return -1;
}
conf.dmp_interrupt_priority = priority;
break;
case 'm': // magnetometer option
show_something = 1;
enable_mag = 1;
conf.enable_magnetometer = 1;
break;
case 'c': // compass option
show_something = 1;
enable_mag = 1;
show_compass = 1;
conf.enable_magnetometer = 1;
break;
case 'a': // show accelerometer option
show_something = 1;
show_accel = 1;
break;
case 'g': // show gyro option
show_something = 1;
show_gyro = 1;
break;
case 'q': // show quaternion option
show_something = 1;
show_quat = 1;
break;
case 't': // show TaitBryan angle option
show_something = 1;
show_tb = 1;
break;
case 'T': // read thermometer option
show_something = 1;
show_temp = 1;
break;
case 'w': // print warnings
conf.show_warnings=1;
break;
case 'o': // let user select imu orientation
orientation_menu=1;
break;
case 'h': // show help option
print_usage();
return -1;
break;
default:
printf("opt: %c\n",c);
printf("invalid argument\n");
print_usage();
return -1;
break;
}
}
// user didn't give an option to show anything. Print warning and return.
if(show_something==0){
print_usage();
printf("please enable an option to print some data\n");
return -1;
}
// If the user gave the -o option to select an orientation then prompt them
if(orientation_menu){
conf.orientation=orientation_prompt();
}
// start by initializing cape as always
if(initialize_cape()){
printf("ERROR: failed to initialize_cape\n");
return -1;
}
// now set up the imu for dmp interrupt operation
if(initialize_imu_dmp(&data, conf)){
printf("initialize_imu_failed\n");
return -1;
}
// write labels for what data will be printed and associate the interrupt
// function to print data immediately after the header.
print_header();
set_imu_interrupt_func(&print_data);
//now just wait, print_data() will be called by the interrupt
while (get_state()!=EXITING) {
usleep(10000);
}
// shut things down
power_off_imu();
cleanup_cape();
return 0;
}
