Exemplo n.º 1
0
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;
}
Exemplo n.º 2
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;

}
Exemplo n.º 3
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;

}
Exemplo n.º 4
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;
}
Exemplo n.º 5
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;

}