diff options
-rw-r--r-- | apps/drivers/drv_accel.h | 6 | ||||
-rw-r--r-- | apps/drivers/drv_mag.h | 6 | ||||
-rw-r--r-- | apps/drivers/hmc5883/hmc5883.cpp | 19 | ||||
-rw-r--r-- | apps/drivers/mpu6000/mpu6000.cpp | 154 | ||||
-rw-r--r-- | apps/sensors/sensors.cpp | 4 |
5 files changed, 129 insertions, 60 deletions
diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h index 47d366bc4..977440518 100644 --- a/apps/drivers/drv_accel.h +++ b/apps/drivers/drv_accel.h @@ -57,9 +57,9 @@ struct accel_report { float z; float range_m_s2; float scaling; - uint16_t x_raw; - uint16_t y_raw; - uint16_t z_raw; + int16_t x_raw; + int16_t y_raw; + int16_t z_raw; }; /** accel scaling factors; Vout = (Vin * Vscale) + Voffset */ diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index 895d2d797..1f5bb998e 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -60,9 +60,9 @@ struct mag_report { float range_ga; float scaling; - uint16_t x_raw; - uint16_t y_raw; - uint16_t z_raw; + int16_t x_raw; + int16_t y_raw; + int16_t z_raw; }; /** mag scaling factors; Vout = (Vin * Vscale) + Voffset */ diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 3a194ee11..99deb5da8 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -649,9 +649,9 @@ HMC5883::collect() } /* swap the data we just received */ - report.x = ((int16_t)hmc_report.x[0] << 8) + hmc_report.x[1]; - report.y = ((int16_t)hmc_report.y[0] << 8) + hmc_report.y[1]; - report.z = ((int16_t)hmc_report.z[0] << 8) + hmc_report.z[1]; + report.x = (((int16_t)hmc_report.x[0]) << 8) + hmc_report.x[1]; + report.y = (((int16_t)hmc_report.y[0]) << 8) + hmc_report.y[1]; + report.z = (((int16_t)hmc_report.z[0]) << 8) + hmc_report.z[1]; /* * If any of the values are -4096, there was an internal math error in the sensor. @@ -662,10 +662,14 @@ HMC5883::collect() (abs(report.z) > 2048)) goto out; - /* raw outputs */ - /* to align the sensor axes with the board, x and y need to be flipped */ + /* + * RAW outputs + * + * to align the sensor axes with the board, x and y need to be flipped + * and y needs to be negated + */ _reports[_next_report].x_raw = report.y; - _reports[_next_report].y_raw = report.x; + _reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x); /* z remains z */ _reports[_next_report].z_raw = report.z; @@ -688,7 +692,8 @@ HMC5883::collect() /* to align the sensor axes with the board, x and y need to be flipped */ _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* flip axes and negate value for y */ + _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 5a801a9b7..cb526d52a 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -386,7 +386,7 @@ MPU6000::init() _gyro_scale.y_scale = 1.0f; _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; - _gyro_range_scale = 1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); + _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; // product-specific scaling @@ -421,7 +421,7 @@ MPU6000::init() _accel_scale.y_scale = 1.0f; _accel_scale.z_offset = 0; _accel_scale.z_scale = 1.0f; - _accel_range_scale = 1.0f / (4096.0f / 9.81f); + _accel_range_scale = (9.81f / 4096.0f); _accel_range_m_s2 = 8.0f * 9.81f; usleep(1000); @@ -595,6 +595,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSRANGE: case ACCELIOCGRANGE: /* XXX not implemented */ + // XXX change these two values on set: + // _accel_range_scale = (9.81f / 4096.0f); + // _accel_range_rad_s = 8.0f * 9.81f; return -EINVAL; default: @@ -634,6 +637,9 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSRANGE: case GYROIOCGRANGE: /* XXX not implemented */ + // XXX change these two values on set: + // _gyro_range_scale = xx + // _gyro_range_m_s2 = xx return -EINVAL; default: @@ -755,6 +761,20 @@ MPU6000::measure_trampoline(void *arg) dev->measure(); } +int16_t +int16_t_from_bytes(uint8_t bytes[]) +{ + union { + uint8_t b[2]; + int16_t w; + } u; + + u.b[1] = bytes[0]; + u.b[0] = bytes[1]; + + return u.w; +} + void MPU6000::measure() { @@ -763,40 +783,80 @@ MPU6000::measure() * Report conversation within the MPU6000, including command byte and * interrupt status. */ - struct Report { + struct MPUReport { uint8_t cmd; uint8_t status; - uint16_t accel_x; - uint16_t accel_y; - uint16_t accel_z; - uint16_t temp; - uint16_t gyro_x; - uint16_t gyro_y; - uint16_t gyro_z; - } report; + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + } mpu_report; #pragma pack(pop) + 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 MPU6000 in one pass. */ - report.cmd = DIR_READ | MPUREG_INT_STATUS; - transfer((uint8_t *)&report, (uint8_t *)&report, sizeof(report)); + mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)); /* - * Adjust and scale results to mg. + * Convert from big to little endian */ - _gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time(); + // report.accel_x = ((int16_t)(mpu_report.accel_x[0]) << 8); + // report.accel_x |= mpu_report.accel_x[1]; + // report.accel_y = ((int16_t)(mpu_report.accel_y[0]) << 8); + // report.accel_y |= mpu_report.accel_y[1]; + // report.accel_z = ((int16_t)(mpu_report.accel_z[0]) << 8); + // report.accel_z |= mpu_report.accel_z[1]; - /* to align the sensor axes with the board, x and y need to be flipped */ - _accel_report.x_raw = report.accel_y; - _accel_report.y_raw = report.accel_x; - /* z remains z and has the right sign */ - _accel_report.z_raw = report.accel_z; + 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)mpu_report.temp[0]) << 8) | mpu_report.temp[1]; + + report.gyro_x = ((int16_t)(mpu_report.gyro_x[0]) << 8) | mpu_report.gyro_x[1]; + report.gyro_y = ((int16_t)(mpu_report.gyro_y[0]) << 8) | mpu_report.gyro_y[1]; + report.gyro_z = ((int16_t)(mpu_report.gyro_z[0]) << 8) | mpu_report.gyro_z[1]; + + /* + * 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; + + /* + * Adjust and scale results to m/s^2. + */ + _gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time(); - /* to align the sensor axes with the board, x and y need to be flipped */ /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -812,22 +872,26 @@ MPU6000::measure() * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ - _accel_report.x = ((report.accel_y * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - _accel_report.y = ((report.accel_x * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - /* z remains z and has the right sign */ + + + /* NOTE: Axes have been swapped to match the board a few lines above. */ + + _accel_report.x_raw = report.accel_x; + _accel_report.y_raw = report.accel_y; + _accel_report.z_raw = report.accel_z; + + _accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + _accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; _accel_report.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; _accel_report.scaling = _accel_range_scale; _accel_report.range_m_s2 = _accel_range_m_s2; - /* to align the sensor axes with the board, x and y need to be flipped */ - _gyro_report.x_raw = report.gyro_y; - _gyro_report.y_raw = report.gyro_x; - /* z remains z and has the right sign */ + _gyro_report.x_raw = report.gyro_x; + _gyro_report.y_raw = report.gyro_y; _gyro_report.z_raw = report.gyro_z; - _gyro_report.x = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - _gyro_report.y = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - /* z remains z and has the right sign */ + _gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + _gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; _gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; _gyro_report.scaling = _gyro_range_scale; _gyro_report.range_rad_s = _gyro_range_rad_s; @@ -963,12 +1027,12 @@ test() warnx("single read"); warnx("time: %lld", a_report.timestamp); - warnx("acc x: \t% 8.4f\tm/s^2", (double)a_report.x); - warnx("acc y: \t% 8.4f\tm/s^2", (double)a_report.y); - warnx("acc z: \t% 8.4f\tm/s^2", (double)a_report.z); - warnx("acc x: \t%d\traw", a_report.x_raw); - warnx("acc y: \t%d\traw", a_report.y_raw); - warnx("acc z: \t%d\traw", a_report.z_raw); + warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x); + warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y); + warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z); + warnx("acc x: \t%d\traw", (int)a_report.x_raw); + warnx("acc y: \t%d\traw", (int)a_report.y_raw); + warnx("acc z: \t%d\traw", (int)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, (double)(a_report.range_m_s2 / 9.81f)); @@ -977,14 +1041,14 @@ test() if (sz != sizeof(g_report)) err(1, "immediate gyro read failed"); - warnx("gyro x: \t% 8.4f\trad/s", (double)g_report.x); - warnx("gyro y: \t% 8.4f\trad/s", (double)g_report.y); - warnx("gyro z: \t% 8.4f\trad/s", (double)g_report.z); - warnx("gyro x: \t%d\traw", g_report.x_raw); - warnx("gyro y: \t%d\traw", g_report.y_raw); - warnx("gyro z: \t%d\traw", g_report.z_raw); - warnx("gyro range: %8.4f rad/s (%8.2f deg/s)", (double)g_report.range_rad_s, - (double)((g_report.range_rad_s / M_PI_F) * 180.0f)); + warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); + warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); + warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); + warnx("gyro x: \t%d\traw", (int)g_report.x_raw); + warnx("gyro y: \t%d\traw", (int)g_report.y_raw); + warnx("gyro z: \t%d\traw", (int)g_report.z_raw); + warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, + (int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f)); /* XXX add poll-rate tests here too */ diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index f75834ddf..f8baa285b 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -641,9 +641,9 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) gyro_report.timestamp = hrt_absolute_time(); - gyro_report.x_raw = ((buf[1] == -32768) ? -32768 : buf[1]); + gyro_report.x_raw = buf[1]; gyro_report.y_raw = ((buf[0] == -32768) ? 32767 : -buf[0]); - gyro_report.z_raw = ((buf[2] == -32768) ? -32768 : buf[2]); + gyro_report.z_raw = buf[2]; /* scaling calculated as: raw * (1/(32768*(500/180*PI))) */ gyro_report.x = (gyro_report.x_raw - _parameters.gyro_offset[0]) * 0.000266316109f; |