C++ LSM303::read方法代码示例

void Compass_Calibrate() {
  long timer=millis();
  LSM303::vector running_min = {2047, 2047, 2047}, running_max = {-2048, -2048, -2048};   
  while((millis()-timer)<=60000){ //calibrate for 1 minutes
 
    compass.read();
    
    running_min.x = min(running_min.x, compass.m.x);
    running_min.y = min(running_min.y, compass.m.y);
    running_min.z = min(running_min.z, compass.m.z);
  
    running_max.x = max(running_max.x, compass.m.x);
    running_max.y = max(running_max.y, compass.m.y);
    running_max.z = max(running_max.z, compass.m.z);
    
    delay(100);
  }
  compass.m_max.x = running_max.x;
  compass.m_max.y = running_max.y;
  compass.m_max.z = running_max.z;
  compass.m_min.x = running_min.x;
  compass.m_min.y = running_min.y;
  compass.m_min.z = running_min.z;
  

}

void loop() {  
  compass.read();
  
  running_min.x = min(running_min.x, compass.m.x);
  running_min.y = min(running_min.y, compass.m.y);
  running_min.z = min(running_min.z, compass.m.z);

  running_max.x = max(running_max.x, compass.m.x);
  running_max.y = max(running_max.y, compass.m.y);
  running_max.z = max(running_max.z, compass.m.z);
  
  Serial.print("M min ");
  Serial.print("X: ");
  Serial.print((int)running_min.x);
  Serial.print(" Y: ");
  Serial.print((int)running_min.y);
  Serial.print(" Z: ");
  Serial.print((int)running_min.z);

  Serial.print(" M max ");  
  Serial.print("X: ");
  Serial.print((int)running_max.x);
  Serial.print(" Y: ");
  Serial.print((int)running_max.y);
  Serial.print(" Z: ");
  Serial.println((int)running_max.z);
  
  delay(100);
}

void task_accelerometer(void* p){
	
	
	/*accelerometer code*/
	while(1)
	{
		
		compass.read();
		//snprintf(report, sizeof(report), "A: %6d %6d %6d    M: %6d %6d %6d",
		//compass.a.x, compass.a.y, compass.a.z,
		//compass.m.x, compass.m.y, compass.m.z);
		//Serial.println(report);
		//dprintf( "A: %6d %6d %6d    M: %6d %6d %6d",
		//compass.a.x, compass.a.y, compass.a.z,
		//compass.m.x, compass.m.y, compass.m.z);
		//dprintf("%d",compass.a.x);
		//dprintf("%d",compass.a.x);
		//Serial.print(compass.a.x);
	//	Serial.print("1");
		//delay(1000);
		vTaskDelay(taskDelay);
	}
	/*accelerometer code*/
	
}

int main() {
    pc.printf("Starting \r\n");

    setup(); //initializes sensors

    t.start();
    timeLastPoll = t.read_ms();
    while(button){

        altitude = ps.pressureToAltitudeMeters(ps.readPressureMillibars());
        gyr.read();
        acc.read();

        fprintf(fp, "%f, %d, %d, %d \r\n",
            altitude,gyr.g.x,gyr.g.y,gyr.g.z);

        pc.printf("%d Att: %2.2f \tGyr: %d %d %d \tAcc: %d %d %d \tT: %d\r\n",
            iter,
            altitude,
            gyr.g.x,gyr.g.y,gyr.g.z,
            acc.a.x,acc.a.y,acc.a.z,
            t.read_ms()-timeLastPoll);

        while( (t.read_ms() - timeLastPoll) < MBED_POLLING_PERIOD){
        }
        // pc.printf("Loop Time: %d",t.read_ms()-timeLastPoll);
        timeLastPoll = t.read_ms();
        iter++;
    }
    fclose(fp);
    pc.printf("File successfully written! \r\n");
    printf("End of Program. \r\n");
}

void Accelerometer::update(){
	/*
  return
  */
	compass.read();

	lastValuesX[index] = compass.a.x;
	lastValuesY[index] = compass.a.y;
	lastValuesZ[index] = compass.a.z;

	long averagedX = averageArray(lastValuesX);
	long averagedY = averageArray(lastValuesY);
	long averagedZ = averageArray(lastValuesZ);

	index = index + 1;
	index = index % SENSITIVITY;

	long averageXY = sqrt(averagedX*averagedX + averagedY*averagedY);
	
       /*Serial.print(averagedX);
       Serial.print("      ");
       Serial.print(averagedY);
       Serial.print("      ");
       Serial.print(averageXY);
       Serial.print("      ");*/
	//int degree = atan2(averagedX, averagedY) * 180.0 / M_PI;
	//Serial.println(degree);

	if(averageXY > 3000)
	{
		Serial.println("SMASH!");
	}
}

