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 }
/** * @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(); }