int
TRONE::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[3] = {0, 0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 3);
if (ret < 0) {
DEVICE_LOG("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint16_t distance_mm = (val[0] << 8) | val[1];
float distance_m = float(distance_mm) * 1e-3f;
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
/* there is no enum item for a combined LASER and ULTRASOUND which it should be */
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = 8;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
/* TODO: set proper ID */
report.id = 0;
// This validation check can be used later
_valid = crc8(val, 2) == val[2] && (float)report.current_distance > report.min_distance
&& (float)report.current_distance < report.max_distance ? 1 : 0;
/* publish it, if we are the primary */
if (_distance_sensor_topic != nullptr) {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
SRF02_I2C::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[2] = {0, 0};
uint8_t cmd = 0x02;
perf_begin(_sample_perf);
ret = transfer(&cmd, 1, nullptr, 0);
ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
DEVICE_DEBUG("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint16_t distance_cm = val[0] << 8 | val[1];
float distance_m = float(distance_cm) * 1e-2f;
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
report.orientation = 8;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
/* TODO: set proper ID */
report.id = 0;
/* publish it, if we are the primary */
if (_distance_sensor_topic != nullptr) {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
/*
* publish some data from the ISR in the ring buffer
*/
void PWMIN::publish(uint16_t status, uint32_t period, uint32_t pulse_width)
{
/* if we missed an edge, we have to give up */
if (status & SR_OVF_PWMIN) {
_error_count++;
return;
}
_last_poll_time = hrt_absolute_time();
struct pwm_input_s pwmin_report;
pwmin_report.timestamp = _last_poll_time;
pwmin_report.error_count = _error_count;
pwmin_report.period = period;
pwmin_report.pulse_width = pulse_width;
_reports->force(&pwmin_report);
}
void leddar_one::_publish(uint16_t distance_mm)
{
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
report.orientation = _rotation;
report.current_distance = ((float)distance_mm / 1000.0f);
report.min_distance = MIN_DISTANCE;
report.max_distance = MAX_DISTANCE;
report.covariance = 0.0f;
report.id = 0;
_reports->force(&report);
if (_topic == nullptr) {
_topic = orb_advertise(ORB_ID(distance_sensor), &report);
} else {
orb_publish(ORB_ID(distance_sensor), _topic, &report);
}
}
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
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
BMA180::measure()
{
/* BMA180 measurement registers */
// #pragma pack(push, 1)
// struct {
// uint8_t cmd;
// int16_t x;
// int16_t y;
// int16_t z;
// } raw_report;
// #pragma pack(pop)
struct accel_report report;
/* start the performance counter */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the BMA180 in one pass;
* starting from the X LSB.
*/
//raw_report.cmd = ADDR_ACC_X_LSB;
// XXX PX4DEV transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
/*
* Adjust and scale results to SI units.
*
* Note that we ignore the "new data" bits. At any time we read, each
* of the axis measurements are the "most recent", even if we've seen
* them before. There is no good way to synchronise with the internal
* measurement flow without using the external interrupt.
*/
report.timestamp = hrt_absolute_time();
report.error_count = 0;
/*
* y of board is x of sensor and x of board is -y of sensor
* perform only the axis assignment here.
* Two non-value bits are discarded directly
*/
report.y_raw = read_reg(ADDR_ACC_X_LSB + 0);
report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
report.x_raw = read_reg(ADDR_ACC_X_LSB + 2);
report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
report.z_raw = read_reg(ADDR_ACC_X_LSB + 4);
report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
/* discard two non-value bits in the 16 bit measurement */
report.x_raw = (report.x_raw / 4);
report.y_raw = (report.y_raw / 4);
report.z_raw = (report.z_raw / 4);
/* invert y axis, due to 14 bit data no overflow can occur in the negation */
report.y_raw = -report.y_raw;
report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
report.scaling = _accel_range_scale;
report.range_m_s2 = _accel_range_m_s2;
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
if (_accel_topic != nullptr && !(_pub_blocked)) {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
}
/* stop the perf counter */
perf_end(_sample_perf);
}
void
BATT_SMBUS::cycle()
{
// get current time
uint64_t now = hrt_absolute_time();
// exit without rescheduling if we have failed to find a battery after 10 seconds
if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) {
warnx("did not find smart battery");
return;
}
// read data from sensor
struct battery_status_s new_report;
// set time of reading
new_report.timestamp = now;
// read voltage
uint16_t tmp;
if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) {
// initialise new_report
memset(&new_report, 0, sizeof(new_report));
// convert millivolts to volts
new_report.voltage_v = ((float)tmp) / 1000.0f;
// read current
uint8_t buff[4];
if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) {
new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 |
(uint32_t)buff[0])) / 1000.0f;
}
// read battery design capacity
if (_batt_capacity == 0) {
if (read_reg(BATT_SMBUS_FULL_CHARGE_CAPACITY, tmp) == OK) {
_batt_capacity = tmp;
}
}
// read remaining capacity
if (_batt_capacity > 0) {
if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) {
if (tmp < _batt_capacity) {
new_report.discharged_mah = _batt_capacity - tmp;
}
}
}
// publish to orb
if (_batt_topic != nullptr) {
orb_publish(_batt_orb_id, _batt_topic, &new_report);
} else {
_batt_topic = orb_advertise(_batt_orb_id, &new_report);
if (_batt_topic == nullptr) {
errx(1, "ADVERT FAIL");
}
}
// copy report for test()
_last_report = new_report;
// post a report to the ring
_reports->force(&new_report);
// notify anyone waiting for data
poll_notify(POLLIN);
// record we are working
_enabled = true;
}
// schedule a fresh cycle call when the measurement is done
work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this,
USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS));
}
int
TFMINI::collect()
{
perf_begin(_sample_perf);
/* clear buffer if last read was too long ago */
int64_t read_elapsed = hrt_elapsed_time(&_last_read);
/* the buffer for read chars is buflen minus null termination */
char readbuf[sizeof(_linebuf)];
unsigned readlen = sizeof(readbuf) - 1;
int ret = 0;
float distance_m = -1.0f;
/* Check the number of bytes available in the buffer*/
int bytes_available = 0;
::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);
if (!bytes_available) {
return -EAGAIN;
}
/* parse entire buffer */
do {
/* read from the sensor (uart buffer) */
ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) {
PX4_ERR("read err: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
/* only throw an error if we time out */
if (read_elapsed > (_conversion_interval * 2)) {
/* flush anything in RX buffer */
tcflush(_fd, TCIFLUSH);
return ret;
} else {
return -EAGAIN;
}
}
_last_read = hrt_absolute_time();
/* parse buffer */
for (int i = 0; i < ret; i++) {
tfmini_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m);
}
/* bytes left to parse */
bytes_available -= ret;
} while (bytes_available > 0);
/* no valid measurement after parsing buffer */
if (distance_m < 0.0f) {
return -EAGAIN;
}
/* publish most recent valid measurement from buffer */
distance_sensor_s report{};
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = _rotation;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
report.signal_quality = -1;
/* TODO: set proper ID */
report.id = 0;
/* publish it */
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
SF0X::collect()
{
int ret;
perf_begin(_sample_perf);
/* clear buffer if last read was too long ago */
uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
/* the buffer for read chars is buflen minus null termination */
char readbuf[sizeof(_linebuf)];
unsigned readlen = sizeof(readbuf) - 1;
/* read from the sensor (uart buffer) */
ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) {
DEVICE_DEBUG("read err: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
/* only throw an error if we time out */
if (read_elapsed > (_conversion_interval * 2)) {
return ret;
} else {
return -EAGAIN;
}
} else if (ret == 0) {
return -EAGAIN;
}
_last_read = hrt_absolute_time();
float distance_m = -1.0f;
bool valid = false;
for (int i = 0; i < ret; i++) {
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
valid = true;
}
}
if (!valid) {
return -EAGAIN;
}
DEVICE_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO"));
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = _rotation;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
/* TODO: set proper ID */
report.id = 0;
/* publish it */
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
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) {
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;
}
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
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
LPS25H::collect()
{
#pragma pack(push, 1)
struct {
uint8_t status;
uint8_t p_xl, p_l, p_h;
int16_t t;
} report;
#pragma pack(pop)
int ret;
perf_begin(_sample_perf);
struct baro_report new_report;
bool sensor_is_onboard = false;
/* 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 1 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 : MSB enables register address auto-increment */
ret = _interface->read(ADDR_STATUS_REG | (1 << 7), (uint8_t *)&report, sizeof(report));
if (ret != OK) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
/* get measurements from the device */
new_report.temperature = 42.5 + (report.t / 480);
/* raw pressure */
uint32_t raw = report.p_xl + (report.p_l << 8) + (report.p_h << 16);
/* Pressure and MSL in mBar */
double p = raw / 4096.0;
double msl = _msl_pressure / 100.0;
double alt = (1.0 - pow(p / msl, 0.190263)) * 44330.8;
new_report.pressure = p;
new_report.altitude = alt;
/* get device ID */
new_report.device_id = _device_id.devid;
if (!(_pub_blocked)) {
if (_baro_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &new_report);
} else {
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &new_report,
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_baro_topic == nullptr) {
DEVICE_DEBUG("ADVERT FAIL");
}
}
}
_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);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
/*
* Method: collect()
*
* This method reads data from serial UART and places it into a buffer
*/
CM8JL65::collect()
{
int bytes_read = 0;
int bytes_processed = 0;
int i = 0;
bool crc_valid = false;
perf_begin(_sample_perf);
/* read from the sensor (uart buffer) */
bytes_read = ::read(_fd, &_linebuf[0], sizeof(_linebuf));
if (bytes_read < 0) {
PX4_DEBUG("read err: %d \n", bytes_read);
perf_count(_comms_errors);
perf_end(_sample_perf);
} else if (bytes_read > 0) {
// printf("Bytes read: %d \n",bytes_read);
i = bytes_read - 6 ;
while ((i >= 0) && (!crc_valid)) {
if (_linebuf[i] == START_FRAME_DIGIT1) {
bytes_processed = i;
while ((bytes_processed < bytes_read) && (!crc_valid)) {
// printf("In the cycle, processing byte %d, 0x%02X \n",bytes_processed, _linebuf[bytes_processed]);
if (OK == cm8jl65_parser(_linebuf[bytes_processed], _frame_data, _parse_state, _crc16, _distance_mm)) {
crc_valid = true;
}
bytes_processed++;
}
_parse_state = STATE0_WAITING_FRAME;
}
i--;
}
}
if (!crc_valid) {
return -EAGAIN;
}
//printf("val (int): %d, raw: 0x%08X, valid: %s \n", _distance_mm, _frame_data, ((crc_valid) ? "OK" : "NO"));
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = _rotation;
report.current_distance = _distance_mm / 1000.0f;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.variance = 0.0f;
report.signal_quality = -1;
/* TODO: set proper ID */
report.id = 0;
/* publish it */
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
bytes_read = OK;
perf_end(_sample_perf);
return OK;
}
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);
}
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
BAROSIM::collect()
{
int ret;
#pragma pack(push, 1)
struct {
float pressure;
float altitude;
float temperature;
} baro_report;
#pragma pack(pop)
perf_begin(_sample_perf);
struct baro_report report;
/* this should be fairly close to the end of the conversion, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
/* read the most recent measurement - read offset/size are hardcoded in the interface */
ret = _interface->dev_read(0, (void *)&baro_report, sizeof(baro_report));
if (ret < 0) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
/* handle a measurement */
if (_measure_phase == 0) {
report.pressure = baro_report.pressure;
report.altitude = baro_report.altitude;
report.temperature = baro_report.temperature;
report.timestamp = hrt_absolute_time();
} else {
report.pressure = baro_report.pressure;
report.altitude = baro_report.altitude;
report.temperature = baro_report.temperature;
report.timestamp = hrt_absolute_time();
/* publish it */
if (!(_pub_blocked)) {
if (_baro_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
} else {
PX4_WARN("BAROSIM::collect _baro_topic not initialized");
}
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
}
/* update the measurement state machine */
INCREMENT(_measure_phase, BAROSIM_MEASUREMENT_RATIO + 1);
perf_end(_sample_perf);
return OK;
}
void
ACCELSIM::_measure()
{
#if 0
static int x = 0;
// Verify the samples are being taken at the expected rate
if (x == 99) {
x = 0;
PX4_INFO("ACCELSIM::measure %" PRIu64, hrt_absolute_time());
} else {
x++;
}
#endif
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t status;
float temperature;
float x;
float y;
float z;
} raw_accel_report;
#pragma pack(pop)
accel_report accel_report = {};
/* start the performance counter */
perf_begin(_accel_sample_perf);
/* fetch data from the sensor */
memset(&raw_accel_report, 0, sizeof(raw_accel_report));
raw_accel_report.cmd = DIR_READ | ACC_READ;
if (OK != transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report))) {
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();
// use the temperature from the last mag reading
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 = (int16_t)(raw_accel_report.x / _accel_range_scale);
accel_report.y_raw = (int16_t)(raw_accel_report.y / _accel_range_scale);
accel_report.z_raw = (int16_t)(raw_accel_report.z / _accel_range_scale);
accel_report.x = raw_accel_report.x;
accel_report.y = raw_accel_report.y;
accel_report.z = raw_accel_report.z;
accel_report.scaling = _accel_range_scale;
accel_report.range_m_s2 = _accel_range_m_s2;
_accel_reports->force(&accel_report);
/* notify anyone waiting for data */
DevMgr::updateNotify(*this);
if (!(m_pub_blocked)) {
/* publish it */
// The first call to measure() is from init() and _accel_topic is not
// yet initialized
if (_accel_topic != nullptr) {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
}
_accel_read++;
/* stop the perf counter */
perf_end(_accel_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
ACCELSIM::mag_measure()
{
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t status;
float temperature;
float x;
float y;
float z;
} raw_mag_report;
#pragma pack(pop)
mag_report mag_report;
memset(&mag_report, 0, sizeof(mag_report));
/* start the performance counter */
perf_begin(_mag_sample_perf);
/* fetch data from the sensor */
memset(&raw_mag_report, 0, sizeof(raw_mag_report));
raw_mag_report.cmd = DIR_READ | MAG_READ;
if (OK != transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report))) {
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.
*/
mag_report.timestamp = hrt_absolute_time();
mag_report.x_raw = (int16_t)(raw_mag_report.x / _mag_range_scale);
mag_report.y_raw = (int16_t)(raw_mag_report.y / _mag_range_scale);
mag_report.z_raw = (int16_t)(raw_mag_report.z / _mag_range_scale);
float xraw_f = (int16_t)(raw_mag_report.x / _mag_range_scale);
float yraw_f = (int16_t)(raw_mag_report.y / _mag_range_scale);
float zraw_f = (int16_t)(raw_mag_report.z / _mag_range_scale);
/* apply user specified rotation */
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
/* remember the temperature. The datasheet isn't clear, but it
* seems to be a signed offset from 25 degrees C in units of 0.125C
*/
_last_temperature = raw_mag_report.temperature;
mag_report.temperature = _last_temperature;
mag_report.x = raw_mag_report.x;
mag_report.y = raw_mag_report.y;
mag_report.z = raw_mag_report.z;
_mag_reports->force(&mag_report);
/* notify anyone waiting for data */
DevMgr::updateNotify(*this);
if (!(m_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);
}
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);
}
int
BAROSIM::collect()
{
//PX4_WARN("baro collect %llu", hrt_absolute_time());
int ret;
#pragma pack(push, 1)
struct raw_baro_s {
float pressure;
float altitude;
float temperature;
} raw_baro;
#pragma pack(pop)
perf_begin(_sample_perf);
struct baro_report report = {};
/* this should be fairly close to the end of the conversion, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
/* read the most recent measurement - read offset/size are hardcoded in the interface */
uint8_t cmd = 0;
ret = transfer(&cmd, 1, (uint8_t *)(&raw_baro), sizeof(raw_baro));
if (ret < 0) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
/* handle a measurement */
if (_measure_phase == 0) {
report.pressure = raw_baro.pressure;
report.altitude = raw_baro.altitude;
report.temperature = raw_baro.temperature;
} else {
report.pressure = raw_baro.pressure;
report.altitude = raw_baro.altitude;
report.temperature = raw_baro.temperature;
/* publish it */
if (!(m_pub_blocked)) {
if (_baro_topic != nullptr) {
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
} else {
PX4_WARN("BAROSIM::collect _baro_topic not initialized");
}
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
//DevMgr::updateNotify(*this);
updateNotify();
}
/* update the measurement state machine */
INCREMENT(_measure_phase, BAROSIM_MEASUREMENT_RATIO + 1);
perf_end(_sample_perf);
return OK;
}