示例#1
0
void
LSM303D::reset()
{
	// ensure the chip doesn't interpret any other bus traffic as I2C
	disable_i2c();

	/* enable accel*/
	_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
	write_reg(ADDR_CTRL_REG1, _reg1_expected);

	/* enable mag */
	_reg7_expected = REG7_CONT_MODE_M;
	write_reg(ADDR_CTRL_REG7, _reg7_expected);
	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
	write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
	write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2

	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
	accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
	accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);

	// we setup the anti-alias on-chip filter as 50Hz. We believe
	// this operates in the analog domain, and is critical for
	// anti-aliasing. The 2 pole software filter is designed to
	// operate in conjunction with this on-chip filter
	accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);

	mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
	mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);

	_accel_read = 0;
	_mag_read = 0;
}
示例#2
0
文件: lsm303d.cpp 项目: avnishks/px4
void
LSM303D::reset()
{
	/* enable accel*/
	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);

	/* enable mag */
	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);

	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
	accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
	accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
	accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);

	mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
	mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);

	_accel_read = 0;
	_mag_read = 0;
}
示例#3
0
int
LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
{
	switch (cmd) {

	case SENSORIOCSPOLLRATE: {
		switch (arg) {

			/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_call_mag_interval = 0;
				return OK;

			/* external signalling not supported */
			case SENSOR_POLLRATE_EXTERNAL:

			/* zero would be bad */
			case 0:
				return -EINVAL;

			/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
			case SENSOR_POLLRATE_DEFAULT:
				/* 100 Hz is max for mag */
				return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);

			/* adjust to a legal polling interval in Hz */
			default: {
					/* do we need to start internal polling? */
					bool want_start = (_call_mag_interval == 0);

					/* convert hz to hrt interval via microseconds */
					unsigned ticks = 1000000 / arg;

					/* check against maximum sane rate */
					if (ticks < 1000)
						return -EINVAL;

					/* update interval for next measurement */
					/* XXX this is a bit shady, but no other way to adjust... */
					_mag_call.period = _call_mag_interval = ticks;

					/* if we need to start the poll state machine, do it */
					if (want_start)
						start();

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (_call_mag_interval == 0)
			return SENSOR_POLLRATE_MANUAL;

		return 1000000 / _call_mag_interval;
	
	case SENSORIOCSQUEUEDEPTH: {
		/* lower bound is mandatory, upper bound is a sanity check */
		if ((arg < 1) || (arg > 100))
			return -EINVAL;

		irqstate_t flags = irqsave();
		if (!_mag_reports->resize(arg)) {
			irqrestore(flags);
			return -ENOMEM;
		}
		irqrestore(flags);

		return OK;
	}

	case SENSORIOCGQUEUEDEPTH:
		return _mag_reports->size();

	case SENSORIOCRESET:
		reset();
		return OK;

	case MAGIOCSSAMPLERATE:
		return mag_set_samplerate(arg);

	case MAGIOCGSAMPLERATE:
		return _mag_samplerate;

	case MAGIOCSLOWPASS:
	case MAGIOCGLOWPASS:
		/* not supported, no internal filtering */
		return -EINVAL;

	case MAGIOCSSCALE:
		/* copy scale in */
		memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
		return OK;

	case MAGIOCGSCALE:
		/* copy scale out */
		memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
		return OK;

	case MAGIOCSRANGE:
		return mag_set_range(arg);

	case MAGIOCGRANGE:
		return _mag_range_ga;

	case MAGIOCSELFTEST:
		return mag_self_test();

	case MAGIOCGEXTERNAL:
		/* no external mag board yet */
		return 0;

	default:
		/* give it to the superclass */
		return SPI::ioctl(filp, cmd, arg);
	}
}
bool AP_InertialSensor_LSM303D::_hardware_init(Sample_rate sample_rate)
{
    if (!_spi_sem->take(100)) {
        hal.scheduler->panic(PSTR("LSM303D: Unable to get semaphore"));
    }

    // initially run the bus at low speed
    _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
       
    // ensure the chip doesn't interpret any other bus traffic as I2C
	disable_i2c();


    /* enable accel*/
    _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
    _register_write(ADDR_CTRL_REG1, _reg1_expected);

    /* enable mag */
    _reg7_expected = REG7_CONT_MODE_M;
    _register_write(ADDR_CTRL_REG7, _reg7_expected);
    _register_write(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
    _register_write(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
    _register_write(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2

    accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
    accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);

    // Hardware filtering
    // we setup the anti-alias on-chip filter as 50Hz. We believe
    // this operates in the analog domain, and is critical for
    // anti-aliasing. The 2 pole software filter is designed to
    // operate in conjunction with this on-chip filter
    accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);

    mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
    mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);

    // TODO: Software filtering
    // accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);

    // uint8_t default_filter;

    // // sample rate and filtering
    // // to minimise the effects of aliasing we choose a filter
    // // that is less than half of the sample rate
    // switch (sample_rate) {
    // case RATE_50HZ:
    //     // this is used for plane and rover, where noise resistance is
    //     // more important than update rate. Tests on an aerobatic plane
    //     // show that 10Hz is fine, and makes it very noise resistant
    //     default_filter = BITS_DLPF_CFG_10HZ;
    //     _sample_shift = 2;
    //     break;
    // case RATE_100HZ:
    //     default_filter = BITS_DLPF_CFG_20HZ;
    //     _sample_shift = 1;
    //     break;
    // case RATE_200HZ:
    // default:
    //     default_filter = BITS_DLPF_CFG_20HZ;
    //     _sample_shift = 0;
    //     break;
    // }
    // _set_filter_register(_LSM303D_filter, default_filter);

    // now that we have initialised, we set the SPI bus speed to high
    _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
    _spi_sem->give();

    return true;
}
int
LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
{
    switch (cmd) {

    case SENSORIOCSPOLLRATE: {
        switch (arg) {

        /* switching to manual polling */
        case SENSOR_POLLRATE_MANUAL:
            stop();
            _call_mag_interval = 0;
            return OK;

        /* external signalling not supported */
        case SENSOR_POLLRATE_EXTERNAL:

        /* zero would be bad */
        case 0:
            return -EINVAL;

        /* set default/max polling rate */
        case SENSOR_POLLRATE_MAX:
        case SENSOR_POLLRATE_DEFAULT:
            /* 50 Hz is max for mag */
            return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);

        /* adjust to a legal polling interval in Hz */
        default: {
            /* do we need to start internal polling? */
            bool want_start = (_call_mag_interval == 0);

            /* convert hz to hrt interval via microseconds */
            unsigned ticks = 1000000 / arg;

            /* check against maximum sane rate */
            if (ticks < 1000)
                return -EINVAL;

            /* adjust sample rate of sensor */
            mag_set_samplerate(arg);

            /* update interval for next measurement */
            /* XXX this is a bit shady, but no other way to adjust... */
            _mag_call.period = _call_mag_interval = ticks;

            /* if we need to start the poll state machine, do it */
            if (want_start)
                start();

            return OK;
        }
        }
    }

    case SENSORIOCGPOLLRATE:
        if (_call_mag_interval == 0)
            return SENSOR_POLLRATE_MANUAL;

        return 1000000 / _call_mag_interval;
    case SENSORIOCSQUEUEDEPTH:
    case SENSORIOCGQUEUEDEPTH:
    case SENSORIOCRESET:
        return ioctl(filp, cmd, arg);

    case MAGIOCSSAMPLERATE:
//	case MAGIOCGSAMPLERATE:
        /* XXX not implemented */
        return -EINVAL;

    case MAGIOCSLOWPASS:
//	case MAGIOCGLOWPASS:
        /* XXX not implemented */
//		_set_dlpf_filter((uint16_t)arg);
        return -EINVAL;

    case MAGIOCSSCALE:
        /* copy scale in */
        memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
        return OK;

    case MAGIOCGSCALE:
        /* copy scale out */
        memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
        return OK;

    case MAGIOCSRANGE:
//	case MAGIOCGRANGE:
        /* XXX not implemented */
        // XXX change these two values on set:
        // _mag_range_scale = xx
        // _mag_range_ga = xx
        return -EINVAL;

    case MAGIOCSELFTEST:
        /* XXX not implemented */
//		return self_test();
        return -EINVAL;

    default:
        /* give it to the superclass */
        return SPI::ioctl(filp, cmd, arg);
    }
}
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;
}