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 DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) { /* Check if calibration values are still up-to-date. */ bool updated; orb_check(_param_update_sub, &updated); if (updated) { parameter_update_s parameter_update; orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update); _update_accel_calibration(); _update_gyro_calibration(); _update_mag_calibration(); } accel_report accel_report = {}; gyro_report gyro_report = {}; mag_report mag_report = {}; accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); // ACCEL float xraw_f = data.accel_m_s2_x; float yraw_f = data.accel_m_s2_y; float zraw_f = data.accel_m_s2_z; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); // MPU9250 driver from DriverFramework does not provide any raw values // TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM accel_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [m / s^2 * 1000]; accel_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [m / s^2 * 1000]; accel_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [m / s^2 * 1000]; // adjust values according to the calibration float x_in_new = (xraw_f - _accel_calibration.x_offset) * _accel_calibration.x_scale; float y_in_new = (yraw_f - _accel_calibration.y_offset) * _accel_calibration.y_scale; float z_in_new = (zraw_f - _accel_calibration.z_offset) * _accel_calibration.z_scale; 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); matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); matrix::Vector3f aval_integrated; _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); // GYRO xraw_f = data.gyro_rad_s_x; yraw_f = data.gyro_rad_s_y; zraw_f = data.gyro_rad_s_z; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); // MPU9250 driver from DriverFramework does not provide any raw values // TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM gyro_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [rad / s * 1000]; gyro_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [rad / s * 1000]; gyro_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [rad / s * 1000]; // adjust values according to the calibration float x_gyro_in_new = (xraw_f - _gyro_calibration.x_offset) * _gyro_calibration.x_scale; float y_gyro_in_new = (yraw_f - _gyro_calibration.y_offset) * _gyro_calibration.y_scale; float z_gyro_in_new = (zraw_f - _gyro_calibration.z_offset) * _gyro_calibration.z_scale; gyro_report.x = _gyro_filter_x.apply(x_gyro_in_new); gyro_report.y = _gyro_filter_y.apply(y_gyro_in_new); gyro_report.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; _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); // If we are not receiving the last sample from the FIFO buffer yet, let's stop here // and wait for more packets. if (!data.is_last_fifo_sample) { return 0; } // The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz. // Therefore, only publish every forth time. ++_publish_count; if (_publish_count < 4) { return 0; } _publish_count = 0; // Update all the counters. perf_set_count(_read_counter, data.read_counter); perf_set_count(_error_counter, data.error_counter); perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter); perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter); perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter); perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter); if (_mag_enabled) { perf_set_count(_mag_fifo_overflow_counter, data.mag_fifo_overflow_counter); } perf_begin(_publish_perf); // TODO: get these right gyro_report.scaling = -1.0f; gyro_report.range_rad_s = -1.0f; gyro_report.device_id = m_id.dev_id; accel_report.scaling = -1.0f; accel_report.range_m_s2 = -1.0f; accel_report.device_id = m_id.dev_id; if (_mag_enabled) { mag_report.timestamp = accel_report.timestamp; mag_report.is_external = false; mag_report.scaling = -1.0f; mag_report.range_ga = -1.0f; mag_report.device_id = m_id.dev_id; xraw_f = data.mag_ga_x; yraw_f = data.mag_ga_y; zraw_f = data.mag_ga_z; rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); // MPU9250 driver from DriverFramework does not provide any raw values // TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM mag_report.x_raw = xraw_f * 1000; // (int16) [Gs * 1000] mag_report.y_raw = yraw_f * 1000; // (int16) [Gs * 1000] mag_report.z_raw = zraw_f * 1000; // (int16) [Gs * 1000] mag_report.x = (xraw_f - _mag_calibration.x_offset) * _mag_calibration.x_scale; mag_report.y = (yraw_f - _mag_calibration.y_offset) * _mag_calibration.y_scale; mag_report.z = (zraw_f - _mag_calibration.z_offset) * _mag_calibration.z_scale; } // TODO: when is this ever blocked? if (!(m_pub_blocked)) { if (_gyro_topic != nullptr) { orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); } if (_accel_topic != nullptr) { orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); } if (_mag_enabled) { if (_mag_topic == nullptr) { _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report, &_mag_orb_class_instance, ORB_PRIO_LOW); } else { orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); } } // Report if there are high vibrations, every 10 times it happens. const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10); // Report every 5s. const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000); if (threshold_reached && due_to_report) { mavlink_log_critical(&_mavlink_log_pub, "High accelerations, range exceeded %llu times", data.accel_range_hit_counter); _last_accel_range_hit_time = hrt_absolute_time(); _last_accel_range_hit_count = data.accel_range_hit_counter; } } perf_end(_publish_perf); // TODO: check the return codes of this function return 0; };
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); }
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); }