void task_headingNdist(void* p){
	while(1){
		//Serial.print("z");
		compass.read();
		//dprintf("%d mem",(int)freeRAM());
		//Serial.print("a");
		float heading = compass.heading();
		//Serial.print("b");
		//dprintf("%d mem2",(int)freeRAM());
		
		//dprintf("%d",(int)compass.a.x);
		float ZaVal,XaVal,YaVal,RZaVal;
		//ZaVal = compass.a.z/16.0;
		XaVal = compass.a.x/16.0;
		YaVal = compass.a.y/16.0;
		ZaVal = compass.a.z/16.0;
		RZaVal = sqrt( ((XaVal*XaVal)+(YaVal*YaVal))/4.0 + (ZaVal*ZaVal));
		
		//ZaVal = sqrt( ((compass.a.x*compass.a.x)+(compass.a.y*compass.a.y))/4.0 + (compass.a.z*compass.a.z));
		//ZaVal = ZaVal/16.0;
		// dprintf("%d", (int)compass.a.x);
		//dprintf("%d", (int)((compass.a.x*compass.a.x)/*+(compass.a.y*compass.a.y)/4.0*/));
		// dprintf("%d x %d y %d z", (int)compass.a.x, (int)compass.a.y,(int) compass.a.z);
		// dprintf("%d z val",(int) ZaVal);
	//	dprintf("%d",(int) ZaVal);
	//Serial.print("a");
	
	//dprintf("%d",(int)heading);
	//dprintf("%d",(int)compass.heading());
	//compass.heading();
	//Serial.print("b");
		//data[ID_DATA_HEADING] = (int) compass.heading();
	/*	if(ZaVal<-965){
			distFromStart+=33;  //1 step is 33 cm
			step++;
		}*/
	float currentTime = millis();
	if(RZaVal>1000 && (currentTime - prevTime) >= 600){
		distFromStart+=33;  //1 step is 33 cm
		step++;
		dprintf("%d_RZaval",(int)RZaVal);
			dprintf("%d ",(int)step);
			prevTime = currentTime;
		}
		data[ID_DATA_DIST] = (int) distFromStart;
		//dprintf("dist is %d",(int)data[ID_DATA_DIST]);
		//dprintf("a");
		//delay(100);*/
	delay(100);
		vTaskDelay(taskDelay);
	}
	
}

int8_t Hal::LSM303_Update()
{
  int offset = 700;
  compass.read();

  d_x = abs(compass.m.x - ax_o);
  d_y = abs(compass.m.y - ay_o);
  d_z = abs(compass.m.z - az_o);

  if ( d_x > offset || d_y > offset || d_z > offset) 
  {
    hasMoved = true;
  }
  ax_o = compass.m.x;
  ay_o = compass.m.y;
  az_o = compass.m.z;
}

