Ejemplo n.º 1
0
/**
 * @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();
}
Ejemplo n.º 2
0
void Accel_Init()
{
	compass.init();
	compass.enableDefault();
	switch (compass.getDeviceType())
	{
		case LSM303::device_D:
		compass.writeReg(LSM303::CTRL2, 0x18); // 8 g full scale: AFS = 011
		break;
		case LSM303::device_DLHC:
		compass.writeReg(LSM303::CTRL_REG4_A, 0x28); // 8 g full scale: FS = 10; high resolution output mode
		break;
		default: // DLM, DLH
		compass.writeReg(LSM303::CTRL_REG4_A, 0x30); // 8 g full scale: FS = 11
	}
	//compass.heading((LSM303::vector<int>){0, 0, -1});
	//compass.m_min = (LSM303::vector<int16_t>){-2304, -2194, -2156};
	//compass.m_max = (LSM303::vector<int16_t>){+2771, +2860, +2753};
}