text: /**
   * @file rc_test_dmp.c
   * @example    rc_test_dmp
   *
   * @brief      serves as an example of how to use the MPU in DMP mode
   *
   *
   *
   * @author     James Strawson
   * @date       1/29/2018
   */
  
  
  #include <stdio.h>
  #include <getopt.h>
  #include <signal.h>
  #include <stdlib.h> // for atoi() and exit()
  #include <rc/mpu.h>
  #include <rc/time.h>
  
  // bus for Robotics Cape and BeagleboneBlue is 2, gpio int pin  is 117
  // change these for your platform
  #define I2C_BUS 2
  #define GPIO_INT_PIN 117
  
  // Global Variables
  int running;
  int silent_mode = 0;
  int show_accel = 0;
  int show_gyro  = 0;
  int enable_mag = 0;
  int show_compass = 0;
  int show_temp  = 0;
  int show_quat  = 0;
  int show_tb = 0;
  int orientation_menu = 0;
  rc_mpu_data_t data;
  
  // local functions
  rc_mpu_orientation_t orientation_prompt();
  void print_usage();
  void print_data();
  void print_header();
  
  
  /**
   * Printed if some invalid argument was given, or -h option given.
   */
  void print_usage()
  {
  	printf("\n Options\n");
  	printf("-r {rate}	Set sample rate in HZ (default 100)\n");
  	printf("		Sample rate must be a divisor of 200\n");
  	printf("-m		Enable Magnetometer\n");
  	printf("-b		Enable Reading Magnetometer before ISR (default after)\n");
  	printf("-c		Show raw compass angle\n");
  	printf("-a		Print Accelerometer Data\n");
  	printf("-g		Print Gyro Data\n");
  	printf("-T		Print Temperature\n");
  	printf("-t		Print TaitBryan Angles\n");
  	printf("-q		Print Quaternion Vector\n");
  	printf("-p {prio}	Set Interrupt Priority and FIFO scheduling policy (requires root)\n");
  	printf("-w		Print I2C bus warnings\n");
  	printf("-o		Show a menu to select IMU orientation\n");
  	printf("-h		Print this help message\n\n");
  
  	return;
  }
  
  /**
   * This is the IMU interrupt function.
   */
  void print_data()
  {
  	printf("\r");
  	printf(" ");
  
  	if(show_compass){
  		printf("   %6.1f   |", data.compass_heading_raw*RAD_TO_DEG);
  		printf("   %6.1f   |", data.compass_heading*RAD_TO_DEG);
  	}
  	if(show_quat && enable_mag){
  		// print fused quaternion
  		printf(" %4.1f %4.1f %4.1f %4.1f |",	data.fused_quat[QUAT_W], \
  							data.fused_quat[QUAT_X], \
  							data.fused_quat[QUAT_Y], \
  							data.fused_quat[QUAT_Z]);
  	}
  	else if(show_quat){
  		// print quaternion
  		printf(" %4.1f %4.1f %4.1f %4.1f |",	data.dmp_quat[QUAT_W], \
  							data.dmp_quat[QUAT_X], \
  							data.dmp_quat[QUAT_Y], \
  							data.dmp_quat[QUAT_Z]);
  	}
  	if(show_tb && enable_mag){
  		// print fused TaitBryan Angles
  		printf("%6.1f %6.1f %6.1f |",	data.fused_TaitBryan[TB_PITCH_X]*RAD_TO_DEG,\
  						data.fused_TaitBryan[TB_ROLL_Y]*RAD_TO_DEG,\
  						data.fused_TaitBryan[TB_YAW_Z]*RAD_TO_DEG);
  	}
  	else if(show_tb){
  		// print TaitBryan angles
  		printf("%6.1f %6.1f %6.1f |",	data.dmp_TaitBryan[TB_PITCH_X]*RAD_TO_DEG,\
  						data.dmp_TaitBryan[TB_ROLL_Y]*RAD_TO_DEG,\
  						data.dmp_TaitBryan[TB_YAW_Z]*RAD_TO_DEG);
  	}
  	if(show_accel){
  		printf(" %5.2f %5.2f %5.2f |",	data.accel[0],\
  						data.accel[1],\
  						data.accel[2]);
  	}
  	if(show_gyro){
  		printf(" %5.1f %5.1f %5.1f |",	data.gyro[0],\
  						data.gyro[1],\
  						data.gyro[2]);
  	}
  	if(show_temp){
  		rc_mpu_read_temp(&data);
  		printf(" %6.2f |", data.temp);
  	}
  	fflush(stdout);
  	return;
  }
  
  /**
   * Based on which data is marked to be printed, print the correct labels. this
   * is printed only once and the actual data is updated on the next line.
   */
  void print_header(){
  	printf(" ");
  	if(show_compass){
  		printf("Raw Compass |");
  		printf("FilteredComp|");
  	}
  	if(enable_mag){
  		if(show_quat) printf("   Fused Quaternion  |");
  		if(show_tb) printf(" FusedTaitBryan(deg) |");
  	} else{
  		if(show_quat) printf("    DMP Quaternion   |");
  		if(show_tb) printf(" DMP TaitBryan (deg) |");
  	}
  	if(show_accel) printf(" Accel XYZ (m/s^2) |");
  	if(show_gyro)  printf("  Gyro XYZ (deg/s) |");
  	if(show_temp)  printf(" Temp(C)|");
  
  	printf("\n");
  }
  
  /**
   * @brief      interrupt handler to catch ctrl-c
   */
  void signal_handler(__attribute__ ((unused)) int dummy)
  {
  	running=0;
  	return;
  }
  
  /**
   * If the user selects the -o option for orientation selection, this menu will
   * displayed to prompt the user for which orientation to use. It will return a
   * valid rc_mpu_orientation_t when a number 1-6 is given or quit when 'q' is
   * pressed. On other inputs the user will be allowed to enter again.
   *
   * @return     the orientation enum chosen by user
   */
  rc_mpu_orientation_t orientation_prompt(){
  	int c;
  
  	printf("\n");
  	printf("Please select a number 1-6 corresponding to the\n");
  	printf("orientation you wish to use. Press 'q' to exit.\n\n");
  	printf(" 1: ORIENTATION_Z_UP\n");
  	printf(" 2: ORIENTATION_Z_DOWN\n");
  	printf(" 3: ORIENTATION_X_UP\n");
  	printf(" 4: ORIENTATION_X_DOWN\n");
  	printf(" 5: ORIENTATION_Y_UP\n");
  	printf(" 6: ORIENTATION_Y_DOWN\n");
  	printf(" 7: ORIENTATION_X_FORWARD\n");
  	printf(" 8: ORIENTATION_X_BACK\n");
  
  	while ((c = getchar()) != EOF){
  		switch(c){
  		case '1':
  			return ORIENTATION_Z_UP;
  			break;
  		case '2':
  			return ORIENTATION_Z_DOWN;
  			break;
  		case '3':
  			return ORIENTATION_X_UP;
  			break;
  		case '4':
  			return ORIENTATION_X_DOWN;
  			break;
  		case '5':
  			return ORIENTATION_Y_UP;
  			break;
  		case '6':
  			return ORIENTATION_Y_DOWN;
  			break;
  		case '7':
  			return ORIENTATION_X_FORWARD;
  			break;
  		case '8':
  			return ORIENTATION_X_BACK;
  			break;
  		case 'q':
  			printf("Quitting\n");
  			exit(0);
  		case '\n':
  			break;
  		default:
  			printf("invalid input\n");
  			break;
  		}
  	}
  	return 0;
  }
  
  /**
   * main() serves to parse user options, initialize the imu and interrupt
   * handler, and wait for the rc_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 rc_mpu_set_dmp_callback().
   *
   * @param[in]  argc  The argc
   * @param      argv  The argv
   *
   * @return     0 on success -1 on failure
   */
  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
  	rc_mpu_config_t conf = rc_mpu_default_config();
  	conf.i2c_bus = I2C_BUS;
  	conf.gpio_interrupt_pin = GPIO_INT_PIN;
  
  	// parse arguments
  	opterr = 0;
  	while ((c=getopt(argc, argv, "sr:mbagrqTtcp:hwo"))!=-1 && argc>1){
  		switch (c){
  		case 's':
  			silent_mode = 1;
  			show_something = 1;
  			break;
  		case 'r': // 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);
  			conf.dmp_interrupt_priority = priority;
  			conf.dmp_interrupt_sched_policy = SCHED_FIFO;
  			break;
  		case 'm': // magnetometer option
  			show_something = 1;
  			enable_mag = 1;
  			conf.enable_magnetometer = 1;
  			break;
  		case 'b': // magnetometer option
  			conf.read_mag_after_callback = 0;
  			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;
  			conf.dmp_fetch_accel_gyro=1;
  			break;
  		case 'g': // show gyro option
  			show_something = 1;
  			show_gyro = 1;
  			conf.dmp_fetch_accel_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.orient=orientation_prompt();
  	}
  	// set signal handler so the loop can exit cleanly
  	signal(SIGINT, signal_handler);
  	running = 1;
  
  	// now set up the imu for dmp interrupt operation
  	if(rc_mpu_initialize_dmp(&data, conf)){
  		printf("rc_mpu_initialize_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();
  	if(!silent_mode) rc_mpu_set_dmp_callback(&print_data);
  	//now just wait, print_data() will be called by the interrupt
  	while(running)	rc_usleep(100000);
  
  	// shut things down
  	rc_mpu_power_off();
  	printf("\n");
  	fflush(stdout);
  	return 0;
  }
  
  