//.........这里部分代码省略.........

			   digitalWrite(TRIGGER_PIN4, HIGH);
			   delayMicroseconds(10);
			   
			   digitalWrite(TRIGGER_PIN4, LOW);
			   pinMode(ECHO_PIN4,INPUT);
			   duration = pulseIn(ECHO_PIN4, HIGH,100000);
			   
			   //Calculate the distance (in cm) based on the speed of sound.
			   distance = duration/58.2;
			//   dprintf("%d 4", (int)distance);
			   
			   
			    digitalWrite(TRIGGER_PIN5, LOW);
			    delayMicroseconds(2);

			    digitalWrite(TRIGGER_PIN5, HIGH);
			    delayMicroseconds(10);
			    
			    digitalWrite(TRIGGER_PIN5, LOW);
			    pinMode(ECHO_PIN5,INPUT);
			    duration = pulseIn(ECHO_PIN5, HIGH,100000);
			    
			    //Calculate the distance (in cm) based on the speed of sound.
			    distance = duration/58.2;
		//	    dprintf("%d 5", (int)distance);
			
			

			
		//Calculate the distance (in cm) based on the speed of sound.
		/*distance = duration/58.2;*/
		float distance1,distance2,distance3,distance4,distance5;
		distance1 = sonar_read(TRIGGER_PIN,ECHO_PIN);
		distance2= sonar_read(TRIGGER_PIN2,ECHO_PIN2);
		distance3 = sonar_read(TRIGGER_PIN3,ECHO_PIN3);
		distance4 = sonar_read(TRIGGER_PIN4,ECHO_PIN4);	
		distance5 = sonar_read(TRIGGER_PIN5,ECHO_PIN5);
		dprintf("%d %d %d %d %d",(int)distance1,(int)distance2,(int)distance3,(int)distance4,(int)distance5);
		
		/*dprintf("%d", (int) sonar_read(TRIGGER_PIN,ECHO_PIN));
		dprintf("%d", (int) sonar_read(TRIGGER_PIN2,ECHO_PIN2));
		dprintf("%d", (int) sonar_read(TRIGGER_PIN3,ECHO_PIN3));
		dprintf("%d", (int) sonar_read(TRIGGER_PIN4,ECHO_PIN4));
		dprintf("%d", (int) sonar_read(TRIGGER_PIN5,ECHO_PIN5));*/
		
		
 /*sonar final code
 digitalWrite(TRIGGER_PIN, LOW);
		  delayMicroseconds(2);

		  digitalWrite(TRIGGER_PIN, HIGH);
		  delayMicroseconds(10);
		  
		  digitalWrite(TRIGGER_PIN, LOW);
		  pinMode(ECHO_PIN,INPUT);
		  duration = pulseIn(ECHO_PIN, HIGH,100000);
		  
		  //Calculate the distance (in cm) based on the speed of sound.
		  distance = duration/58.2;
		  
		  */
		  
		  /*
		 pinMode(ECHO_PIN,INPUT);
		 digitalWrite(TRIGGER_PIN,HIGH);

void task_sensor_poll(void* p){
	
  while(1){
  
  /***********************************
  **        reading sensors
  ************************************/
  compass.read();
 /* float heading = compass.heading();
  float XaVal, YaVal, ZaVal, fXa, fYa,fZa, pitch, roll,pitch_print, roll_print;
  const float alpha = 0.15;
  XaVal = compass.a.x/16.0; //Acceleration data registers contain a left-aligned 12-bit number, so values should be shifted right by 4 bits (divided by 16)
  YaVal = compass.a.y/16.0; //unit is in cm/s2
  ZaVal = compass.a.z/16.0;
  /*
   
  /***********************************
  **       keypad
  ************************************/
  char key = keypad.getKey();

  //print out the key that is pressed 
  if (key != NO_KEY){
    Serial.print("You have pressed ");
    Serial.println(key);
  }

  /***********************************
  **       altitude
  ************************************/
  float pressure = ps.readPressureMillibars() + 248.5;
  float altitude = ps.pressureToAltitudeMeters(pressure);
  
/*  Serial.print("Pressure is ");
  Serial.print(pressure);
  Serial.println(" mbar");
  Serial.print("Altitude is ");
  Serial.print(altitude);
  Serial.println(" m.");
  
  /******************************************************
  **  gyro meter reading
  ******************************************************/
  gyro.read();
/*  Serial.println("Gyro meter ");
  Serial.print("X: ");
  Serial.print((int)gyro.g.x * 8.75 /1000);
  Serial.println(" degree/second");
  Serial.print("Y: ");
  Serial.print((int)gyro.g.y * 8.75 /1000);
  Serial.println(" degree/second");
  Serial.print("Z: ");
  Serial.print((int)gyro.g.z * 8.75 /1000);
  Serial.println(" degree/second");
  Serial.println("");




 /*******************************************************************
                          get Headings
  When given no arguments, the heading() function returns the angular
  difference in the horizontal plane between a default vector and
  north, in degrees.
  /*
  When given no arguments, the heading() function returns the angular
  difference in the horizontal plane between a default vector and
  north, in degrees.
  
  The default vector is chosen by the library to point along the
  surface of the PCB, in the direction of the top of the text on the
  silkscreen. This is the +X axis on the Pololu LSM303D carrier and
  the -Y axis on the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH
  carriers.
  
  To use a different vector as a reference, use the version of heading()
  that takes a vector argument; for example, use
  
    compass.heading((LSM303::vector<int>){0, 0, 1});
  
  to use the +Z axis as a reference.
  
  *******************************************************************/
 /* String direction = "";
  if(heading>=340 || heading <= 20)
    direction = "North";
  else if (heading>=70 && heading <= 110)
    direction = "East";
    else if (heading>=160 && heading <= 200)
    direction = "South";
    else if (heading>=250 && heading <= 290)
    direction = "West";
    
    
    else if (heading>20 && heading < 70)
    direction = "North East";
    else if (heading>110 && heading < 160)
    direction = "South East";
    else if (heading>200 && heading < 250)
    direction = "South West";
//.........这里部分代码省略.........