#include "drop_bone_imu.h"

#include "udp.h"

#include <stdio.h>

#include <stdlib.h>

#include <linux/i2c-dev.h>

#include <unistd.h>

#include <string.h>

#include <fcntl.h>

#include <math.h>

#include <poll.h>

#include <sys/ioctl.h>

#include <time.h>

static int fd; // file descriptor for the I2C bus

static signed char gyro_orientation[9] = {1, 0, 0,

0, 1, 0,

0, 0, 1};

int no_interrupt_flag;

int verbose_flag;

int print_usage_flag;

int silent_flag;

int no_broadcast_flag; //useful for testing when offline

int main(int argc, char** argv){

parse_args(argc, argv);

if(print_usage_flag) {

print_usage();

return 0;

}

init();

short accel[3], gyro[3], sensors[1];

long quat[4];

unsigned long timestamp;

unsigned char more[0];

struct pollfd fdset[1];

char buf[1];

// File descriptor for the GPIO interrupt pin

int gpio_fd = open(GPIO_INT_FILE, O_RDONLY | O_NONBLOCK);

// Create an event on the GPIO value file

memset((void*)fdset, 0, sizeof(fdset));

fdset[0].fd = gpio_fd;

fdset[0].events = POLLPRI;

while (1){

// Blocking poll to wait for an edge on the interrupt

if(!no_interrupt_flag) {

poll(fdset, 1, -1);

}

if (no_interrupt_flag || fdset[0].revents & POLLPRI) {

// Read the file to make it reset the interrupt

if(!no_interrupt_flag) {

read(fdset[0].fd, buf, 1);

}

int fifo_read = dmp_read_fifo(gyro, accel, quat, &timestamp, sensors, more);

if (fifo_read != 0) {

//printf("Error reading fifo.\n");

continue;

}

float angles[NOSENTVALS];

rescale_l(quat, angles+9, QUAT_SCALE, 4);

// rescale the gyro and accel values received from the IMU from longs that the

// it uses for efficiency to the floats that they actually are and store these values in the angles array

rescale_s(gyro, angles+3, GYRO_SCALE, 3);

rescale_s(accel, angles+6, ACCEL_SCALE, 3);

// turn the quaternation (that is already in angles) into euler angles and store it in the angles array

euler(angles+9, angles);

if(!silent_flag && verbose_flag) {

printf("Yaw: %+5.1f\tPitch: %+5.1f\tRoll: %+5.1f\n", angles[0]*180.0/PI, angles[1]*180.0/PI, angles[2]*180.0/PI);

}

// send the values in angles over UDP as a string (in udp.c/h)

if(!no_broadcast_flag) {

udp_send(angles, 13);

}

}

}

}

void parse_args(int argc, char** argv) {

int ch, opt_count;

no_interrupt_flag = 0;

verbose_flag = 0;

silent_flag = 0;

print_usage_flag = 0;

no_broadcast_flag = 0;

//flag i for no interrupt, v for no verbose

while((ch=getopt(argc, argv, "ivsbo:h?")) != -1) {

switch(ch) {

case 'i': no_interrupt_flag=1; break;

case 'v': verbose_flag=1; break;

case 's': silent_flag=1; break;

case 'b': no_broadcast_flag=1; break;

case 'o':

// Parse the orientation matrix

opt_count = sscanf(optarg, "%hhi,%hhi,%hhi,%hhi,%hhi,%hhi,%hhi,%hhi,%hhi", gyro_orientation, gyro_orientation+1,

gyro_orientation+2, gyro_orientation+3, gyro_orientation+4, gyro_orientation+5,

gyro_orientation+6, gyro_orientation+7, gyro_orientation+8);

if (opt_count != 9) {

printf("Must supply 9 comma separated values to the orientation option.\n");

exit(-1); // Exit because we don't know what the behaviour will be!

}

break;

case 'h':

case '?': print_usage_flag=1; break;

}

}

}

void print_usage() {

printf("DropBoneImu- software interface and broadcast client for MPU 6050:\n");

printf("Usage: dropboneimu [-i] [-v] [-s] [-b] [-h, -?]\n\n");

printf("Arguments:\n");

printf("-i\tDisable interrupt pin. Will make code run if not wired up with interrupt pin\n");

printf("-v\tVerbose mode. Print out yaw, pitch and roll as they are received\n");

printf("-s\tSilent mode. Do not print anything\n");

printf("-b\tNo broadcasts. Stops the udp server from broadcasting information received from the MPU over UDP\n");

printf("-o\tOrientation matrix of the robot coordinates relative to MPU 6050 coordinates.\n\t9 comma separated values representing X, Y and Z vectors of robot axes.\n");

printf("-h, -?\tDisplay this usage message, then exit\n");

}

