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

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");
}

/**
 * @brief provides imu readings in a 50 Hz rate.
 *
 */
void loop() {
    if((millis()-timer)>=20) { // Main loop runs at 50Hz
        timer=millis();

        //Read data from the hardware

        gyro.read();
        compass.readAcc();
        compass.readMag();

        //Assign read data to the ros messages

        imu_msg.angular_velocity.x=gyro.g.x;
        imu_msg.angular_velocity.y=gyro.g.y;
        imu_msg.angular_velocity.z=gyro.g.z;

        imu_msg.linear_acceleration.x=compass.a.x;
        imu_msg.linear_acceleration.y=compass.a.y;
        imu_msg.linear_acceleration.z=compass.a.z;

        mag_msg.magnetic_field.x=compass.m.x;
        mag_msg.magnetic_field.y=compass.m.y;
        mag_msg.magnetic_field.z=compass.m.z;

        //Publish the data to the ros message system

        imu_pub.publish( &imu_msg );
        mag_pub.publish( &mag_msg);
        nh.spinOnce();
    }
    nh.spinOnce();
}

void Read_Gyro()
{
  gyro.read();
  
  AN[0] = gyro.g.x;
  AN[1] = gyro.g.y;
  AN[2] = gyro.g.z;
  gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]);
  gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]);
  gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]);
}

void task_gyro(void* p){
	
	
	/*gyro code*/
	while(1){
		
		gyro.read();// once gyro read is inside, code stops output. without gyro.read(), output is 0
	//	dprintf("X is %d, Y is %d, Z is %d", (int)gyro.g.x,(int)gyro.g.y,(int)gyro.g.z);
		//delay(1000);
		vTaskDelay(taskDelay);
		
	}

	/* gyro code*/
}

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

			   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";
//.........这里部分代码省略.........

/*
 * Main Loop 
 */
void loop() {
	wdt_reset();
	mD.vals.uslCount++;									//Increment main datarecord count
	AccelerometerScaled Ascaled = accel.ReadScaledAxis();	//Get Scaled Accelerometer
	AccelerometerRaw Araw = accel.ReadRawAxis();			//Get Raw Accelerometer
	MagnetometerScaled Mscaled = compass.ReadScaledAxis();	//Get Scaled Magnetometer
	MagnetometerRaw Mraw = compass.ReadRawAxis();			//Get Raw Magnetometer
	LGgyro.read();											//Get Gyro

	// offset compass by hard iron
	Mraw.XAxis += 40;
	Mraw.YAxis += 261;
	Mraw.ZAxis += 54;

	//write Acc, Mag, & Gyro values to record
	float AxisGs = Ascaled.XAxis;
	mD.vals.AcXPayload = AxisGs * 100;
	AxisGs = Ascaled.YAxis;
	mD.vals.AcYPayload = AxisGs * 100;
	AxisGs = Ascaled.ZAxis;
	mD.vals.AcZPayload = AxisGs * 100;
	mD.vals.MgXPayload = Mscaled.XAxis;
	mD.vals.MgYPayload = Mscaled.YAxis;
	mD.vals.MgZPayload = Mscaled.ZAxis;
	mD.vals.GyXPayload = LGgyro.g.x;
	mD.vals.GyYPayload = LGgyro.g.y;
	mD.vals.GyZPayload = LGgyro.g.z;

	//Perform tilt compensation calculation save to record
	sixDOF.compCompass(Mraw.XAxis, -Mraw.YAxis, -Mraw.ZAxis, Araw.XAxis, Araw.YAxis, Araw.ZAxis, true);
	float compHeading = sixDOF.atan2Int(sixDOF.xAxisComp(), sixDOF.yAxisComp());
	compHeading = compHeading /100;
	if (compHeading < 0 ) {
		compHeading = abs(compHeading);
	} else {
		compHeading = 180 - compHeading + 180;
	}
	mD.vals.CmpssPayload = compHeading;
	
	//get BMP085 values save to record
	dps.getTemperature(&TmpPayloadFULL); 
	dps.getPressure(&mD.vals.PressurePayload);

 	mD.vals.TmpPayload = (int16_t)(TmpPayloadFULL);	
	mD.vals.TmpExternal = (int16_t)(sensors.getTempC(outsideThermometer)* 10);
	sensors.requestTemperaturesByAddress(outsideThermometer); // Send the command to get temperatures
	
	//get GPS data
	byte lcount = 0;									//reset a loop counter
	while (!NEWGPSDATA && lcount++ < 255) {				//Exit the loop if we have new data or have been round it a number of times
		NEWGPSDATA = feedgps();							
	}
	if (NEWGPSDATA) {									//We have new GPS data, get all of the fields we need.
		int tmp_year = 0;
		gps.crack_datetime(&tmp_year, &mD.vals.month, &mD.vals.day,&mD.vals.hour, &mD.vals.minute, &mD.vals.second, &mD.vals.hundredths, &mD.vals.age);
		mD.vals.year = tmp_year - 2000;
		
        if (gps.altitude() != TinyGPS_HJOE::GPS_INVALID_ALTITUDE && gps.altitude() >= 0) {
			gps.get_position(&mD.vals.iLat, &mD.vals.iLong, &mD.vals.age);
			mD.vals.iAlt = gps.altitude(); 
			mD.vals.iAngle = gps.course();
			mD.vals.iHspeed = gps.speed(); 
			mD.vals.bSats = gps.satellites();
			mD.vals.ihdop = gps.hdop();
		}
		SET_LED_Status(SET_LED_BLUE,0);					//Flash blue to show we are getting GPS data
	} else {
		SET_LED_Status(SET_LED_GREEN,0);				//Flash Green to show that we are looping but not getting GPS data
	}

	if(ETSerialIn.receiveData()){

	}
  
	//flip flop between I2C's to avoid both on one loop
	if (SENDWIRE && (millis() - elapseSIM900) > WAIT_SIM900) {
		mD.vals.tCount++;
		ETI2Cout.sendData(I2C_SLV_SIM900_ADDRESS);		
		elapseSIM900 = millis();
	}

	if (!SENDWIRE && (millis() - elapseNTXB) > WAIT_NTXB) {
		mD.vals.tCount++;
		ETI2Cout.sendData(I2C_SLV_NTXB_ADDRESS);
		elapseNTXB = millis();
		//get I2C_SLV_SIM900_ADDRESS data
	}

	writeSDData();										//Write the data record to the SD card
	SET_LED_Status(SET_LED_OFF,0);						//turn off the LED
	NEWGPSDATA = false;									//Reset the New GPS Data flag
	SENDWIRE = !SENDWIRE;								//Flipflop this 
}