// @TODO: use fixed point math to reclaim CPU usage
int PX4Magnetometer::publish(float x, float y, float z, float temperature)
{
sensor_mag_s report{};
report.device_id = _device_id.devid;
report.error_count = 0;
report.scaling = _scale;
report.timestamp = hrt_absolute_time();
report.temperature = temperature;
report.is_external = false;
// Raw values (ADC units 0 - 65535)
report.x_raw = x;
report.y_raw = y;
report.z_raw = z;
// Apply the rotation.
rotate_3f(_rotation, x, y, z);
// Apply FS range scale and the calibrating offset/scale
x = ((x * _scale) - _cal.x_offset) * _cal.x_scale;
y = ((y * _scale) - _cal.y_offset) * _cal.y_scale;
z = ((z * _scale) - _cal.z_offset) * _cal.z_scale;
// Filtered values
report.x = _filter_x.apply(x);
report.y = _filter_y.apply(y);
report.z = _filter_z.apply(z);
poll_notify(POLLIN);
orb_publish(ORB_ID(sensor_mag), _topic, &report);
return PX4_OK;
}
int
HMC5883::collect()
{
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t z[2];
uint8_t y[2];
} hmc_report;
#pragma pack(pop)
struct {
int16_t x, y, z;
} report;
int ret = -EIO;
uint8_t cmd;
uint8_t check_counter;
perf_begin(_sample_perf);
struct mag_report new_report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.error_count = perf_event_count(_comms_errors);
/*
* @note We could read the status register here, which could tell us that
* we were too early and that the output registers are still being
* written. In the common case that would just slow us down, and
* we're better off just never being early.
*/
/* get measurements from the device */
cmd = ADDR_DATA_OUT_X_MSB;
ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report));
if (ret != OK) {
perf_count(_comms_errors);
debug("data/status read error");
goto out;
}
/* swap the data we just received */
report.x = (((int16_t)hmc_report.x[0]) << 8) + hmc_report.x[1];
report.y = (((int16_t)hmc_report.y[0]) << 8) + hmc_report.y[1];
report.z = (((int16_t)hmc_report.z[0]) << 8) + hmc_report.z[1];
/*
* If any of the values are -4096, there was an internal math error in the sensor.
* Generalise this to a simple range check that will also catch some bit errors.
*/
if ((abs(report.x) > 2048) ||
(abs(report.y) > 2048) ||
(abs(report.z) > 2048)) {
perf_count(_comms_errors);
goto out;
}
/*
* RAW outputs
*
* to align the sensor axes with the board, x and y need to be flipped
* and y needs to be negated
*/
new_report.x_raw = report.y;
new_report.y_raw = -report.x;
/* z remains z */
new_report.z_raw = report.z;
/* scale values for output */
#ifdef PX4_I2C_BUS_ONBOARD
if (_bus == PX4_I2C_BUS_ONBOARD) {
// convert onboard so it matches offboard for the
// scaling below
report.y = -report.y;
report.x = -report.x;
}
#endif
/* the standard external mag by 3DR has x pointing to the
* right, y pointing backwards, and z down, therefore switch x
* and y and invert y */
new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
// apply user specified rotation
rotate_3f(_rotation, new_report.x, new_report.y, new_report.z);
if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report);
if (_mag_topic < 0)
debug("failed to create sensor_mag publication");
}
}
_last_report = new_report;
/* post a report to the ring */
if (_reports->force(&new_report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
/*
periodically check the range register and configuration
registers. With a bad I2C cable it is possible for the
registers to become corrupt, leading to bad readings. It
doesn't happen often, but given the poor cables some
vehicles have it is worth checking for.
*/
check_counter = perf_event_count(_sample_perf) % 256;
if (check_counter == 0) {
check_range();
}
if (check_counter == 128) {
check_conf();
}
ret = OK;
out:
perf_end(_sample_perf);
return ret;
}
int
IST8310::collect()
{
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t y[2];
uint8_t z[2];
} report_buffer;
#pragma pack(pop)
struct {
int16_t x, y, z;
} report;
int ret;
uint8_t check_counter;
perf_begin(_sample_perf);
struct mag_report new_report;
const bool sensor_is_external = external();
float xraw_f;
float yraw_f;
float zraw_f;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.is_external = sensor_is_external;
new_report.error_count = perf_event_count(_comms_errors);
new_report.scaling = _range_scale;
new_report.device_id = _device_id.devid;
/*
* @note We could read the status register here, which could tell us that
* we were too early and that the output registers are still being
* written. In the common case that would just slow us down, and
* we're better off just never being early.
*/
/* get measurements from the device */
ret = read(ADDR_DATA_OUT_X_LSB, (uint8_t *)&report_buffer, sizeof(report_buffer));
if (ret != OK) {
perf_count(_comms_errors);
DEVICE_DEBUG("I2C read error");
goto out;
}
/* swap the data we just received */
report.x = (((int16_t)report_buffer.x[1]) << 8) | (int16_t)report_buffer.x[0];
report.y = (((int16_t)report_buffer.y[1]) << 8) | (int16_t)report_buffer.y[0];
report.z = (((int16_t)report_buffer.z[1]) << 8) | (int16_t)report_buffer.z[0];
/*
* Check if value makes sense according to the FSR and Resolution of
* this sensor, discarding outliers
*/
if (report.x > IST8310_MAX_VAL_XY || report.x < IST8310_MIN_VAL_XY ||
report.y > IST8310_MAX_VAL_XY || report.y < IST8310_MIN_VAL_XY ||
report.z > IST8310_MAX_VAL_Z || report.z < IST8310_MIN_VAL_Z) {
perf_count(_range_errors);
DEVICE_DEBUG("data/status read error");
goto out;
}
/* temperature measurement is not available on IST8310 */
new_report.temperature = 0;
/*
* raw outputs
*
* Sensor doesn't follow right hand rule, swap x and y to make it obey
* it.
*/
new_report.x_raw = report.y;
new_report.y_raw = report.x;
new_report.z_raw = report.z;
/* scale values for output */
xraw_f = report.y;
yraw_f = report.x;
zraw_f = report.z;
/* apply user specified rotation */
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (!(_pub_blocked)) {
if (_mag_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
&_orb_class_instance, sensor_is_external ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_mag_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
_last_report = new_report;
/* post a report to the ring */
_reports->force(&new_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/*
periodically check the range register and configuration
registers. With a bad I2C cable it is possible for the
registers to become corrupt, leading to bad readings. It
doesn't happen often, but given the poor cables some
vehicles have it is worth checking for.
*/
check_counter = perf_event_count(_sample_perf) % 256;
if (check_counter == 128) {
check_conf();
}
ret = OK;
out:
perf_end(_sample_perf);
return ret;
}
void
L3GD20::measure()
{
#if L3GD20_USE_DRDY
// if the gyro doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
perf_count(_reschedules);
hrt_call_delay(&_call, 100);
return;
}
#endif
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t temp;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} raw_report;
#pragma pack(pop)
gyro_report report;
/* start the performance counter */
perf_begin(_sample_perf);
/* fetch data from the sensor */
memset(&raw_report, 0, sizeof(raw_report));
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
#if L3GD20_USE_DRDY
if ((raw_report.status & 0xF) != 0xF) {
/*
we waited for DRDY, but did not see DRDY on all axes
when we captured. That means a transfer error of some sort
*/
perf_count(_errors);
return;
}
#endif
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
report.timestamp = hrt_absolute_time();
report.error_count = 0; // not recorded
switch (_orientation) {
case SENSOR_BOARD_ROTATION_000_DEG:
/* keep axes in place */
report.x_raw = raw_report.x;
report.y_raw = raw_report.y;
break;
case SENSOR_BOARD_ROTATION_090_DEG:
/* swap x and y */
report.x_raw = raw_report.y;
report.y_raw = raw_report.x;
break;
case SENSOR_BOARD_ROTATION_180_DEG:
/* swap x and y and negate both */
report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
break;
case SENSOR_BOARD_ROTATION_270_DEG:
/* swap x and y and negate y */
report.x_raw = raw_report.y;
report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
break;
}
report.z_raw = raw_report.z;
report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
report.x = _gyro_filter_x.apply(report.x);
report.y = _gyro_filter_y.apply(report.y);
report.z = _gyro_filter_z.apply(report.z);
// apply user specified rotation
rotate_3f(_rotation, report.x, report.y, report.z);
report.scaling = _gyro_range_scale;
report.range_rad_s = _gyro_range_rad_s;
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_orb_id, _gyro_topic, &report);
}
_read++;
/* stop the perf counter */
perf_end(_sample_perf);
}
void
BMI160::measure()
{
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct BMIReport bmi_report;
struct Report {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the BMI160 in one pass.
*/
bmi_report.cmd = BMIREG_GYR_X_L | DIR_READ;
uint8_t status = read_reg(BMIREG_STATUS);
if (OK != transfer((uint8_t *)&bmi_report, ((uint8_t *)&bmi_report), sizeof(bmi_report))) {
return;
}
check_registers();
if ((!(status & (0x80))) && (!(status & (0x04)))) {
perf_end(_sample_perf);
perf_count(_duplicates);
_got_duplicate = true;
return;
}
_last_accel[0] = bmi_report.accel_x;
_last_accel[1] = bmi_report.accel_y;
_last_accel[2] = bmi_report.accel_z;
_got_duplicate = false;
uint8_t temp_l = read_reg(BMIREG_TEMP_0);
uint8_t temp_h = read_reg(BMIREG_TEMP_1);
report.temp = ((temp_h << 8) + temp_l);
report.accel_x = bmi_report.accel_x;
report.accel_y = bmi_report.accel_y;
report.accel_z = bmi_report.accel_z;
report.gyro_x = bmi_report.gyro_x;
report.gyro_y = bmi_report.gyro_y;
report.gyro_z = bmi_report.gyro_z;
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_z == 0 &&
report.temp == 0 &&
report.gyro_x == 0 &&
report.gyro_y == 0 &&
report.gyro_z == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the bmi160 does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return;
}
perf_count(_good_transfers);
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again. We still increment
// _good_transfers, but don't return any data yet
_register_wait--;
return;
}
/*
* Report buffers.
*/
accel_report arb;
gyro_report grb;
/*
* Adjust and scale results to m/s^2.
*/
grb.timestamp = arb.timestamp = hrt_absolute_time();
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
/* NOTE: Axes have been swapped to match the board a few lines above. */
arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;
float xraw_f = report.accel_x;
float yraw_f = report.accel_y;
float zraw_f = report.accel_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
math::Vector<3> aval_integrated;
bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
arb.x_integral = aval_integrated(0);
arb.y_integral = aval_integrated(1);
arb.z_integral = aval_integrated(2);
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
_last_temperature = 23 + report.temp * 1.0f / 512.0f;
arb.temperature_raw = report.temp;
arb.temperature = _last_temperature;
/* return device ID */
arb.device_id = _device_id.devid;
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;
xraw_f = report.gyro_x;
yraw_f = report.gyro_y;
zraw_f = report.gyro_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
math::Vector<3> gval_integrated;
bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
grb.x_integral = gval_integrated(0);
grb.y_integral = gval_integrated(1);
grb.z_integral = gval_integrated(2);
grb.scaling = _gyro_range_scale;
grb.range_rad_s = _gyro_range_rad_s;
grb.temperature_raw = report.temp;
grb.temperature = _last_temperature;
/* return device ID */
grb.device_id = _gyro->_device_id.devid;
_accel_reports->force(&arb);
_gyro_reports->force(&grb);
/* notify anyone waiting for data */
if (accel_notify) {
poll_notify(POLLIN);
}
if (gyro_notify) {
_gyro->parent_poll_notify();
}
if (accel_notify && !(_pub_blocked)) {
/* log the time of this report */
perf_begin(_controller_latency_perf);
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (gyro_notify && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
/* stop measuring */
perf_end(_sample_perf);
}
int
LIS3MDL::collect()
{
#pragma pack(push, 1)
struct {
uint8_t x[2];
uint8_t y[2];
uint8_t z[2];
} lis_report;
struct {
int16_t x;
int16_t y;
int16_t z;
int16_t t;
} report;
#pragma pack(pop)
int ret = 0;
uint8_t buf_rx[2] = {0};
float xraw_f;
float yraw_f;
float zraw_f;
struct mag_report new_mag_report;
bool sensor_is_onboard = false;
perf_begin(_sample_perf);
new_mag_report.timestamp = hrt_absolute_time();
new_mag_report.error_count = perf_event_count(_comms_errors);
new_mag_report.scaling = _range_scale;
new_mag_report.device_id = _device_id.devid;
ret = _interface->read(ADDR_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report));
/**
* Weird behavior: the X axis will be read instead of the temperature registers if you use a pointer to a packed struct...not sure why.
* This works now, but further investigation to determine why this happens would be good (I am guessing a type error somewhere)
*/
ret = _interface->read(ADDR_OUT_T_L, (uint8_t *)&buf_rx, sizeof(buf_rx));
if (ret != OK) {
perf_count(_comms_errors);
PX4_WARN("Register read error.");
return ret;
}
report.x = (int16_t)((lis_report.x[1] << 8) | lis_report.x[0]);
report.y = (int16_t)((lis_report.y[1] << 8) | lis_report.y[0]);
report.z = (int16_t)((lis_report.z[1] << 8) | lis_report.z[0]);
report.t = (int16_t)((buf_rx[1] << 8) | buf_rx[0]);
float temperature = report.t;
new_mag_report.temperature = 25.0f + (temperature / 8.0f);
// XXX revisit for SPI part, might require a bus type IOCTL
unsigned dummy = 0;
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
new_mag_report.is_external = !sensor_is_onboard;
/**
* RAW outputs
*/
new_mag_report.x_raw = report.x;
new_mag_report.y_raw = report.y;
new_mag_report.z_raw = report.z;
xraw_f = report.x;
yraw_f = report.y;
zraw_f = report.z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
new_mag_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_mag_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_mag_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (!(_pub_blocked)) {
if (_mag_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_mag_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_mag_report,
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_mag_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
_last_report = new_mag_report;
/* post a report to the ring */
_reports->force(&new_mag_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
QMC5883::collect()
{
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t y[2];
uint8_t z[2];
} qmc_report;
#pragma pack(pop)
struct {
int16_t x, y, z;
} report;
int ret;
uint8_t check_counter;
perf_begin(_sample_perf);
struct mag_report new_report;
bool sensor_is_onboard = false;
float xraw_f;
float yraw_f;
float zraw_f;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.error_count = perf_event_count(_comms_errors);
new_report.scaling = _range_scale;
new_report.device_id = _device_id.devid;
/*
* @note We could read the status register here, which could tell us that
* we were too early and that the output registers are still being
* written. In the common case that would just slow us down, and
* we're better off just never being early.
*/
/* get measurements from the device */
ret = _interface->read(QMC5883_ADDR_DATA_OUT_X_LSB, (uint8_t *)&qmc_report, sizeof(qmc_report));
if (ret != OK) {
perf_count(_comms_errors);
DEVICE_DEBUG("data/status read error");
goto out;
}
/* map data we just received LSB, MSB */
report.x = (((int16_t)qmc_report.x[1]) << 8) + qmc_report.x[0];
report.y = (((int16_t)qmc_report.y[1]) << 8) + qmc_report.y[0];
report.z = (((int16_t)qmc_report.z[1]) << 8) + qmc_report.z[0];
/*
* If any of the values are -4096, there was an internal math error in the sensor.
* Generalise this to a simple range check that will also catch some bit errors.
*/
if ((abs(report.x) > QMC5883_MAX_COUNT) ||
(abs(report.y) > QMC5883_MAX_COUNT) ||
(abs(report.z) > QMC5883_MAX_COUNT)) {
perf_count(_comms_errors);
goto out;
}
/* get temperature measurements from the device */
new_report.temperature = 0;
if (_temperature_counter++ == 100) {
uint8_t raw_temperature[2];
_temperature_counter = 0;
ret = _interface->read(QMC5883_ADDR_TEMP_OUT_LSB,
raw_temperature, sizeof(raw_temperature));
if (ret == OK) {
int16_t temp16 = (((int16_t)raw_temperature[1]) << 8) +
raw_temperature[0];
new_report.temperature = QMC5883_TEMP_OFFSET + temp16 * 1.0f / 100.0f;
}
} else {
new_report.temperature = _last_report.temperature;
}
/*
* RAW outputs
*
* to align the sensor axes with the board, x and y need to be flipped
* and y needs to be negated
*/
new_report.x_raw = -report.y;
new_report.y_raw = report.x;
/* z remains z */
new_report.z_raw = report.z;
/* scale values for output */
// XXX revisit for SPI part, might require a bus type IOCTL
unsigned dummy;
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
new_report.is_external = !sensor_is_onboard;
if (sensor_is_onboard) {
// convert onboard so it matches offboard for the
// scaling below
report.y = -report.y;
report.x = -report.x;
}
/* the standard external mag by 3DR has x pointing to the
* right, y pointing backwards, and z down, therefore switch x
* and y and invert y */
//TODO: sort out axes mapping
xraw_f = -report.y;
yraw_f = report.x;
zraw_f = report.z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (!(_pub_blocked)) {
if (_mag_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_mag_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
_last_report = new_report;
/* post a report to the ring */
_reports->force(&new_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/*
periodically check the configuration
registers. With a bad I2C cable it is possible for the
registers to become corrupt, leading to bad readings. It
doesn't happen often, but given the poor cables some
vehicles have it is worth checking for.
*/
check_counter = perf_event_count(_sample_perf) % 256;
if (check_counter == 0) {
check_conf();
}
ret = OK;
out:
perf_end(_sample_perf);
return ret;
}
void
MPU9250::measure()
{
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct MPUReport mpu_report;
struct Report {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the MPU9250 in one pass.
*/
if (OK != _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED),
(uint8_t *)&mpu_report,
sizeof(mpu_report))) {
return;
}
check_registers();
if (check_duplicate(&mpu_report.accel_x[0])) {
return;
}
#ifdef USE_I2C
if (_mag->is_passthrough()) {
#endif
_mag->_measure(mpu_report.mag);
#ifdef USE_I2C
} else {
_mag->measure();
}
#endif
/*
* Convert from big to little endian
*/
report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
report.temp = int16_t_from_bytes(mpu_report.temp);
report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
if (check_null_data((uint32_t *)&report, sizeof(report) / 4)) {
return;
}
if (_register_wait != 0) {
// we are waiting for some good transfers before using the sensor again
// We still increment _good_transfers, but don't return any data yet
_register_wait--;
return;
}
/*
* Swap axes and negate y
*/
int16_t accel_xt = report.accel_y;
int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
int16_t gyro_xt = report.gyro_y;
int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
/*
* Apply the swap
*/
report.accel_x = accel_xt;
report.accel_y = accel_yt;
report.gyro_x = gyro_xt;
report.gyro_y = gyro_yt;
/*
* Report buffers.
*/
accel_report arb;
gyro_report grb;
/*
* Adjust and scale results to m/s^2.
*/
grb.timestamp = arb.timestamp = hrt_absolute_time();
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
/* NOTE: Axes have been swapped to match the board a few lines above. */
arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;
float xraw_f = report.accel_x;
float yraw_f = report.accel_y;
float zraw_f = report.accel_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
math::Vector<3> aval_integrated;
bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
arb.x_integral = aval_integrated(0);
arb.y_integral = aval_integrated(1);
arb.z_integral = aval_integrated(2);
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
_last_temperature = (report.temp) / 361.0f + 35.0f;
arb.temperature_raw = report.temp;
arb.temperature = _last_temperature;
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;
xraw_f = report.gyro_x;
yraw_f = report.gyro_y;
zraw_f = report.gyro_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
math::Vector<3> gval_integrated;
bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
grb.x_integral = gval_integrated(0);
grb.y_integral = gval_integrated(1);
grb.z_integral = gval_integrated(2);
grb.scaling = _gyro_range_scale;
grb.range_rad_s = _gyro_range_rad_s;
grb.temperature_raw = report.temp;
grb.temperature = _last_temperature;
_accel_reports->force(&arb);
_gyro_reports->force(&grb);
/* notify anyone waiting for data */
if (accel_notify) {
poll_notify(POLLIN);
}
if (gyro_notify) {
_gyro->parent_poll_notify();
}
if (accel_notify && !(_pub_blocked)) {
/* log the time of this report */
perf_begin(_controller_latency_perf);
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (gyro_notify && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
/* stop measuring */
perf_end(_sample_perf);
}
void
FXOS8700CQ::measure()
{
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd[2];
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
int16_t mx;
int16_t my;
int16_t mz;
} raw_accel_mag_report;
#pragma pack(pop)
accel_report accel_report;
/* start the performance counter */
perf_begin(_accel_sample_perf);
check_registers();
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again.
_register_wait--;
perf_end(_accel_sample_perf);
return;
}
/* fetch data from the sensor */
memset(&raw_accel_mag_report, 0, sizeof(raw_accel_mag_report));
raw_accel_mag_report.cmd[0] = DIR_READ(FXOS8700CQ_DR_STATUS);
raw_accel_mag_report.cmd[1] = ADDR_7(FXOS8700CQ_DR_STATUS);
transfer((uint8_t *)&raw_accel_mag_report, (uint8_t *)&raw_accel_mag_report, sizeof(raw_accel_mag_report));
if (!(raw_accel_mag_report.status & DR_STATUS_ZYXDR)) {
perf_end(_accel_sample_perf);
perf_count(_accel_duplicates);
return;
}
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
accel_report.timestamp = hrt_absolute_time();
/*
* Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity.
* Temperature data is only valid between –40 °C and 125 °C. The temperature sensor
* output is only valid when M_CTRL_REG1[m_hms] > 0b00. Please note that the
* temperature sensor is uncalibrated and its output for a given temperature will vary from
* one device to the next
*/
write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_A | M_CTRL_REG1_OS(7));
_last_temperature = (read_reg(FXOS8700CQ_TEMP)) * 0.96f;
write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_AM | M_CTRL_REG1_OS(7));
accel_report.temperature = _last_temperature;
// report the error count as the sum of the number of bad
// register reads and bad values. This allows the higher level
// code to decide if it should use this sensor based on
// whether it has had failures
accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
accel_report.x_raw = swap16RightJustify14(raw_accel_mag_report.x);
accel_report.y_raw = swap16RightJustify14(raw_accel_mag_report.y);
accel_report.z_raw = swap16RightJustify14(raw_accel_mag_report.z);
/* Save off the Mag readings todo: revist integrating theses */
_last_raw_mag_x = swap16(raw_accel_mag_report.mx);
_last_raw_mag_y = swap16(raw_accel_mag_report.my);
_last_raw_mag_z = swap16(raw_accel_mag_report.mz);
float xraw_f = accel_report.x_raw;
float yraw_f = accel_report.y_raw;
float zraw_f = accel_report.z_raw;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
/*
we have logs where the accelerometers get stuck at a fixed
large value. We want to detect this and mark the sensor as
being faulty
*/
if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
fabsf(_last_accel[1] - y_in_new) < 0.001f &&
fabsf(_last_accel[2] - z_in_new) < 0.001f &&
fabsf(x_in_new) > 20 &&
fabsf(y_in_new) > 20 &&
fabsf(z_in_new) > 20) {
_constant_accel_count += 1;
} else {
_constant_accel_count = 0;
}
if (_constant_accel_count > 100) {
// we've had 100 constant accel readings with large
// values. The sensor is almost certainly dead. We
// will raise the error_count so that the top level
// flight code will know to avoid this sensor, but
// we'll still give the data so that it can be logged
// and viewed
perf_count(_bad_values);
_constant_accel_count = 0;
}
_last_accel[0] = x_in_new;
_last_accel[1] = y_in_new;
_last_accel[2] = z_in_new;
accel_report.x = _accel_filter_x.apply(x_in_new);
accel_report.y = _accel_filter_y.apply(y_in_new);
accel_report.z = _accel_filter_z.apply(z_in_new);
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
math::Vector<3> aval_integrated;
bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
accel_report.x_integral = aval_integrated(0);
accel_report.y_integral = aval_integrated(1);
accel_report.z_integral = aval_integrated(2);
accel_report.scaling = _accel_range_scale;
accel_report.range_m_s2 = _accel_range_m_s2;
/* return device ID */
accel_report.device_id = _device_id.devid;
_accel_reports->force(&accel_report);
/* notify anyone waiting for data */
if (accel_notify) {
poll_notify(POLLIN);
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
}
_accel_read++;
/* stop the perf counter */
perf_end(_accel_sample_perf);
}
int
PMW3901::collect()
{
int ret = OK;
int16_t delta_x_raw, delta_y_raw;
float delta_x, delta_y;
perf_begin(_sample_perf);
uint64_t timestamp = hrt_absolute_time();
uint64_t dt_flow = timestamp - _previous_collect_timestamp;
_previous_collect_timestamp = timestamp;
_flow_dt_sum_usec += dt_flow;
readMotionCount(delta_x_raw, delta_y_raw);
_flow_sum_x += delta_x_raw;
_flow_sum_y += delta_y_raw;
// returns if the collect time has not been reached
if (_flow_dt_sum_usec < _collect_time) {
return ret;
}
delta_x = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians
delta_y = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians
struct optical_flow_s report;
report.timestamp = timestamp;
report.pixel_flow_x_integral = static_cast<float>(delta_x);
report.pixel_flow_y_integral = static_cast<float>(delta_y);
// rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT
float zeroval = 0.0f;
rotate_3f(_yaw_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
rotate_3f(_yaw_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
report.frame_count_since_last_readout = 4; //microseconds
report.integration_timespan = _flow_dt_sum_usec; //microseconds
report.sensor_id = 0;
// This sensor doesn't provide any quality metric. However if the sensor is unable to calculate the optical flow it will
// output 0 for the delta. Hence, we set the measurement to "invalid" (quality = 0) if the values are smaller than FLT_EPSILON
if (fabsf(report.pixel_flow_x_integral) < FLT_EPSILON && fabsf(report.pixel_flow_y_integral) < FLT_EPSILON) {
report.quality = 0;
} else {
report.quality = 255;
}
/* No gyro on this board */
report.gyro_x_rate_integral = NAN;
report.gyro_y_rate_integral = NAN;
report.gyro_z_rate_integral = NAN;
// set (conservative) specs according to datasheet
report.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s
report.min_ground_distance = 0.1f; // Datasheet: 80mm
report.max_ground_distance = 30.0f; // Datasheet: infinity
_flow_dt_sum_usec = 0;
_flow_sum_x = 0;
_flow_sum_y = 0;
if (_optical_flow_pub == nullptr) {
_optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &report);
} else {
orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &report);
}
/* post a report to the ring */
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
perf_end(_sample_perf);
return ret;
}
int
PX4FLOW::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 };
perf_begin(_sample_perf);
if (PX4FLOW_REG == 0x00) {
ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE);
}
if (PX4FLOW_REG == 0x16) {
ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE);
}
if (ret < 0) {
debug("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
if (PX4FLOW_REG == 0) {
memcpy(&f, val, I2C_FRAME_SIZE);
memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
}
if (PX4FLOW_REG == 0x16) {
memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
}
struct optical_flow_s report;
report.timestamp = hrt_absolute_time();
report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert to meters
report.quality = f_integral.qual; //0:bad ; 255 max quality
report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
report.integration_timespan = f_integral.integration_timespan; //microseconds
report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds
report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
report.sensor_id = 0;
/* rotate measurements according to parameter */
float zeroval = 0.0f;
rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
if (_px4flow_topic < 0) {
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
} else {
/* publish it */
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
}
/* post a report to the ring */
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
IIS328DQ::measure()
{
/* status register and data as read back from the device */
struct {
uint8_t cmd;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} raw_accel_report;
uint8_t raw_data[8];
accel_report accel_report = {0};
/* start the performance counter */
perf_begin(_sample_perf);
check_registers();
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again.
_register_wait--;
perf_end(_sample_perf);
return;
}
/* fetch data from the sensor */
memset(&raw_data, 0, sizeof(raw_data));
raw_data[0] = ADDR_STATUS_REG | DIR_READ | ADDR_INCREMENT;
spi_transfer(&iis328dq_spi, raw_data, raw_data, sizeof(raw_data));
raw_accel_report.status = raw_data[1];
raw_accel_report.x = ((int16_t)raw_data[3] << 8) | raw_data[2];
raw_accel_report.y = ((int16_t)raw_data[5] << 8) | raw_data[4];
raw_accel_report.z = ((int16_t)raw_data[7] << 8) | raw_data[6];
if (!(raw_accel_report.status & STATUS_ZYXDA)) {
perf_end(_sample_perf);
perf_count(_duplicates);
return;
}
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
accel_report.timestamp = hrt_absolute_time();
// report the error count as the sum of the number of bad
// register reads and bad values. This allows the higher level
// code to decide if it should use this sensor based on
// whether it has had failures
accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
accel_report.x_raw = raw_accel_report.x;
accel_report.y_raw = raw_accel_report.y;
accel_report.z_raw = raw_accel_report.z;
float xraw_f = raw_accel_report.x;
float yraw_f = raw_accel_report.y;
float zraw_f = raw_accel_report.z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
/*
we have logs where the accelerometers get stuck at a fixed
large value. We want to detect this and mark the sensor as
being faulty
*/
if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
fabsf(_last_accel[1] - y_in_new) < 0.001f &&
fabsf(_last_accel[2] - z_in_new) < 0.001f &&
fabsf(x_in_new) > 20 &&
fabsf(y_in_new) > 20 &&
fabsf(z_in_new) > 20) {
_constant_accel_count += 1;
} else {
_constant_accel_count = 0;
}
if (_constant_accel_count > 100) {
// we've had 100 constant accel readings with large
// values. The sensor is almost certainly dead. We
// will raise the error_count so that the top level
// flight code will know to avoid this sensor, but
// we'll still give the data so that it can be logged
// and viewed
perf_count(_bad_values);
_constant_accel_count = 0;
}
_last_accel[0] = x_in_new;
_last_accel[1] = y_in_new;
_last_accel[2] = z_in_new;
accel_report.x = _accel_filter_x.apply(x_in_new);
accel_report.y = _accel_filter_y.apply(y_in_new);
accel_report.z = _accel_filter_z.apply(z_in_new);
accel_report.scaling = _accel_range_scale;
accel_report.range_m_s2 = _accel_range_m_s2;
//pilot_warn("accel data:rawxyz[%04x,%04x,%04x] size=%d\n", accel_report.x_raw, accel_report.y_raw, accel_report.z_raw, sizeof(raw_accel_report));
ringbuf_force(_reports, &accel_report, sizeof(accel_report));
/* notify anyone waiting for data */
poll_notify(POLLIN);
// if (!(_pub_blocked))
{
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
_read++;
/* stop the perf counter */
perf_end(_sample_perf);
}
void
BMI055_accel::measure()
{
uint8_t index = 0, accel_data[7];
uint16_t lsb, msb, msblsb;
uint8_t status_x, status_y, status_z;
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct Report {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t temp;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the BMI055 in one pass.
*/
accel_data[index] = BMI055_ACC_X_L | DIR_READ;
if (OK != transfer(accel_data, accel_data, sizeof(accel_data))) {
return;
}
check_registers();
/* Extracting accel data from the read data */
index = 1;
lsb = (uint16_t)accel_data[index++];
status_x = (lsb & BMI055_NEW_DATA_MASK);
msb = (uint16_t)accel_data[index++];
msblsb = (msb << 8) | lsb;
report.accel_x = ((int16_t)msblsb >> 4); /* Data in X axis */
lsb = (uint16_t)accel_data[index++];
status_y = (lsb & BMI055_NEW_DATA_MASK);
msb = (uint16_t)accel_data[index++];
msblsb = (msb << 8) | lsb;
report.accel_y = ((int16_t)msblsb >> 4); /* Data in Y axis */
lsb = (uint16_t)accel_data[index++];
status_z = (lsb & BMI055_NEW_DATA_MASK);
msb = (uint16_t)accel_data[index++];
msblsb = (msb << 8) | lsb;
report.accel_z = ((int16_t)msblsb >> 4); /* Data in Z axis */
// Checking the status of new data
if ((!status_x) || (!status_y) || (!status_z)) {
perf_end(_sample_perf);
perf_count(_duplicates);
_got_duplicate = true;
return;
}
_got_duplicate = false;
uint8_t temp = read_reg(BMI055_ACC_TEMP);
report.temp = temp;
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_z == 0 &&
report.temp == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the bmi055 accel does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return;
}
perf_count(_good_transfers);
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again. We still increment
// _good_transfers, but don't return any data yet
_register_wait--;
return;
}
/*
* Report buffers.
*/
accel_report arb;
arb.timestamp = hrt_absolute_time();
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
*/
arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;
float xraw_f = report.accel_x;
float yraw_f = report.accel_y;
float zraw_f = report.accel_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
math::Vector<3> aval_integrated;
bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
arb.x_integral = aval_integrated(0);
arb.y_integral = aval_integrated(1);
arb.z_integral = aval_integrated(2);
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
_last_temperature = 23 + report.temp * 1.0f / 512.0f;
arb.temperature_raw = report.temp;
arb.temperature = _last_temperature;
arb.device_id = _device_id.devid;
_accel_reports->force(&arb);
/* notify anyone waiting for data */
if (accel_notify) {
poll_notify(POLLIN);
}
if (accel_notify && !(_pub_blocked)) {
/* log the time of this report */
perf_begin(_controller_latency_perf);
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
/* stop measuring */
perf_end(_sample_perf);
}
void
BMI055_gyro::measure()
{
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct BMI_GyroReport bmi_gyroreport;
struct Report {
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the BMI055 gyro in one pass.
*/
bmi_gyroreport.cmd = BMI055_GYR_X_L | DIR_READ;
if (OK != transfer((uint8_t *)&bmi_gyroreport, ((uint8_t *)&bmi_gyroreport), sizeof(bmi_gyroreport))) {
return;
}
check_registers();
uint8_t temp = read_reg(BMI055_ACC_TEMP);
report.temp = temp;
report.gyro_x = bmi_gyroreport.gyro_x;
report.gyro_y = bmi_gyroreport.gyro_y;
report.gyro_z = bmi_gyroreport.gyro_z;
if (report.temp == 0 &&
report.gyro_x == 0 &&
report.gyro_y == 0 &&
report.gyro_z == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the bmi055 does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return;
}
perf_count(_good_transfers);
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again. We still increment
// _good_transfers, but don't return any data yet
_register_wait--;
return;
}
/*
* Report buffers.
*/
gyro_report grb;
grb.timestamp = hrt_absolute_time();
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
grb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;
float xraw_f = report.gyro_x;
float yraw_f = report.gyro_y;
float zraw_f = report.gyro_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
matrix::Vector3f gval_integrated;
bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt);
grb.x_integral = gval_integrated(0);
grb.y_integral = gval_integrated(1);
grb.z_integral = gval_integrated(2);
grb.scaling = _gyro_range_scale;
grb.range_rad_s = _gyro_range_rad_s;
grb.temperature_raw = report.temp;
grb.temperature = _last_temperature;
grb.device_id = _device_id.devid;
_gyro_reports->force(&grb);
/* notify anyone waiting for data */
if (gyro_notify) {
poll_notify(POLLIN);
}
if (gyro_notify && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
}
/* stop measuring */
perf_end(_sample_perf);
}
void
L3GD20::measure()
{
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd;
int8_t temp;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} raw_report;
#pragma pack(pop)
gyro_report report;
/* start the performance counter */
perf_begin(_sample_perf);
check_registers();
/* fetch data from the sensor */
memset(&raw_report, 0, sizeof(raw_report));
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
if (!(raw_report.status & STATUS_ZYXDA)) {
perf_end(_sample_perf);
perf_count(_duplicates);
return;
}
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_bad_registers);
switch (_orientation) {
case SENSOR_BOARD_ROTATION_000_DEG:
/* keep axes in place */
report.x_raw = raw_report.x;
report.y_raw = raw_report.y;
break;
case SENSOR_BOARD_ROTATION_090_DEG:
/* swap x and y */
report.x_raw = raw_report.y;
report.y_raw = raw_report.x;
break;
case SENSOR_BOARD_ROTATION_180_DEG:
/* swap x and y and negate both */
report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
break;
case SENSOR_BOARD_ROTATION_270_DEG:
/* swap x and y and negate y */
report.x_raw = raw_report.y;
report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
break;
}
report.z_raw = raw_report.z;
#if defined(CONFIG_ARCH_BOARD_MINDPX_V2)
int16_t tx = -report.y_raw;
int16_t ty = -report.x_raw;
int16_t tz = -report.z_raw;
report.x_raw = tx;
report.y_raw = ty;
report.z_raw = tz;
#endif
report.temperature_raw = raw_report.temp;
float xraw_f = report.x_raw;
float yraw_f = report.y_raw;
float zraw_f = report.z_raw;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float xin = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float yin = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float zin = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
report.x = _gyro_filter_x.apply(xin);
report.y = _gyro_filter_y.apply(yin);
report.z = _gyro_filter_z.apply(zin);
math::Vector<3> gval(xin, yin, zin);
math::Vector<3> gval_integrated;
bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt);
report.x_integral = gval_integrated(0);
report.y_integral = gval_integrated(1);
report.z_integral = gval_integrated(2);
report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
report.scaling = _gyro_range_scale;
report.range_rad_s = _gyro_range_rad_s;
_reports->force(&report);
if (gyro_notify) {
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
}
}
_read++;
/* stop the perf counter */
perf_end(_sample_perf);
}
int
PX4FLOW::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 };
perf_begin(_sample_perf);
if (PX4FLOW_REG == 0x00) {
ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE);
}
if (PX4FLOW_REG == 0x16) {
ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE);
}
if (ret < 0) {
DEVICE_DEBUG("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
if (PX4FLOW_REG == 0) {
memcpy(&f, val, I2C_FRAME_SIZE);
memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
}
if (PX4FLOW_REG == 0x16) {
memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
}
struct optical_flow_s report;
report.timestamp = hrt_absolute_time();
report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert to meters
report.quality = f_integral.qual; //0:bad ; 255 max quality
report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
report.integration_timespan = f_integral.integration_timespan; //microseconds
report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds
report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
report.sensor_id = 0;
/* rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT */
float zeroval = 0.0f;
rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
if (_px4flow_topic == nullptr) {
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
} else {
/* publish it */
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
}
/* publish to the distance_sensor topic as well */
struct distance_sensor_s distance_report;
distance_report.timestamp = report.timestamp;
distance_report.min_distance = PX4FLOW_MIN_DISTANCE;
distance_report.max_distance = PX4FLOW_MAX_DISTANCE;
distance_report.current_distance = report.ground_distance_m;
distance_report.covariance = 0.0f;
distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
/* TODO: the ID needs to be properly set */
distance_report.id = 0;
distance_report.orientation = _sonar_rotation;
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report);
/* post a report to the ring */
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
FXOS8700CQ::mag_measure()
{
/* status register and data as read back from the device */
mag_report mag_report {};
/* start the performance counter */
perf_begin(_mag_sample_perf);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
mag_report.timestamp = hrt_absolute_time();
mag_report.is_external = is_external();
mag_report.x_raw = _last_raw_mag_x;
mag_report.y_raw = _last_raw_mag_y;
mag_report.z_raw = _last_raw_mag_z;
float xraw_f = mag_report.x_raw;
float yraw_f = mag_report.y_raw;
float zraw_f = mag_report.z_raw;
/* apply user specified rotation */
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
mag_report.scaling = _mag_range_scale;
mag_report.range_ga = (float)_mag_range_ga;
mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
mag_report.temperature = _last_temperature;
mag_report.device_id = _mag->_device_id.devid;
_mag_reports->force(&mag_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
}
_mag_read++;
/* stop the perf counter */
perf_end(_mag_sample_perf);
}
int
LIS3MDL::collect()
{
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t y[2];
uint8_t z[2];
uint8_t t[2];
} lis_report;
struct {
int16_t x;
int16_t y;
int16_t z;
int16_t t;
} report;
#pragma pack(pop)
int ret;
// uint8_t check_counter;
perf_begin(_sample_perf);
struct mag_report new_report;
bool sensor_is_onboard = false;
float xraw_f;
float yraw_f;
float zraw_f;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
new_report.error_count = perf_event_count(_comms_errors);
new_report.range_ga = _range_ga;
new_report.scaling = _range_scale;
new_report.device_id = _device_id.devid;
/*
* @note We could read the status register here, which could tell us that
* we were too early and that the output registers are still being
* written. In the common case that would just slow us down, and
* we're better off just never being early.
*/
/* get measurements from the device */
ret = _interface->read(ADDR_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report));
if (ret != OK) {
perf_count(_comms_errors);
DEVICE_DEBUG("data/status read error");
goto out;
}
/* convert the data we just received */
report.x = (((int16_t)lis_report.x[1]) << 8) + lis_report.x[0];
report.y = (((int16_t)lis_report.y[1]) << 8) + lis_report.y[0];
report.z = (((int16_t)lis_report.z[1]) << 8) + lis_report.z[0];
report.t = (((int16_t)lis_report.t[1]) << 8) + lis_report.t[0];
/* get measurements from the device */
new_report.temperature = report.t;
new_report.temperature = 25 + (report.t / (16 * 8.0f));
// XXX revisit for SPI part, might require a bus type IOCTL
unsigned dummy;
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
new_report.is_external = !sensor_is_onboard;
/*
* RAW outputs
*
*/
new_report.x_raw = report.x;
new_report.y_raw = report.y;
new_report.z_raw = report.z;
/* the LIS3MDL mag on Pixhawk Pro by Drotek has x pointing towards,
* y pointing to the right, and z down, therefore no switch needed,
* it is better to have no artificial rotation inside the
* driver and then use the startup script with -R command with the
* real rotation between the sensor and body frame */
xraw_f = report.x;
yraw_f = report.y;
zraw_f = report.z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (!(_pub_blocked)) {
if (_mag_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_mag_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
_last_report = new_report;
/* post a report to the ring */
_reports->force(&new_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
check_conf();
ret = OK;
out:
perf_end(_sample_perf);
return ret;
}
void
MPU9250_mag::measure(struct ak8963_regs data)
{
bool mag_notify = true;
if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) {
perf_count(_mag_duplicates);
return;
}
/* monitor for if data overrun flag is ever set */
if (data.st1 & 0x02) {
perf_count(_mag_overruns);
}
/* monitor for if magnetic sensor overflow flag is ever set noting that st2
* is usually not even refreshed, but will always be in the same place in the
* mpu's buffers regardless, hence the actual count would be bogus
*/
if (data.st2 & 0x08) {
perf_count(_mag_overflows);
}
mag_report mrb;
mrb.timestamp = hrt_absolute_time();
/*
* Align axes - note the accel & gryo are also re-aligned so this
* doesn't look obvious with the datasheet
*/
mrb.x_raw = data.x;
mrb.y_raw = -data.y;
mrb.z_raw = -data.z;
float xraw_f = data.x;
float yraw_f = -data.y;
float zraw_f = -data.z;
/* apply user specified rotation */
rotate_3f(_parent->_rotation, xraw_f, yraw_f, zraw_f);
mrb.x = ((xraw_f * _mag_range_scale * _mag_asa_x) - _mag_scale.x_offset) * _mag_scale.x_scale;
mrb.y = ((yraw_f * _mag_range_scale * _mag_asa_y) - _mag_scale.y_offset) * _mag_scale.y_scale;
mrb.z = ((zraw_f * _mag_range_scale * _mag_asa_z) - _mag_scale.z_offset) * _mag_scale.z_scale;
mrb.range_ga = (float)48.0;
mrb.scaling = _mag_range_scale;
mrb.temperature = _parent->_last_temperature;
mrb.error_count = perf_event_count(_mag_errors);
_mag_reports->force(&mrb);
/* notify anyone waiting for data */
if (mag_notify) {
poll_notify(POLLIN);
}
if (mag_notify && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mrb);
}
}
void
FXAS21002C::measure()
{
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} raw_gyro_report;
#pragma pack(pop)
struct gyro_report gyro_report;
/* start the performance counter */
perf_begin(_sample_perf);
check_registers();
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again.
_register_wait--;
perf_end(_sample_perf);
return;
}
/* fetch data from the sensor */
memset(&raw_gyro_report, 0, sizeof(raw_gyro_report));
raw_gyro_report.cmd = DIR_READ(FXAS21002C_STATUS);
transfer((uint8_t *)&raw_gyro_report, (uint8_t *)&raw_gyro_report, sizeof(raw_gyro_report));
if (!(raw_gyro_report.status & DR_STATUS_ZYXDR)) {
perf_end(_sample_perf);
perf_count(_duplicates);
return;
}
/*
* The TEMP register contains an 8-bit 2's complement temperature value with a range
* of –128 °C to +127 °C and a scaling of 1 °C/LSB. The temperature data is only
* compensated (factory trim values applied) when the device is operating in the Active
* mode and actively measuring the angular rate.
*/
if ((_read % _current_rate) == 0) {
_last_temperature = read_reg(FXAS21002C_TEMP) * 1.0f;
gyro_report.temperature = _last_temperature;
}
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
gyro_report.timestamp = hrt_absolute_time();
// report the error count as the number of bad
// register reads. This allows the higher level
// code to decide if it should use this sensor based on
// whether it has had failures
gyro_report.error_count = perf_event_count(_bad_registers);
gyro_report.x_raw = swap16(raw_gyro_report.x);
gyro_report.y_raw = swap16(raw_gyro_report.y);
gyro_report.z_raw = swap16(raw_gyro_report.z);
float xraw_f = gyro_report.x_raw;
float yraw_f = gyro_report.y_raw;
float zraw_f = gyro_report.z_raw;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
gyro_report.x = _gyro_filter_x.apply(x_in_new);
gyro_report.y = _gyro_filter_y.apply(y_in_new);
gyro_report.z = _gyro_filter_z.apply(z_in_new);
matrix::Vector3f gval(x_in_new, y_in_new, z_in_new);
matrix::Vector3f gval_integrated;
bool gyro_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt);
gyro_report.x_integral = gval_integrated(0);
gyro_report.y_integral = gval_integrated(1);
gyro_report.z_integral = gval_integrated(2);
gyro_report.scaling = _gyro_range_scale;
/* return device ID */
gyro_report.device_id = _device_id.devid;
_reports->force(&gyro_report);
/* notify anyone waiting for data */
if (gyro_notify) {
poll_notify(POLLIN);
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
}
}
_read++;
/* stop the perf counter */
perf_end(_sample_perf);
}
void
MPU9250::measure()
{
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct MPUReport mpu_report;
struct ICMReport icm_report;
struct Report {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the MPU9250 in one pass
*/
if ((!_magnetometer_only || _mag->is_passthrough()) && _register_wait == 0) {
if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) {
if (OK != read_reg_range(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&mpu_report, sizeof(mpu_report))) {
perf_end(_sample_perf);
return;
}
} else { // ICM20948
select_register_bank(REG_BANK(ICMREG_20948_ACCEL_XOUT_H));
if (OK != read_reg_range(ICMREG_20948_ACCEL_XOUT_H, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&icm_report,
sizeof(icm_report))) {
perf_end(_sample_perf);
return;
}
}
check_registers();
if (check_duplicate(MPU_OR_ICM(&mpu_report.accel_x[0], &icm_report.accel_x[0]))) {
return;
}
}
/*
* In case of a mag passthrough read, hand the magnetometer data over to _mag. Else,
* try to read a magnetometer report.
*/
# ifdef USE_I2C
if (_mag->is_passthrough()) {
# endif
_mag->_measure(mpu_report.mag);
# ifdef USE_I2C
} else {
_mag->measure();
}
# endif
/*
* Continue evaluating gyro and accelerometer results
*/
if (!_magnetometer_only && _register_wait == 0) {
/*
* Convert from big to little endian
*/
if (_whoami == ICM_WHOAMI_20948) {
report.accel_x = int16_t_from_bytes(icm_report.accel_x);
report.accel_y = int16_t_from_bytes(icm_report.accel_y);
report.accel_z = int16_t_from_bytes(icm_report.accel_z);
report.temp = int16_t_from_bytes(icm_report.temp);
report.gyro_x = int16_t_from_bytes(icm_report.gyro_x);
report.gyro_y = int16_t_from_bytes(icm_report.gyro_y);
report.gyro_z = int16_t_from_bytes(icm_report.gyro_z);
} else { // MPU9250/MPU6500
report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
report.temp = int16_t_from_bytes(mpu_report.temp);
report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
}
if (check_null_data((uint16_t *)&report, sizeof(report) / 2)) {
return;
}
}
if (_register_wait != 0) {
/*
* We are waiting for some good transfers before using the sensor again.
* We still increment _good_transfers, but don't return any data yet.
*
*/
_register_wait--;
return;
}
/*
* Get sensor temperature
*/
_last_temperature = (report.temp) / 333.87f + 21.0f;
/*
* Convert and publish accelerometer and gyrometer data.
*/
if (!_magnetometer_only) {
/*
* Keeping the axes as they are for ICM20948 so orientation will match the actual chip orientation
*/
if (_whoami != ICM_WHOAMI_20948) {
/*
* Swap axes and negate y
*/
int16_t accel_xt = report.accel_y;
int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
int16_t gyro_xt = report.gyro_y;
int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
/*
* Apply the swap
*/
report.accel_x = accel_xt;
report.accel_y = accel_yt;
report.gyro_x = gyro_xt;
report.gyro_y = gyro_yt;
}
/*
* Report buffers.
*/
sensor_accel_s arb;
sensor_gyro_s grb;
/*
* Adjust and scale results to m/s^2.
*/
grb.timestamp = arb.timestamp = hrt_absolute_time();
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
/* NOTE: Axes have been swapped to match the board a few lines above. */
arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;
float xraw_f = report.accel_x;
float yraw_f = report.accel_y;
float zraw_f = report.accel_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
matrix::Vector3f aval(x_in_new, y_in_new, z_in_new);
matrix::Vector3f aval_integrated;
bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
arb.x_integral = aval_integrated(0);
arb.y_integral = aval_integrated(1);
arb.z_integral = aval_integrated(2);
arb.scaling = _accel_range_scale;
arb.temperature = _last_temperature;
/* return device ID */
arb.device_id = _accel->_device_id.devid;
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;
xraw_f = report.gyro_x;
yraw_f = report.gyro_y;
zraw_f = report.gyro_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
matrix::Vector3f gval_integrated;
bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
grb.x_integral = gval_integrated(0);
grb.y_integral = gval_integrated(1);
grb.z_integral = gval_integrated(2);
grb.scaling = _gyro_range_scale;
grb.temperature = _last_temperature;
/* return device ID */
grb.device_id = _gyro->_device_id.devid;
_accel_reports->force(&arb);
_gyro_reports->force(&grb);
/* notify anyone waiting for data */
if (accel_notify) {
_accel->poll_notify(POLLIN);
}
if (gyro_notify) {
_gyro->parent_poll_notify();
}
if (accel_notify && !(_accel->_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (gyro_notify && !(_gyro->_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
}
/* stop measuring */
perf_end(_sample_perf);
}
