int PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY) { int ret; uint8_t data[10] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0, DIR_READ(0x05), 0, DIR_READ(0x06), 0 }; ret = transfer(&data[0], &data[0], 10); if (OK != ret) { perf_count(_comms_errors); DEVICE_LOG("spi::transfer returned %d", ret); return ret; } deltaX = ((int16_t)data[5] << 8) | data[3]; deltaY = ((int16_t)data[9] << 8) | data[7]; ret = OK; return ret; }
uint8_t FXAS21002C::read_reg(unsigned reg) { uint8_t cmd[2]; cmd[0] = DIR_READ(reg); cmd[1] = 0; transfer(cmd, cmd, sizeof(cmd)); return cmd[1]; }
uint8_t FXOS8700CQ::read_reg(unsigned reg) { uint8_t cmd[3]; cmd[0] = DIR_READ(reg); cmd[1] = ADDR_7(reg); cmd[2] = 0; transfer(cmd, cmd, sizeof(cmd)); return cmd[2]; }
int PMW3901::readRegister(unsigned reg, uint8_t *data, unsigned count) { uint8_t cmd[5]; // read up to 4 bytes int ret; cmd[0] = DIR_READ(reg); ret = transfer(&cmd[0], &cmd[0], count + 1); if (OK != ret) { perf_count(_comms_errors); DEVICE_LOG("spi::transfer returned %d", ret); return ret; } memcpy(&data[0], &cmd[1], count); return ret; }
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 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); }