//// 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; }