hmc5883l::hmc5883l(i2cfile* i2c_ptr, ros::NodeHandle* nh_ptr, ros::Rate rate, hmc5883l_callBackFunc dataReadyCallBack = 0) {
ROS_INFO("HMC5883L : Initializing");
char str[80];
i2c = i2c_ptr;
sem_init(&lock, 0, 1);
ctl_reg_a = (0x03 << HMC5883L_MA0) | (0x06 << HMC5883L_DO0) | (0x00 << HMC5883L_MS0);
i2c->write_byte(addr, HMC5883L_CONF_REG_A, ctl_reg_a);
mode_reg = (0x00 << HMC5883L_MD0);
i2c->write_byte(addr, HMC5883L_MODE_REG, mode_reg);
setScale(hmc5883l_scale_130);
nh = nh_ptr;
timer = nh->createTimer(rate, &hmc5883l::timerCallback, this);
if (dataReadyCallBack)
dataCallback = dataReadyCallBack;
sprintf(str, "Sampling @ %2.2f Hz", (float) 1.0f / rate.expectedCycleTime().toSec());
ROS_INFO("HMC5883L : \t%s", str);
ROS_INFO("HMC5883L : Initialization done");
}
micromag::micromag(i2cfile* i2c_ptr, ros::NodeHandle* nh_ptr, ros::Rate rate,
micromag_callBackFunc dataReadyCallBack = 0, int windowSize = 32) {
ROS_INFO("micromag : Initializing");
char str[80];
i2c = i2c_ptr;
sem_init(&lock, 0, 1);
double freq = 1 / rate.expectedCycleTime().toSec();
setWindow(windowSize);
nh = nh_ptr;
timer = nh->createTimer(rate, µmag::timerCallback, this);
if (dataReadyCallBack)
dataCallback = dataReadyCallBack;
sprintf(str, "Sampling @ %2.2f Hz", freq);
ROS_INFO("micromag : \t%s", str);
ROS_INFO("micromag : Initialization done");
// Probe device... Return 0 on failure..
}