librobotcontrol-sys 0.4.0

/**
 * @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, interrupt pin is on gpio3.21
// change these for your platform
#define I2C_BUS 2
#define GPIO_INT_PIN_CHIP 3
#define GPIO_INT_PIN_PIN  21

// Global Variables
static int running = 0;
static int silent_mode = 0;
static int show_accel = 0;
static int show_gyro  = 0;
static int enable_mag = 0;
static int show_compass = 0;
static int show_temp  = 0;
static int show_quat  = 0;
static int show_tb = 0;
static int orientation_menu = 0;
static rc_mpu_data_t data;

// local functions
static rc_mpu_orientation_t __orientation_prompt(void);
static void __print_usage(void);
static void __print_data(void);
static void __print_header(void);


/**
 * Printed if some invalid argument was given, or -h option given.
 */
static void __print_usage(void)
{
	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.
 */
static void __print_data(void)
{
	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.
 */
static void __print_header(void)
{
	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
 */
static 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_chip = GPIO_INT_PIN_CHIP;
	conf.gpio_interrupt_pin = GPIO_INT_PIN_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;
}