int LSM303D::init() { int ret = ERROR; int mag_ret; /* do SPI init (and probe) first */ if (SPI::init() != OK) { warnx("SPI init failed"); goto out; } /* allocate basic report buffers */ _accel_reports = new RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; /* advertise accel topic */ struct accel_report zero_report; memset(&zero_report, 0, sizeof(zero_report)); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) goto out; reset(); /* advertise mag topic */ struct mag_report zero_mag_report; memset(&zero_mag_report, 0, sizeof(zero_mag_report)); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report); /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); if (mag_ret != OK) { _mag_topic = -1; } ret = OK; out: return ret; }
int LSM303D::init() { int ret = ERROR; int mag_ret; /* do SPI init (and probe) first */ if (SPI::init() != OK) goto out; /* allocate basic report buffers */ _num_accel_reports = 2; _oldest_accel_report = _next_accel_report = 0; _accel_reports = new struct accel_report[_num_accel_reports]; if (_accel_reports == nullptr) goto out; /* advertise accel topic */ memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); _num_mag_reports = 2; _oldest_mag_report = _next_mag_report = 0; _mag_reports = new struct mag_report[_num_mag_reports]; if (_mag_reports == nullptr) goto out; reset(); /* advertise mag topic */ memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); if (mag_ret != OK) { _mag_topic = -1; } ret = OK; out: return ret; }
int LSM303D::init() { int ret = ERROR; int mag_ret; /* do SPI init (and probe) first */ if (SPI::init() != OK) { warnx("SPI init failed"); goto out; } /* allocate basic report buffers */ _accel_reports = new RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; /* advertise accel topic */ _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) goto out; reset(); /* do CDev init for the mag device node */ ret = _mag->init(); if (ret != OK) { warnx("MAG init failed"); goto out; } _class_instance = register_class_devname(ACCEL_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { // we are the primary accel device, so advertise to // the ORB struct accel_report zero_report; memset(&zero_report, 0, sizeof(zero_report)); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); } out: return ret; }
ssize_t LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); mag_report *mrb = reinterpret_cast<mag_report *>(buffer); int ret = 0; /* buffer must be large enough */ if (count < 1) return -ENOSPC; /* if automatic measurement is enabled */ if (_call_mag_interval > 0) { /* * While there is space in the caller's buffer, and reports, copy them. */ while (count--) { if (_mag_reports->get(mrb)) { ret += sizeof(*mrb); mrb++; } } /* if there was no data, warn the caller */ return ret ? ret : -EAGAIN; } /* manual measurement */ _mag_reports->flush(); _mag->measure(); /* measurement will have generated a report, copy it out */ if (_mag_reports->get(mrb)) ret = sizeof(*mrb); return ret; }
int LSM303D::init() { int ret = ERROR; /* do SPI init (and probe) first */ if (SPI::init() != OK) { warnx("SPI init failed"); goto out; } /* allocate basic report buffers */ _accel_reports = new RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; /* advertise accel topic */ _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) goto out; reset(); /* do CDev init for the mag device node */ ret = _mag->init(); if (ret != OK) { warnx("MAG init failed"); goto out; } /* fill report structures */ measure(); if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) { /* advertise sensor topic, measure manually to initialize valid report */ struct mag_report mrp; _mag_reports->get(&mrp); /* measurement will have generated a report, publish */ _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp); if (_mag->_mag_topic < 0) debug("failed to create sensor_mag publication"); } _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { /* advertise sensor topic, measure manually to initialize valid report */ struct accel_report arp; _accel_reports->get(&arp); /* measurement will have generated a report, publish */ _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); if (_accel_topic < 0) debug("failed to create sensor_accel publication"); } out: return ret; }
int LSM303D::init() { int ret = ERROR; int mag_ret; int fd_mag; /* do SPI init (and probe) first */ if (SPI::init() != OK) goto out; /* allocate basic report buffers */ _num_accel_reports = 2; _oldest_accel_report = _next_accel_report = 0; _accel_reports = new struct accel_report[_num_accel_reports]; if (_accel_reports == nullptr) goto out; /* advertise accel topic */ memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); _num_mag_reports = 2; _oldest_mag_report = _next_mag_report = 0; _mag_reports = new struct mag_report[_num_mag_reports]; if (_mag_reports == nullptr) goto out; /* advertise mag topic */ memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); /* enable accel, XXX do this with an ioctl? */ write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A); /* enable mag, XXX do this with an ioctl? */ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); /* XXX should we enable FIFO? */ set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */ set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ set_samplerate(400); /* max sample rate */ mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ mag_set_samplerate(100); /* XXX test this when another mag is used */ /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); if (mag_ret != OK) { _mag_topic = -1; } ret = OK; out: return ret; }