rc_test_dmp.c

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