bool MPU9250::initialize(int sample_rate_div, int low_pass_filter)
{
uint8_t i = 0;
char MPU_Init_Data[MPU_InitRegNum][2] = {
//{0x80, MPUREG_PWR_MGMT_1}, // Reset Device - Disabled because it seems to corrupt initialisation of AK8963
{0x01, MPUREG_PWR_MGMT_1}, // Clock Source
{0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro
{low_pass_filter, MPUREG_CONFIG}, // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
{0x18, MPUREG_GYRO_CONFIG}, // +-2000dps
{0x08, MPUREG_ACCEL_CONFIG}, // +-4G
{0x09, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz
{0x30, MPUREG_INT_PIN_CFG}, //
//{0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz
//{0x20, MPUREG_USER_CTRL}, // Enable AUX
{0x20, MPUREG_USER_CTRL}, // I2C Master mode
{0x0D, MPUREG_I2C_MST_CTRL}, // I2C configuration multi-master IIC 400KHz
{AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for write.
//{0x09, MPUREG_I2C_SLV4_CTRL},
//{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
{AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
{0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
{0x81, MPUREG_I2C_SLV0_CTRL}, //Enable I2C and set 1 byte
{AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
{0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
{0x81, MPUREG_I2C_SLV0_CTRL} //Enable I2C and set 1 byte
};
//spi.format(8,0);
//spi.frequency(1000000);
for(i=0; i<MPU_InitRegNum; i++) {
WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
bcm2835_delayMicroseconds(100000); //I2C must slow down the write speed, otherwise it won't work
}
set_acc_scale(BITS_FS_16G);
set_gyro_scale(BITS_FS_2000DPS);
calib_mag();
return 0;
}
int main(int argc, char *argv[]) {
/////VARIABLE DECLARATIONS/////
//SENSORS
mraa_i2c_context accel, gyro, mag;
float a_res, g_res, m_res;
data_t accel_data, gyro_data, mag_data, zero_rate;
int16_t temperature;
float pitch_angle, roll_angle, yaw_angle;
char *x_accel_message;
char *y_accel_message;
char *z_accel_message;
char posture_message[20];
int curr_posture;
int prev_posture = 0;
//SOCKETS AND MESSAGES
int sockfd; //Socket descriptor
int portno;
char component[256];
char endpoint[] = "127.0.0.1";
struct sockaddr_in serv_addr;
struct hostent *server; //struct containing a bunch of info
char message[256];
char serv_message[256];
int count;
int n, n1;
/////SENSOR INITIALIZATION AND SETUP
accel = accel_init();
set_accel_scale(accel, A_SCALE_2G);
set_accel_ODR(accel, A_ODR_100);
a_res = calc_accel_res(A_SCALE_2G);
gyro = gyro_init();
set_gyro_scale(gyro, G_SCALE_245DPS);
set_gyro_ODR(accel, G_ODR_190_BW_70);
g_res = calc_gyro_res(G_SCALE_245DPS);
mag = mag_init();
set_mag_scale(mag, M_SCALE_2GS);
set_mag_ODR(mag, M_ODR_125);
m_res = calc_mag_res(M_SCALE_2GS);
zero_rate = calc_gyro_offset(gyro, g_res);
portno = 2015;
//create socket
if (argc > 1) //if user supplies IP address as argument
upperbodyIP = argv[1]; //take user input as upper body IP
else
upperbodyIP = "192.168.0.30";
sockfd = socket(AF_INET, SOCK_STREAM, 0); //create a new socket
if (sockfd < 0)
error("ERROR opening socket");
server = gethostbyname(upperbodyIP); //takes a string like "www.yahoo.com", and returns a struct hostent which contains information, as IP address, address type, the length of the addresses...
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(0);
}
//setup the server struct
memset((char *) &serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET; //initialize server's address
bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length);
serv_addr.sin_port = htons(portno);
//connect to server
if (connect(sockfd,(struct sockaddr *)&serv_addr,sizeof(serv_addr)) < 0) //establish a connection to the server
error("ERROR connecting");
//WAIT FOR THE OKAY BY THE SERVER//
memset(serv_message, 0, 256);
n = read(sockfd, serv_message, 256);
if (strcmp(serv_message, "START") != 0)
error("strange response from server");
//read accel and gyro data
count = 0;
while(1) {
accel_data = read_accel(accel, a_res);
gyro_data = read_gyro(gyro, g_res);
//mag_data = read_mag(mag, m_res);
//temperature = read_temp(accel);
getAngles(accel_data, gyro_data, zero_rate, &pitch_angle, &roll_angle, &yaw_angle);
printf("is moving: %d ", isMoving(gyro_data));
printf("yaw angle: %f ", yaw_angle);
if (count == 3) {
//send posture to main edison
if (isMoving(gyro_data)==0) //if patient is stationary, calculate new posture
curr_posture = getPosture(accel_data, pitch_angle, roll_angle);
else
curr_posture = UNDEFINED; //else just use the undefined/transition case
prev_posture = curr_posture; //set new value for prev posture
memset(posture_message, 0, sizeof(char)*20);
snprintf(posture_message, 10, "%d,%f", curr_posture, yaw_angle);
//posture_message = construct_message(POS, accel_data, curr_posture);
printf("posture message: %s ", posture_message);
n = write(sockfd, posture_message, strlen(posture_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
/*
bzero(message, 256);
printf("waiting for response ");
n1 = read(sockfd, message, 10);
if (n1 < 0)
error("ERROR reading from upper body");
printf("got response\n");
*/
count = 0;
}
printPostureString(curr_posture);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", gyro_data.x, gyro_data.y, gyro_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", mag_data.x, mag_data.y, mag_data.z);
//printf("\t%ld\n", temperature);
count++;
usleep(100000);
}
return 0;
}
int main(int argc, char *argv[]) {
/////VARIABLE DECLARATIONS/////
//SENSORS
mraa_i2c_context accel, gyro, mag;
float a_res, g_res, m_res;
data_t accel_data, gyro_data, mag_data;
int16_t temperature;
float pitch_angle, yaw_angle;
char *x_accel_message;
char *y_accel_message;
char *z_accel_message;
char *posture_message;
int curr_posture;
int prev_posture = 0;
//SOCKETS AND MESSAGES
int sockfd; //Socket descriptor
int portno, n;
char component[256];
char endpoint[] = "127.0.0.1";
struct sockaddr_in serv_addr;
struct hostent *server; //struct containing a bunch of info
char message[256];
int count;
/////SENSOR INITIALIZATION AND SETUP
accel = accel_init();
set_accel_scale(accel, A_SCALE_2G);
set_accel_ODR(accel, A_ODR_100);
a_res = calc_accel_res(A_SCALE_2G);
gyro = gyro_init();
set_gyro_scale(gyro, G_SCALE_245DPS);
set_gyro_ODR(accel, G_ODR_190_BW_70);
g_res = calc_gyro_res(G_SCALE_245DPS);
mag = mag_init();
set_mag_scale(mag, M_SCALE_2GS);
set_mag_ODR(mag, M_ODR_125);
m_res = calc_mag_res(M_SCALE_2GS);
portno = 41234;
//create socket
sockfd = socket(AF_INET, SOCK_DGRAM, 0); //create a new socket
if (sockfd < 0)
error("ERROR opening socket");
server = gethostbyname("127.0.0.1"); //takes a string like "www.yahoo.com", and returns a struct hostent which contains information, as IP address, address type, the length of the addresses...
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(0);
}
//setup the server struct
memset((char *) &serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET; //initialize server's address
bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length);
serv_addr.sin_port = htons(portno);
//connect to server
if (connect(sockfd,(struct sockaddr *)&serv_addr,sizeof(serv_addr)) < 0) //establish a connection to the server
error("ERROR connecting");
//read accel and gyro data
count = 0;
while(1) {
accel_data = read_accel(accel, a_res);
gyro_data = read_gyro(gyro, g_res);
//mag_data = read_mag(mag, m_res);
//temperature = read_temp(accel);
getAngles(accel_data, &pitch_angle, &yaw_angle);
printf("is moving: %d\n", isMoving(gyro_data) );
//printf("yaw angle: %f ", yaw_angle);
//printf("pitch angle: %f ", pitch_angle);
//printf("z vector: %f\n", accel_data.z);
if (count == 3) {
//send posture to cloud
if (isMoving(gyro_data)==0) //if patient is stationary, calculate new posture
curr_posture = getPosture(accel_data, pitch_angle, yaw_angle);
else
curr_posture = prev_posture; //else just use the old posture
if (curr_posture==UNDEFINED)
curr_posture = prev_posture;
prev_posture = curr_posture; //set new value for prev posture
posture_message = construct_message(POS, accel_data, curr_posture);
n = write(sockfd, posture_message, strlen(posture_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
//store accel data in string
/*
x_accel_message = construct_message(X_DIR, accel_data);
//printf("%s\n", full_message_x);
//send UDP message
n = write(sockfd,x_accel_message,strlen(x_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
y_accel_message = construct_message(Y_DIR, accel_data);
//printf("%s\n", full_message_y);
n = write(sockfd,y_accel_message,strlen(y_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
z_accel_message = construct_message(Z_DIR, accel_data);
//printf("%s\n", full_message_z);
n = write(sockfd,z_accel_message,strlen(z_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
*/
count = 0;
}
char *posture_string;
switch(curr_posture)
{
case 0:
posture_string = "upright";
break;
case 1:
posture_string = "face up";
break;
case 2:
posture_string = "face down";
break;
case 3:
posture_string = "left";
break;
case 4:
posture_string = "right";
break;
default:
posture_string = "undefined";
}
printf("current orientation: %s \n", posture_string);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", gyro_data.x, gyro_data.y, gyro_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", mag_data.x, mag_data.y, mag_data.z);
//printf("\t%ld\n", temperature);
count++;
usleep(100000);
}
return 0;
}
int main(int argc, char *argv[]) {
data_t accel_data, gyro_data;
data_t gyro_offset;
float a_res, g_res;
mraa_i2c_context accel, gyro;
accel_scale_t a_scale = A_SCALE_8G;
gyro_scale_t g_scale = G_SCALE_2000DPS;
int client_socket_fd, portno, n;
struct sockaddr_in serv_addr;
struct hostent *server;
int flags;
float buffer[BUFF_SIZE];
int send_idx = 0, curr_idx = 0;
int next_it, send_it, curr_it; // helper variables for converting idx (byte) to it (float)
int num_pts = 0;
int i;
int init_sam = (SAMP_RATE*INIT_SEC)+1; // number of samples during init period
int quit = 0; // quit flag
char stop; // dummy buffer for stop ping
// Read command line arguments, need to get the host IP address and port
if (argc < 3) {
fprintf(stderr, "USAGE: %s <hostname> <port>\n", argv[0]);
exit(1);
}
// Convert the arguments to the appropriate data types
portno = atoi(argv[2]);
// setup the socket
// technically PF_INET. Refer to:
// http://stackoverflow.com/questions/6729366/what-is-the-difference-between-af-inet-and-pf-inet-in-socket-programming
//client_socket_fd = socket(AF_INET, SOCK_STREAM, 0);
client_socket_fd = socket(PF_INET, SOCK_STREAM, 0);
// check if the socket was created successfully. If it wasnt, display an error and exit
if(client_socket_fd < 0) {
error("ERROR opening socket");
}
// check if the IP entered by the user is valid
server = gethostbyname(argv[1]);
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(1);
}
// clear our the serv_addr buffer
memset((char *) &serv_addr, 0, sizeof(serv_addr));
// set up the socket
serv_addr.sin_family = AF_INET;
memcpy((char *)&serv_addr.sin_addr.s_addr, (char *)server->h_addr, server->h_length);
serv_addr.sin_port = htons(portno);
// try to connect to the server
if (connect(client_socket_fd,(struct sockaddr *) &serv_addr,sizeof(serv_addr)) < 0){
error("ERROR connecting");
}
// make non-blocking. Refer to:
// http://developerweb.net/viewtopic.php?id=3000
/* Set socket to non-blocking */
if ((flags = fcntl(client_socket_fd, F_GETFL, 0)) < 0) {
error("ERROR getting socket flags");
}
if (fcntl(client_socket_fd, F_SETFL, flags | O_NONBLOCK) < 0) {
error("ERROR setting socket to non-blocking");
}
printf("Socket connected to server!\n");
printf("Now beginning sensor initialization\n");
// initialize sensors, set scale, and calculate resolution.
accel = accel_init();
set_accel_scale(accel, a_scale);
a_res = calc_accel_res(a_scale);
gyro = gyro_init();
set_gyro_scale(gyro, g_scale);
g_res = calc_gyro_res(g_scale);
gyro_offset = calc_gyro_offset(gyro, g_res);
printf("GYRO OFFSETS x: %f y: %f z: %f\n",
gyro_offset.x, gyro_offset.y, gyro_offset.z);
//Read the sensor data and print them.
printf("STARTING TO COLLECT DATA\n");
printf("\n \t\tAccelerometer\t\t\t||");
printf("\t\t\tGyroscope\t\t\t\n");
while (1) {
for (i = 0; i < PACK_PTS; i++) {
n = read(client_socket_fd, &stop, 1);
if (n < 0) {
if (!(errno == EWOULDBLOCK || errno == EAGAIN))
error("ERROR reading from client socket");
// else no stop ping yet
} else {
#ifdef DEBUG
printf("RECEIVED STOP PING\n");
#endif
quit = 1;
break;
}
if (num_pts == init_sam) printf("INITIALIZATION PERIOD DONE\n");
accel_data = read_accel(accel, a_res);
gyro_data = read_gyro(gyro, g_res);
if (num_pts%PRINT_PERIOD == 0) {
printf("%i points\n", num_pts);
printf(" send_idx: %d curr_idx: %d\n", send_idx, curr_idx);
printf(" X: %f\t Y: %f\t Z: %f\t||", accel_data.x, accel_data.y, accel_data.z);
printf("\tX: %f\t Y: %f\t Z: %f p: %d\n", gyro_data.x - gyro_offset.x, gyro_data.y - gyro_offset.y, gyro_data.z - gyro_offset.z, num_pts);
}
#ifdef DEBUG
printf("X: %f\t Y: %f\t Z: %f\t||", accel_data.x, accel_data.y, accel_data.z);
printf("\tX: %f\t Y: %f\t Z: %f p: %d\t\n", gyro_data.x - gyro_offset.x, gyro_data.y - gyro_offset.y, gyro_data.z - gyro_offset.z, num_pts);
#endif
curr_it = curr_idx/sizeof(float);
buffer[curr_it] = gyro_data.x - gyro_offset.x;
buffer[curr_it+1] = gyro_data.y - gyro_offset.y;
buffer[curr_it+2] = gyro_data.z - gyro_offset.z;
buffer[curr_it+3] = accel_data.x;
buffer[curr_it+4] = accel_data.y;
buffer[curr_it+5] = accel_data.z;
num_pts++;
next_it = (curr_it + SAMP_SIZE)%BUFF_SIZE;
send_it = send_idx/sizeof(float); // truncate integer
if ((next_it >= send_it) &&
(next_it <= (send_it+SAMP_SIZE-1))) {
// note that curr_idx always advances SAMP_SIZE at a time
// went around ring buffer. panic!
fprintf(stderr, "Ring buffer is full. Abort!\n");
exit(1);
}
curr_idx = next_it*sizeof(float);
} // batch done
// stop writing packets if received stop ping
if (quit) break;
// write batch to server
n = write(client_socket_fd, ((char*)buffer)+send_idx,
((curr_idx>send_idx)?
(curr_idx-send_idx):
(BUFF_BYTES-send_idx)));
// n contains how many bytes were written to the socket
// if n is less than 0, then there was an error
// Refer to:
// http://stackoverflow.com/questions/3153939/properly-writing-to-a-nonblocking-socket-in-c
// http://beej.us/guide/bgnet/output/html/singlepage/bgnet.html
if (n < 0) {
if (errno == EWOULDBLOCK || errno == EAGAIN) {
fprintf(stderr, "FAILED WRITE for byte %d. Currently at byte %d\n",
send_idx, curr_idx);
} else {
error("ERROR writing to socket");
}
} else { // successful write
send_idx = (send_idx+n)%BUFF_BYTES;
}
#ifdef DEBUG
printf("send_idx: %d curr_idx: %d\n", send_idx, curr_idx);
#endif
usleep(1000000/SAMP_RATE);
}
close(client_socket_fd);
return 0;
}
int main(int argc, char *argv[]) {
/////VARIABLE DECLARATIONS/////
//SENSORS
mraa_i2c_context accel, gyro, mag;
float a_res, g_res, m_res;
data_t accel_data, gyro_data, mag_data;
int16_t temperature;
char *x_accel_message;
char *y_accel_message;
char *z_accel_message;
//SOCKETS AND MESSAGES
int sockfd; //Socket descriptor
int portno, n;
char component[256];
char endpoint[] = "127.0.0.1";
struct sockaddr_in serv_addr;
struct hostent *server; //struct containing a bunch of info
char message[256];
int count;
/////SENSOR INITIALIZATION AND SETUP
accel = accel_init();
set_accel_scale(accel, A_SCALE_2G);
set_accel_ODR(accel, A_ODR_100);
a_res = calc_accel_res(A_SCALE_2G);
gyro = gyro_init();
set_gyro_scale(gyro, G_SCALE_245DPS);
set_gyro_ODR(accel, G_ODR_190_BW_70);
g_res = calc_gyro_res(G_SCALE_245DPS);
mag = mag_init();
set_mag_scale(mag, M_SCALE_2GS);
set_mag_ODR(mag, M_ODR_125);
m_res = calc_mag_res(M_SCALE_2GS);
/////SOCKET SETUP/////
//resolve arguments from command line
//if (argc < 2) {
// fprintf(stderr,"Error: need argument (component)\n");
// exit(0);
//}
//store arguments into variables
//strcpy(component, argv[1]);
//this is the port number that the edison reserves for the cloud?
portno = 41234;
//create socket
sockfd = socket(AF_INET, SOCK_DGRAM, 0); //create a new socket
if (sockfd < 0)
error("ERROR opening socket");
server = gethostbyname("127.0.0.1"); //takes a string like "www.yahoo.com", and returns a struct hostent which contains information, as IP address, address type, the length of the addresses...
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(0);
}
//setup the server struct
memset((char *) &serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET; //initialize server's address
bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length);
serv_addr.sin_port = htons(portno);
//connect to server
if (connect(sockfd,(struct sockaddr *)&serv_addr,sizeof(serv_addr)) < 0) //establish a connection to the server
error("ERROR connecting");
//lets only read acceleration form now
count = 0;
while(1) {
accel_data = read_accel(accel, a_res);
//gyro_data = read_gyro(gyro, g_res);
//mag_data = read_mag(mag, m_res);
//temperature = read_temp(accel);
if (count == 3) {
//store accel data in string
x_accel_message = construct_accel_message(X_DIR, accel_data);
//printf("%s\n", full_message_x);
//send UDP message
n = write(sockfd,x_accel_message,strlen(x_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
y_accel_message = construct_accel_message(Y_DIR, accel_data);
//printf("%s\n", full_message_y);
n = write(sockfd,y_accel_message,strlen(y_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
z_accel_message = construct_accel_message(Z_DIR, accel_data);
//printf("%s\n", full_message_z);
n = write(sockfd,z_accel_message,strlen(z_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
count = 0;
}
printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", gyro_data.x, gyro_data.y, gyro_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", mag_data.x, mag_data.y, mag_data.z);
//printf("\t%ld\n", temperature);
count++;
usleep(100000);
}
return 0;
}