int ft_xor(t_process *process, int i) { t_parameters param; param.o = 8; decode_ocp(process, ¶m, i); param.jump = 1 + g_op_tab[i].has_ocp; if (g_op_tab[i].has_ocp == 0) return (1); params_value(process, ¶m, i); if (check_registers(¶m, process, i) == 1 || param.type[2] != REG_CODE || check_param_error(param, i) == 1 || g_op_tab[i].param_nbr < 3 || g_op_tab[i].param_nbr > 4) return (param.jump); if (param.type[0] == REG_CODE) param.value[0] = RBE(process->registers[param.value[0] - 1], REG_SIZE); else if (param.type[0] == IND_CODE) PV[0] = rm(mem(process->pc + PV[0], 1, PA, process), REG_SIZE, PP); if (param.type[1] == REG_CODE) param.value[1] = RBE(process->registers[param.value[1] - 1], REG_SIZE); else if (param.type[1] == IND_CODE) PV[1] = rm(mem(process->pc + PV[1], 1, PA, process), REG_SIZE, PP); ft_write_big_endian((PV[0] ^ PV[1]), PR[PV[2] - 1], REG_SIZE); change_carry(process, (PV[0] ^ PV[1])); return (param.jump); }
int load_index(t_process *process, int i) { t_parameters param; param.o = 10; decode_ocp(process, ¶m, i); param.jump = 1 + g_op_tab[i].has_ocp; if (g_op_tab[i].has_ocp == 0) return (1); params_value(process, ¶m, i); if (check_registers(¶m, process, i) == 1 || g_op_tab[i].param_nbr < 3 || check_param_error(param, i) == 1 || g_op_tab[i].param_nbr > 4) return (param.jump); if (param.type[0] == REG_CODE) param.value[0] = RBE(process->registers[param.value[0] - 1], REG_SIZE); if (param.type[1] == REG_CODE) param.value[1] = RBE(process->registers[param.value[1] - 1], REG_SIZE); PV[0] = rm(mem(process->pc + PV[0] + PV[1], 1, PA, process), REG_SIZE, PP); if (param.type[2] == REG_CODE) WBE(PV[0], PR[PV[2] - 1], REG_SIZE); else if (param.type[2] == IND_CODE) wm(PV[0], mem(process->pc + PV[0], 1, PA, process), REG_SIZE, PP); return (param.jump); }
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 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 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 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 main(int argc, char **argv) { char *dev = NULL; int arg; int driver_debug = 0; int delay_counter = 0; int options_stop_on_error = 1; int options_stop_quiet = 0; unsigned int driver_version; int cpu_type; int iface; int reg_check_loops = 1; int align_check_loops = 1; int external_mem_check_loops = 1; int internal_mem_check_loops = 1; int external_ram_execute_loops = 1; int internal_ram_execute_loops = 1; if (argc <= 1) { usage(); } for (arg = 1; arg < argc; arg++) { if (argv[arg][0] != '-') { if (dev) { printf(" Device name specified more than once"); exit(1); } dev = argv[arg]; } else { switch (argv[arg][1]) { case 'd': arg++; if (arg == argc) { printf(" -d option requires a parameter"); exit(0); } driver_debug = strtoul(argv[arg], NULL, 0); break; case 'v': options_verbose = 1; break; case 'D': arg++; if (arg == argc) { printf(" -D option requires a parameter"); exit(0); } delay_counter = strtoul(argv[arg], NULL, 0); break; case 'r': arg++; if (arg == argc) { printf(" -r option requires a parameter"); exit(0); } reg_check_loops = strtoul(argv[arg], NULL, 0); break; case 'm': arg++; if (arg == argc) { printf(" -m option requires a parameter"); exit(0); } internal_mem_check_loops = strtoul(argv[arg], NULL, 0); break; case 'M': arg++; if (arg == argc) { printf(" -M option requires a parameter"); exit(0); } external_mem_check_loops = strtoul(argv[arg], NULL, 0); break; case 'e': arg++; if (arg == argc) { printf(" -e option requires a parameter"); exit(0); } internal_ram_execute_loops = strtoul(argv[arg], NULL, 0); break; case 'E': arg++; if (arg == argc) { printf(" -E option requires a parameter"); exit(0); } external_ram_execute_loops = strtoul(argv[arg], NULL, 0); break; case 'a': arg++; if (arg == argc) { printf(" -a option requires a parameter"); exit(0); } align_check_loops = strtoul(argv[arg], NULL, 0); break; case 'C': options_stop_on_error = 0; break; case 'Q': options_stop_quiet = 1; break; default: printf(" Unknown option!"); case '?': case 'h': usage(); break; } } } if (!dev) { printf(" ERROR: No BDM device set.\n"); exit(1); } if (bdmOpen(dev) < 0) show_error("Open"); if (driver_debug) bdmSetDriverDebugFlag(driver_debug); if (delay_counter) bdmSetDelay(delay_counter); if (bdmGetDrvVersion(&driver_version) < 0) show_error("GetDrvVersion"); printf("BDM Driver Version : %x.%x\n", driver_version >> 8, driver_version & 0xff); if (bdmGetProcessor(&cpu_type) < 0) show_error("GetProcessor"); switch(cpu_type) { case BDM_CPU32: printf("Processor : CPU32\n"); break; default: printf("Unsupported processor type!\n"); clean_exit(1); break; } if (bdmGetInterface(&iface) < 0) show_error("GetInterface"); switch(iface) { case BDM_CPU32_ERIC: printf("Interface : Eric's CPU32\n"); break; case BDM_CPU32_ICD: printf("Interface : ICD (P&E) CPU32\n"); break; default: printf("Unknown or unsupported interface type!\n"); clean_exit(1); break; } stop_on_error = options_stop_on_error; stop_quiet = options_stop_quiet; if (reg_check_loops) check_registers(reg_check_loops); if (internal_mem_check_loops) verify_internal_mem(internal_mem_check_loops); /* * Only test the whole memory in the address test; the others take too long. */ if (external_mem_check_loops) verify_external_mem(external_mem_check_loops, EXTERNAL_MEM_SIZE, 0x400); if (align_check_loops) check_alignment(align_check_loops); if (internal_ram_execute_loops) execute(INTERNAL_RAM, internal_ram_execute_loops); if (external_ram_execute_loops) execute(EXTERNAL_RAM, external_ram_execute_loops); clean_exit(0); 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(); #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); perf_end(_sample_perf); return; } #endif /* 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 (_bus == PX4_SPI_BUS_SENSORS && (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 = 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; report.temperature_raw = raw_report.temp; 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); report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp; // 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(sensor_gyro), _gyro_topic, &report); } _read++; /* stop the perf counter */ perf_end(_sample_perf); }
int main(int argc, char *argv[]) { const char *header = NULL; const char *output = NULL; gboolean check_only = FALSE; int verbose = 0, opt; GList *block_list = NULL; while( (opt = getopt_long( argc, argv, short_options, long_options, NULL )) != -1 ) { switch(opt) { case 'c': check_only = TRUE; break; case 'd': header = optarg; break; case 'h': print_usage(); exit(0); case 'o': output = optarg; break; case 'v': verbose++; break; } } if( optind == argc ) { print_usage(); exit(1); } if( output == NULL ) { output = makeFilename(argv[optind],".c"); } if( header == NULL ) { header = makeFilename(argv[optind],".h"); } for( ; optind < argc; optind++ ) { block_list = ioparse(argv[optind], block_list); } if( verbose ) { dump_register_blocks(stdout, block_list, verbose>1); } int errors = check_registers(block_list); if( errors != 0 ) { fprintf( stderr, "Aborting due to validation errors\n" ); return 2; } if( !check_only ) { FILE *f = fopen( output, "wo" ); if( f == NULL ) { fprintf( stderr, "Unable to open output file '%s': %s\n", output, strerror(errno) ); return 3; } write_source( f, block_list, header ); fclose(f); f = fopen( header, "wo" ); if( f == NULL ) { fprintf( stderr, "Unable to open header file '%s': %s\n", header, strerror(errno) ); return 4; } write_header( f, block_list ); fclose(f); } for( GList *ptr = block_list; ptr != NULL; ptr = ptr->next ) { char *data = build_page_initializer((regblock_t)ptr->data); fwrite_dump( data, LXDREAM_PAGE_SIZE, stdout ); g_free(data); } return 0; }
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); }
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 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); }