int init(){

open_bus();

unsigned char whoami=0;

i2c_read(MPU6050_ADDR, MPU6050_WHO_AM_I, 1, &whoami);

if(!silent_flag) {

printf("WHO_AM_I: %x\n", whoami);

}

struct int_param_s int_param;

if(!silent_flag) {

printf("MPU init: %i\n", mpu_init(&int_param));

printf("MPU sensor init: %i\n", mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL));

printf("MPU configure fifo: %i\n", mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL));

printf("DMP firmware: %i\n ",dmp_load_motion_driver_firmware());

printf("DMP orientation: %i\n ",dmp_set_orientation(

inv_orientation_matrix_to_scalar(gyro_orientation)));

}

unsigned short dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;

if(!silent_flag) {

printf("DMP feature enable: %i\n", dmp_enable_feature(dmp_features));

printf("DMP set fifo rate: %i\n", dmp_set_fifo_rate(DEFAULT_MPU_HZ));

printf("DMP enable %i\n", mpu_set_dmp_state(1));

}

if(!no_interrupt_flag) {

mpu_set_int_level(1); // Interrupt is low when firing

dmp_set_interrupt_mode(DMP_INT_CONTINUOUS); // Fire interrupt on new FIFO value

}

return 0;

}

int i2c_write(unsigned char slave_addr, unsigned char reg_addr,

unsigned char length, unsigned char const *data){

unsigned char tmp[length+1];

tmp[0] = reg_addr;

memcpy(tmp+1, data, length);

if (write(fd, tmp, length+1) != length + 1){

return -1;

}

return 0;

}

int i2c_read(unsigned char slave_addr, unsigned char reg_addr,

unsigned char length, unsigned char *data){

if (write(fd,&reg_addr, 1) != 1){

return -1;

}

if (read(fd,data, length) != length){

return -2;

}

return 0;

}

int open_bus() {

if ((fd = open(BBB_I2C_FILE, O_RDWR)) < 0) {

/* ERROR HANDLING: you can check errno to see what went wrong */

perror("Failed to open the i2c bus");

return 1;

}

if (ioctl(fd, I2C_SLAVE, MPU6050_ADDR) < 0) {

perror("Failed to acquire bus access and/or talk to slave.\n");

/* ERROR HANDLING; you can check errno to see what went wrong */

return 1;

}

return 0;

}

// multiply two quaternions

int q_multiply(float* q1, float* q2, float* result) {

float tmp[4];

tmp[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];

tmp[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2];

tmp[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1];

tmp[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0];

memcpy(result, tmp, 4*sizeof(float));

return 0;

}

// rescale an array of longs by scale factor into an array of floats

int rescale_l(long* input, float* output, float scale_factor, char length) {

int i;

for(i=0;i<length;++i)

output[i] = input[i] * scale_factor;

return 0;

}

// rescale an array of shorts by scale factor into an array of floats

int rescale_s(short* input, float* output, float scale_factor, char length) {

int i;

for(i=0;i<length;++i)

output[i] = input[i] * scale_factor;

return 0;

}

void delay_ms(unsigned long num_ms){

}

void get_ms(unsigned long *count){

}

void reg_int_cb(struct int_param_s *param){

}

inline int min ( int a, int b ){

return a < b ? a : b;

}

inline void __no_operation(){

}

void euler(float* q, float* euler_angles) {

euler_angles[2] = atan2(2*(q[0]*q[1] + q[2]*q[3]), 1 - 2*(q[1]*q[1] + q[2]*q[2])); // phi, roll

euler_angles[1] = asin(2*(q[0]*q[2] + q[1]*q[3])); // theta, pitch

euler_angles[0] = atan2(2*(q[0]*q[3] - q[1]*q[2]), 1 - 2*(q[2]*q[2] + q[3]*q[3])); // psi, yaw

}

// Functions for setting gyro/accel orientation

unsigned short inv_row_2_scale(const signed char *row)

{

unsigned short b;

if (row[0] > 0)

b = 0;

else if (row[0] < 0)

b = 4;

else if (row[1] > 0)

b = 1;

else if (row[1] < 0)

b = 5;

else if (row[2] > 0)

b = 2;

else if (row[2] < 0)

b = 6;

else

b = 7; // error

return b;

}

unsigned short inv_orientation_matrix_to_scalar(

const signed char *mtx)

{

unsigned short scalar;

/*

XYZ 010_001_000 Identity Matrix

XZY 001_010_000

YXZ 010_000_001

YZX 000_010_001

ZXY 001_000_010

ZYX 000_001_010

*/

scalar = inv_row_2_scale(mtx);

scalar |= inv_row_2_scale(mtx + 3) << 3;

scalar |= inv_row_2_scale(mtx + 6) << 6;

return scalar;

}