int HMC5883::init() { int ret = ERROR; /* do I2C init (and probe) first */ if (I2C::init() != OK) goto out; /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(mag_report)); if (_reports == nullptr) goto out; /* reset the device configuration */ reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the mag topic if we are * the primary mag */ struct mag_report zero_report; memset(&zero_report, 0, sizeof(zero_report)); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); if (_mag_topic < 0) debug("failed to create sensor_mag object"); } ret = OK; /* sensor is ok, but not calibrated */ _sensor_ok = true; out: return ret; }
PX4Magnetometer::PX4Magnetometer(const char *path, device::Device *interface, uint8_t dev_type, enum Rotation rotation, float scale) : CDev(path), _interface(interface) { _device_id.devid = _interface->get_device_id(); // _device_id.devid_s.bus_type = (device::Device::DeviceBusType)_interface->get_device_bus_type(); // _device_id.devid_s.bus = _interface->get_device_bus(); // _device_id.devid_s.address = _interface->get_device_address(); _device_id.devid_s.devtype = dev_type; CDev::init(); _class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH); _rotation = rotation; _scale = scale; _cal.x_offset = 0; _cal.x_scale = 1.0f; _cal.y_offset = 0; _cal.y_scale = 1.0f; _cal.z_offset = 0; _cal.z_scale = 1.0f; }
int LIS3MDL::init() { int ret = PX4_ERROR; ret = CDev::init(); if (ret != OK) { DEVICE_DEBUG("CDev init failed"); return ret; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); if (_reports == nullptr) { return PX4_ERROR; } /* reset the device configuration */ reset(); _class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); return PX4_OK; }
int LidarLitePWM::init() { /* do regular cdev init */ int ret = CDev::init(); if (ret != OK) { return ERROR; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s)); if (_reports == nullptr) { return ERROR; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the distance_sensor topic */ struct distance_sensor_s ds_report; measure(); _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { debug("failed to create distance_sensor object. Did you start uORB?"); } } return OK; }
int PX4FMU::init() { int ret; assert(!_initialized); /* do regular cdev init */ ret = CDev::init(); if (ret != OK) { return ret; } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { /* lets not be too verbose */ } else if (_class_instance < 0) { warnx("FAILED registering class device\n"); } work_start(); return OK; }
int L3G4200D::init() { int ret = ERROR; /* do SPI init (and probe) first */ if (SPI::init() != OK) goto out; /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(gyro_report)); if (_reports == nullptr) goto out; _class_instance = register_class_devname(GYRO_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { /* advertise sensor topic */ struct gyro_report zero_report; memset(&zero_report, 0, sizeof(zero_report)); _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); } reset(); ret = OK; out: return ret; }
int LidarLiteI2C::init() { int ret = PX4_ERROR; /* do I2C init (and probe) first */ if (I2C::init() != OK) { return ret; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s)); if (_reports == nullptr) { return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ struct distance_sensor_s ds_report = {}; measure(); _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); } ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; return ret; }
int HMC5883::init() { int ret = ERROR; /* do I2C init (and probe) first */ if (I2C::init() != OK) goto out; /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(mag_report)); if (_reports == nullptr) goto out; /* reset the device configuration */ reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); ret = OK; /* sensor is ok, but not calibrated */ _sensor_ok = true; out: return ret; }
int QMC5883::init() { int ret = PX4_ERROR; ret = CDev::init(); if (ret != OK) { DEVICE_DEBUG("CDev init failed"); goto out; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); if (_reports == nullptr) { goto out; } /* reset the device configuration */ reset(); _class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); ret = OK; /* sensor is ok */ _sensor_ok = true; out: return ret; }
int LPS25H::init() { int ret; ret = CDev::init(); if (ret != OK) { DEVICE_DEBUG("CDev init failed"); goto out; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s)); if (_reports == nullptr) { DEVICE_DEBUG("can't get memory for reports"); ret = -ENOMEM; goto out; } if (reset() != OK) { goto out; } /* register alternate interfaces if we have to */ _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH); ret = OK; out: return ret; }
int BMI055_gyro::init() { int ret; /* do SPI init (and probe) first */ ret = SPI::init(); /* if probe/setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("SPI setup failed"); return ret; } /* allocate basic report buffers */ _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); if (_gyro_reports == nullptr) { goto out; } if (reset() != OK) { goto out; } /* Initialize offsets and scales */ _gyro_scale.x_offset = 0; _gyro_scale.x_scale = 1.0f; _gyro_scale.y_offset = 0; _gyro_scale.y_scale = 1.0f; _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; /* if probe/setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("gyro init failed"); return ret; } _gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); measure(); /* advertise sensor topic, measure manually to initialize valid report */ struct gyro_report grp; _gyro_reports->get(&grp); _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, &_gyro_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_gyro_topic == nullptr) { warnx("ADVERT FAIL"); } out: return ret; }
int FXOS8700CQ_mag::init() { int ret; ret = CDev::init(); if (ret == OK) { _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); } return ret; }
int PX4FLOW::init() { int ret = PX4_ERROR; /* do I2C init (and probe) first */ if (I2C::init() != OK) { return ret; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s)); if (_reports == nullptr) { return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ struct distance_sensor_s ds_report = {}; if (_class_instance == CLASS_DEVICE_PRIMARY) { _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } else { DEVICE_LOG("not primary range device, not advertising"); } ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; /* get yaw rotation from sensor frame to body frame */ param_t rot = param_find("SENS_FLOW_ROT"); /* only set it if the parameter exists */ if (rot != PARAM_INVALID) { int32_t val = 6; // the recommended installation for the flow sensor is with the Y sensor axis forward param_get(rot, &val); _sensor_rotation = (enum Rotation)val; } return ret; }
int LSM303D_mag::init() { int ret; ret = CDev::init(); if (ret != OK) goto out; _mag_class_instance = register_class_devname(MAG_DEVICE_PATH); out: return ret; }
int FXAS21002C::init() { /* do SPI init (and probe) first */ if (SPI::init() != OK) { PX4_ERR("SPI init failed"); return PX4_ERROR; } param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF"); float gyro_cut = FXAS21002C_DEFAULT_FILTER_FREQ; if (gyro_cut_ph != PARAM_INVALID && param_get(gyro_cut_ph, &gyro_cut) == PX4_OK) { PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut)); set_sw_lowpass_filter(FXAS21002C_DEFAULT_RATE, gyro_cut); } else { PX4_ERR("IMU_GYRO_CUTOFF param invalid"); } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); if (_reports == nullptr) { return PX4_ERROR; } reset(); /* fill report structures */ measure(); _class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); /* advertise sensor topic, measure manually to initialize valid report */ struct gyro_report grp; _reports->get(&grp); /* measurement will have generated a report, publish */ _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, &_orb_class_instance, (external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic == nullptr) { PX4_ERR("ADVERT ERR"); return PX4_ERROR; } return PX4_OK; }
int L3GD20::init() { int ret = ERROR; /* do SPI init (and probe) first */ if (SPI::init() != OK) goto out; /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(gyro_report)); if (_reports == nullptr) goto out; _class_instance = register_class_devname(GYRO_DEVICE_PATH); switch (_class_instance) { case CLASS_DEVICE_PRIMARY: _orb_id = ORB_ID(sensor_gyro0); break; case CLASS_DEVICE_SECONDARY: _orb_id = ORB_ID(sensor_gyro1); break; case CLASS_DEVICE_TERTIARY: _orb_id = ORB_ID(sensor_gyro2); break; } reset(); measure(); /* advertise sensor topic, measure manually to initialize valid report */ struct gyro_report grp; _reports->get(&grp); _gyro_topic = orb_advertise(_orb_id, &grp); if (_gyro_topic < 0) { debug("failed to create sensor_gyro publication"); } ret = OK; out: return ret; }
int ADIS16477_gyro::init() { int ret = CDev::init(); /* if probe/setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("gyro init failed"); return ret; } _gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); return ret; }
int IIS328DQ::init() { int ret = ERROR; if (probe() != OK) goto out; /* do SPI init (and probe) first */ if (CDev::init() != OK) goto out; /* allocate basic report buffers */ if (_reports != NULL) { ringbuf_deinit(_reports); vPortFree(_reports); _reports = NULL; } /* allocate basic report buffers */ _reports = (ringbuf_t *) pvPortMalloc (sizeof(ringbuf_t)); ringbuf_init(_reports, 2, sizeof(accel_report)); if (_reports == NULL) goto out; _class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); reset(); measure(); /* advertise sensor topic, measure manually to initialize valid report */ struct accel_report arp; ringbuf_get(_reports, &arp, sizeof(arp)); _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_accel_topic == NULL) { DEVICE_DEBUG("failed to create sensor_accel publication"); } 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; }
int SF0X::init() { /* status */ int ret = 0; do { /* create a scope to handle exit conditions using break */ /* do regular cdev init */ ret = CDev::init(); if (ret != OK) { break; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { warnx("mem err"); ret = -1; break; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } } while (0); /* close the fd */ ::close(_fd); _fd = -1; return OK; }
int AirspeedSim::init() { int ret = ERROR; /* init base class */ if (CDev::init() != OK) { DEVICE_DEBUG("CDev init failed"); goto out; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); if (_reports == nullptr) { goto out; } /* register alternate interfaces if we have to */ _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); /* publication init */ if (_class_instance == CLASS_DEVICE_PRIMARY) { /* advertise sensor topic, measure manually to initialize valid report */ struct differential_pressure_s arp; measure(); _reports->get(&arp); /* measurement will have generated a report, publish */ _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); if (_airspeed_pub == nullptr) { PX4_WARN("uORB started?"); } } ret = OK; out: return ret; }
int FXAS21002C::init() { int ret = PX4_ERROR; /* do SPI init (and probe) first */ if (SPI::init() != OK) { PX4_ERR("SPI init failed"); goto out; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); if (_reports == nullptr) { goto out; } reset(); /* fill report structures */ measure(); _class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); /* advertise sensor topic, measure manually to initialize valid report */ struct gyro_report grp; _reports->get(&grp); /* measurement will have generated a report, publish */ _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, &_orb_class_instance, (external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic == nullptr) { PX4_ERR("ADVERT ERR"); } ret = OK; out: return ret; }
int LSM303D_mag::init() { int ret; ret = CDev::init(); if (ret != OK) goto out; _mag_class_instance = register_class_devname(MAG_DEVICE_PATH); if (_mag_class_instance == CLASS_DEVICE_PRIMARY) { // we are the primary mag device, so advertise to // the ORB struct mag_report zero_report; memset(&zero_report, 0, sizeof(zero_report)); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); } out: return ret; }
int PX4FMU::init() { int ret; ASSERT(_task == -1); /* do regular cdev init */ ret = CDev::init(); if (ret != OK) return ret; /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { log("default PWM output device"); } else if (_class_instance < 0) { log("FAILED registering class device"); } /* reset GPIOs */ gpio_reset(); /* start the IO interface task */ _task = px4_task_spawn_cmd("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1600, (main_t)&PX4FMU::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); return -errno; } return OK; }
int HMC5883::init() { int ret = ERROR; /* do I2C init (and probe) first */ if (I2C::init() != OK) goto out; /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(mag_report)); if (_reports == nullptr) goto out; /* reset the device configuration */ reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); switch (_class_instance) { case CLASS_DEVICE_PRIMARY: _mag_orb_id = ORB_ID(sensor_mag0); break; case CLASS_DEVICE_SECONDARY: _mag_orb_id = ORB_ID(sensor_mag1); break; case CLASS_DEVICE_TERTIARY: _mag_orb_id = ORB_ID(sensor_mag2); break; } ret = OK; /* sensor is ok, but not calibrated */ _sensor_ok = true; out: return ret; }
int CM8JL65::init() { /* status */ int ret = 0; do { /* create a scope to handle exit conditions using break */ /* do regular cdev init */ ret = CDev::init(); if (ret != OK) { break; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { PX4_ERR("alloc failed"); ret = -1; break; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic == nullptr) { PX4_ERR("failed to create distance_sensor object"); } } while (0); return ret; }
int L3GD20::init() { int ret = ERROR; /* do SPI init (and probe) first */ if (SPI::init() != OK) { goto out; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); if (_reports == nullptr) { goto out; } _class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); reset(); measure(); /* advertise sensor topic, measure manually to initialize valid report */ struct gyro_report grp; _reports->get(&grp); _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic == nullptr) { DEVICE_DEBUG("failed to create sensor_gyro publication"); } ret = OK; out: return ret; }
int MPU9250_mag::init() { int ret; ret = CDev::init(); /* if setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("MPU9250 mag init failed"); return ret; } _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) { goto out; } _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); ak8963_setup(); /* advertise sensor topic, measure manually to initialize valid report */ struct mag_report mrp; _mag_reports->get(&mrp); _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, &_mag_orb_class_instance, ORB_PRIO_LOW); // &_mag_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_mag_topic == nullptr) { warnx("ADVERT FAIL"); } out: return ret; }
int LL40LS::init() { int ret = ERROR; /* do I2C init (and probe) first */ if (I2C::init() != OK) { goto out; } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) { goto out; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ struct range_finder_report rf_report; measure(); _reports->get(&rf_report); _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); if (_range_finder_topic < 0) { debug("failed to create sensor_range_finder object. Did you start uOrb?"); } } ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; out: return ret; }
int Airspeed::init() { int ret = ERROR; /* do I2C init (and probe) first */ if (I2C::init() != OK) goto out; /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(differential_pressure_s)); if (_reports == nullptr) goto out; /* register alternate interfaces if we have to */ _class_instance = register_class_devname(AIRSPEED_DEVICE_PATH); /* publication init */ if (_class_instance == CLASS_DEVICE_PRIMARY) { /* advertise sensor topic, measure manually to initialize valid report */ struct differential_pressure_s arp; measure(); _reports->get(&arp); /* measurement will have generated a report, publish */ _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); if (_airspeed_pub < 0) warnx("uORB started?"); } ret = OK; out: return ret; }