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

/**
 * @brief Initializes the gyro/accelerometer and the magnetometer unit of the imu.
 * 		As well as the arduino subsystem
 */
void setup() {
    Wire.begin();
    delay(1500);

    /********/
    /* GYRO */
    /********/
    gyro.init();
    gyro.writeReg(L3G_CTRL_REG4, 0x00); // 245 dps scale
    gyro.writeReg(L3G_CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
    //8.75 mdps/LSB

    /****************/
    /* MAGNETOMETER */
    /****************/
    compass.init();
    compass.enableDefault();
    compass.writeReg(LSM303::CTRL2, 0x08); // 4 g scale: AFS = 001
    //0.122 mg/LSB
    compass.writeReg(LSM303::CTRL5, 0x10); // Magnetometer Low Resolution 50 Hz
    //Magnetometer 4 gauss scale : 0.16mgauss/LSB


    //ROS-TF base frame of the imu_data
    imu_msg.header.frame_id="base_imu_link";

    //Register ROS messages
    nh.initNode();
    nh.advertise(imu_pub);
    nh.advertise(mag_pub);

    //starting value for the timer
    timer=millis();
}

void Gyro_Init()
{
	gyro.init();
	gyro.enableDefault();
	gyro.writeReg(L3G::CTRL_REG4, 0x20); // 2000 dps full scale
	gyro.writeReg(L3G::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
}

void setup(){
	
	Serial.begin(115200);
	Wire.begin(); //gyro code
	gyro.init();//gyro code
	gyro.enableDefault(); //gyro code
	ps.enableDefault(); //baro code
	
	 handshaken = 0;
		
		/*accelerometer code*/
		compass.init();
		compass.enableDefault();
		/*accelerometer code*/
		/*sonar code*/
		pinMode(TRIGGER_PIN, OUTPUT);
		pinMode(ECHO_PIN, INPUT);
		
		pinMode(TRIGGER_PIN2, OUTPUT);
		pinMode(ECHO_PIN2, INPUT);
		pinMode(TRIGGER_PIN3, OUTPUT);
		pinMode(ECHO_PIN3, INPUT);
		pinMode(TRIGGER_PIN4, OUTPUT);
		pinMode(ECHO_PIN4, INPUT);
		pinMode(TRIGGER_PIN5, OUTPUT);
		pinMode(ECHO_PIN5, INPUT);
		
		/*sonar code*/
		   pinMode(MOTOR, OUTPUT);
		   
		   
		   /*
  Calibration values; the default values of +/-32767 for each axis
  lead to an assumed magnetometer bias of 0. Use the ACCalibrate 
  program to determine appropriate values for your particular unit.
  */
  //compass.m_min = (LSM303::vector<int16_t>){-32767, -32767, -32767};
  //compass.m_max = (LSM303::vector<int16_t>){+32767, +32767, +32767};
  
  //LSM303::vector<int16_t> running_min = {-2872, -3215, -1742}, running_max = {+3019, +3108, +3570}; //calibration init data for compass 
  compass.m_min = (LSM303::vector<int16_t>){-2872, -3215, -1742};
  compass.m_max = (LSM303::vector<int16_t>) {+3019, +3108, +3570};
}

void setupSensor(){
    // Sensor Set Up
    rled = 1; gled = 1; bled = 1;

    autoSD.ready_datalogger(); // Prepares a datalog filename according to index.txt
    pc.printf("Index: %d\r\n",autoSD.curr_index());         
    fp = fopen(autoSD.filepath, "w");
    if (fp == NULL) {
        pc.printf("Unable to write the file \r\n");
        while(1){ bled = !bled; wait_ms(100);}
    }

    if(!ps.init()){ //enable pressure sensor
        pc.printf("Unable to talk to Barometer\r\n");
        while(1){ rled = !rled; wait_ms(100);}
    }
    else{
        ps.enableFIFO();
    }

    if(!gyr.init()){
        pc.printf("Unable to talk to Gyroscope\r\n");
        while(1){ rled = !rled; wait_ms(100);}
    }
    else{
        gyr.enableFIFO();
    }

    if(!acc.init()){
        pc.printf("Unable to talk to Accelerometer\r\n");
        while(1){ rled = !rled; wait_ms(100);}
    }
    else{
        acc.enableFIFO();
    }


    pc.printf("Sensor set up successfully\r\n");

}

/*
 * Setup 
 */
void setup() {
	wdt_enable(WDTO_8S);
	wdt_reset();
	//Setup Ports
	Serial.begin(115200);				//Start Debug Serial 0
	Serial1.begin(9600); 				//Start GPS Serial 1
	Serial2.begin(9600);
 
	pinMode(PIN_LED_GREEN, OUTPUT);		//Blue GREEN
	pinMode(PIN_LED_RED, OUTPUT);		//Blue RED
	pinMode(PIN_LED_BLUE, OUTPUT);		//Blue LED
	pinMode(PIN_SPI_CS,OUTPUT);  		//Chip Select Pin for the SD Card
	pinMode(10, OUTPUT);				//SDcard library expect 10 to set set as output.
	
	// Initialise the GPS
	wdt_disable();
	gps.init();						
	gps.configureUbloxSettings();		// Configure Ublox for MY_HIGH altitude mode
	wdt_enable(WDTO_8S);
	// join I2C bus //start I2C transfer to the Module/Transmitter
	Wire.begin();
	//Set up the two EasyTransfer methods
	ETI2Cout.begin(details(mD.i2cOut), &Wire);	//setup the data structure to transfer out
	ETSerialIn.begin(details(vals), &Serial2);
	
	//Start up the LGgyro
    if (LGgyro.init()) {
		#ifdef DEBUG_ON	
			Serial.println("LGgyro OK");
		#endif
		LGgyro.enableDefault();
	} else {
		#ifdef DEBUG_ON	
			Serial.println("LGgyro not working");
		#endif
		SET_LED_Status(SET_LED_WHITE,500); 	//White LED
		SET_LED_Status(SET_LED_RED,1000); 	//Red LED 
	}

	//Start up the accelerometer
	accel = ADXL345(); 						// Create an instance of the accelerometer
	if(accel.EnsureConnected()) {			// Check that the accelerometer is connected.
		#ifdef DEBUG_ON	
			Serial.println("Connected to ADXL345.");
		#endif		
		accel.SetRange(2, true);				// Set the range of the accelerometer to a maximum of 2G.
		accel.EnableMeasurements();				// Tell the accelerometer to start taking measurements.		
	} else{
		#ifdef DEBUG_ON	
			Serial.println("Could not connect to ADXL345.");
		#endif
		SET_LED_Status(SET_LED_WHITE,500); 	//White LED
		SET_LED_Status(SET_LED_RED,2000); 	//Red LED 
	}

	//Start up the compass
	compass = HMC5883L(); 						// Construct a new HMC5883 compass.
	#ifdef DEBUG_ON	
		if(compass.EnsureConnected() == 1) {
			Serial.println("Connected to HMC5883L.");
		} else {
			Serial.println("Not Connected to HMC5883L.");
		}
	#endif
	error = compass.SetScale(1.3); 				// Set the scale of the compass.
	#ifdef DEBUG_ON	
		if(error != 0) {							// If there is an error, print it out.
			Serial.println("Compass Error 1");
			Serial.println(compass.GetErrorText(error));
		} else {
			Serial.println("Compass Ok 1");
		}
	#endif
	error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
	#ifdef DEBUG_ON	
		if(error != 0) {							// If there is an error, print it out.
			Serial.println("Compass error 2");
			Serial.println(compass.GetErrorText(error));
		} else {
			Serial.println("Compass Ok 2");
		}
	#endif	
	
	//Start up the Pressure Sensor
	dps = BMP085();
	dps.init(); 
	#ifdef DEBUG_ON
		Serial.print("BMP Mode ");
		Serial.println(dps.getMode());
	#endif	
	wdt_reset();
	// Start up the OneWire Sensors library and turn off blocking takes too long!
	sensors.begin();
	sensors.setWaitForConversion(false);
  	sensors.requestTemperaturesByAddress(outsideThermometer); // Send the command to get temperature
	
	//Initialise all of the record values
//.........这里部分代码省略.........