diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-24 02:16:26 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-24 02:16:26 +0200 |
commit | 295e9da1bac6598d91efeba10b0302fdee19ac0d (patch) | |
tree | 60971058a5eb403ae27ac1ba516bf1df90b481c3 /apps | |
parent | 0e44d3810ed599a12539cbcb5134588b03c83474 (diff) | |
download | px4-firmware-295e9da1bac6598d91efeba10b0302fdee19ac0d.tar.gz px4-firmware-295e9da1bac6598d91efeba10b0302fdee19ac0d.tar.bz2 px4-firmware-295e9da1bac6598d91efeba10b0302fdee19ac0d.zip |
Added required scalings, added gyro to MPU6000 test, changed sensors app to read from new drivers
Diffstat (limited to 'apps')
-rw-r--r-- | apps/drivers/drv_accel.h | 7 | ||||
-rw-r--r-- | apps/drivers/drv_gyro.h | 8 | ||||
-rw-r--r-- | apps/drivers/drv_mag.h | 8 | ||||
-rw-r--r-- | apps/drivers/mpu6000/mpu6000.cpp | 78 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 239 |
5 files changed, 236 insertions, 104 deletions
diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h index 370cc5d87..65ef8420f 100644 --- a/apps/drivers/drv_accel.h +++ b/apps/drivers/drv_accel.h @@ -50,10 +50,15 @@ * structure. */ struct accel_report { + uint64_t timestamp; float x; float y; float z; - uint64_t timestamp; + float range_m_s2; + float scaling; + uint16_t x_raw; + uint16_t y_raw; + uint16_t z_raw; }; /** accel scaling factors; Vout = (Vin * Vscale) + Voffset */ diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h index 82e23f62a..2f1dab6ba 100644 --- a/apps/drivers/drv_gyro.h +++ b/apps/drivers/drv_gyro.h @@ -50,10 +50,16 @@ * structure. */ struct gyro_report { + uint64_t timestamp; float x; float y; float z; - uint64_t timestamp; + float range_rad_s; + float scaling; + + int16_t x_raw; + int16_t y_raw; + int16_t z_raw; }; /** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */ diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index a59291778..f87cb7704 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -52,10 +52,16 @@ * Output values are in gauss. */ struct mag_report { + uint64_t timestamp; float x; float y; float z; - uint64_t timestamp; + float range_ga; + float scaling; + + uint16_t x_raw; + uint16_t y_raw; + uint16_t z_raw; }; /** mag scaling factors; Vout = (Vin * Vscale) + Voffset */ diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 999790e8a..b86e2da2f 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -364,12 +364,26 @@ MPU6000::init() write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz usleep(1000); - // FS & DLPF FS=2000¼/s, DLPF = 98Hz (low pass filter) + // FS & DLPF FS=2000 deg/s, DLPF = 98Hz (low pass filter) write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ); usleep(1000); - write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); // Gyro scale 2000¼/s + // Gyro scale 2000 deg/s () + write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); usleep(1000); + // correct gyro scale factors + // scale to rad/s in SI units + // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s + // scaling factor: + // 1/(2^15)*(2000/180)*PI + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); + _gyro_range_scale = 1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); + // product-specific scaling switch (_product) { case MPU6000ES_REV_C4: @@ -394,6 +408,16 @@ MPU6000::init() break; } + // Correct accel scale factors of 4096 LSB/g + // scale to m/s^2 ( 1g = 9.81 m/s^2) + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f / (4096.0f / 9.81f); + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f / (4096.0f / 9.81f); + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f / (4096.0f / 9.81f); + _accel_range_scale = 1.0f / (4096.0f / 9.81f); + usleep(1000); // INT CFG => Interrupt on Data Ready @@ -472,7 +496,7 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) if (_call_interval == 0) measure(); - /* copy out the latest report */ + /* copy out the latest reports */ memcpy(buffer, &_accel_report, sizeof(_accel_report)); ret = sizeof(_accel_report); @@ -746,10 +770,18 @@ MPU6000::measure() */ _gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time(); + _accel_report.x_raw = report.accel_x; + _accel_report.y_raw = report.accel_x; + _accel_report.z_raw = report.accel_x; + _accel_report.x = report.accel_x * _accel_range_scale; _accel_report.y = report.accel_y * _accel_range_scale; _accel_report.z = report.accel_z * _accel_range_scale; + _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_x * _gyro_range_scale; _gyro_report.y = report.gyro_y * _gyro_range_scale; _gyro_report.z = report.gyro_z * _gyro_range_scale; @@ -816,17 +848,26 @@ int test() { int fd = -1; + int fd_gyro = -1; struct accel_report report; + struct gyro_report g_report; ssize_t sz; const char *reason = "test OK"; do { - /* get the driver */ fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { - reason = "can't open driver, use <mpu6000 start> first"; + reason = "can't open accel driver, use <mpu6000 start> first"; + break; + } + + /* get the driver */ + fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + + if (fd_gyro < 0) { + reason = "can't open gyro driver, use <mpu6000 start> first"; break; } @@ -834,17 +875,32 @@ test() sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - reason = "immediate read failed"; + reason = "immediate acc read failed"; break; } printf("single read\n"); fflush(stdout); - printf("time: %lld\n", report.timestamp); - printf("x: %5.2f\n", (double)report.x); - printf("y: %5.2f\n", (double)report.y); - printf("z: %5.2f\n", (double)report.z); - printf("test: %8.4f\n", 1.5); + printf("time: %lld\n", report.timestamp); + printf("acc x: %5.2f m/s^2\n", (double)report.x); + printf("acc y: %5.2f m/s^2\n", (double)report.y); + printf("acc z: %5.2f m/s^2\n", (double)report.z); + printf("acc range: %4.0f m/s^2 (%3.0f g)\n", (double)report.range_m_s2, + (double)(report.range_m_s2 / 9.81f)); + + /* do a simple demand read */ + sz = read(fd, &g_report, sizeof(g_report)); + + if (sz != sizeof(g_report)) { + reason = "immediate gyro read failed"; + break; + } + + printf("gyro x: %5.2f rad/s\n", (double)g_report.x); + printf("gyro y: %5.2f rad/s\n", (double)g_report.y); + printf("gyro z: %5.2f rad/s\n", (double)g_report.z); + printf("gyro range: %3.0f rad/s (%5.0f deg/s)\n", (double)g_report.range_rad_s, + (double)(g_report.range_rad_s / M_PI_F * 180.0f)); } while (0); diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 37a9e8913..9249e971e 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -54,11 +54,14 @@ #include <float.h> #include <arch/board/up_hrt.h> -#include <arch/board/drv_lis331.h> #include <arch/board/drv_bma180.h> #include <arch/board/drv_l3gd20.h> -#include <arch/board/drv_hmc5883l.h> + #include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_mag.h> +#include <drivers/drv_baro.h> + #include <arch/board/up_adc.h> #include <systemlib/systemlib.h> @@ -407,28 +410,28 @@ static int sensors_init(void) printf("[sensors] Sensor configuration..\n"); /* open magnetometer */ - fd_magnetometer = open("/dev/hmc5883l", O_RDONLY); + fd_magnetometer = open("/dev/mag", O_RDONLY); if (fd_magnetometer < 0) { - fprintf(stderr, "[sensors] HMC5883L open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + fprintf(stderr, "[sensors] MAG open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); fflush(stderr); /* this sensor is critical, exit on failed init */ errno = ENOSYS; return ERROR; } else { - printf("[sensors] HMC5883L open ok\n"); + printf("[sensors] MAG open ok\n"); } /* open barometer */ - fd_barometer = open("/dev/ms5611", O_RDONLY); + fd_barometer = open("/dev/baro", O_RDONLY); if (fd_barometer < 0) { - fprintf(stderr, "[sensors] MS5611 open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + fprintf(stderr, "[sensors] BARO open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); fflush(stderr); } else { - printf("[sensors] MS5611 open ok\n"); + printf("[sensors] BARO open ok\n"); } /* open gyro */ @@ -444,7 +447,7 @@ static int sensors_init(void) int errno_accelerometer = (int)*get_errno_ptr(); if (!(fd_accelerometer < 0)) { - printf("[sensors] Accelerometer open ok\n"); + printf("[sensors] ACCEL open ok\n"); } /* only attempt to use BMA180 if MPU-6000 is not available */ @@ -454,7 +457,7 @@ static int sensors_init(void) errno_bma180 = (int)*get_errno_ptr(); if (!(fd_bma180 < 0)) { - printf("[sensors] Accelerometer (BMA180) open ok\n"); + printf("[sensors] ACCEL (BMA180) open ok\n"); } } else { fd_bma180 = -1; @@ -464,7 +467,7 @@ static int sensors_init(void) if (fd_accelerometer < 0 && fd_bma180 < 0) { /* print error message only if both failed, discard message else at all to not confuse users */ if (fd_accelerometer < 0) { - fprintf(stderr, "[sensors] MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer)); + fprintf(stderr, "[sensors] ACCEL: MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer)); fflush(stderr); /* this sensor is redundant with BMA180 */ } @@ -483,7 +486,7 @@ static int sensors_init(void) if (fd_accelerometer < 0 && fd_bma180 < 0) { /* print error message only if both failed, discard message else at all to not confuse users */ if (fd_accelerometer < 0) { - fprintf(stderr, "[sensors] MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer)); + fprintf(stderr, "[sensors] GYRO: MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer)); fflush(stderr); /* this sensor is redundant with BMA180 */ } @@ -620,11 +623,13 @@ int sensors_thread_main(int argc, char *argv[]) ssize_t ret_adc; int nsamples_adc; - int16_t buf_gyro[3]; + /* for PX4FMU 1.5 compatibility */ int16_t buf_accelerometer[3]; + + struct gyro_report buf_gyro; struct accel_report buf_accel_report; - int16_t buf_magnetometer[7]; - float buf_barometer[3]; + struct mag_report buf_magnetometer; + struct baro_report buf_barometer; bool mag_calibration_enabled = false; @@ -662,29 +667,28 @@ int sensors_thread_main(int argc, char *argv[]) /* Empty sensor buffers, avoid junk values */ /* Read first two values of each sensor into void */ - if (fd_gyro > 0)(void)read(fd_gyro, buf_gyro, sizeof(buf_gyro)); - if (fd_bma180 > 0)(void)read(fd_bma180, buf_accelerometer, 6); - if (fd_accelerometer > 0)(void)read(fd_accelerometer, buf_accelerometer, 12); - (void)read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer)); - - if (fd_barometer > 0)(void)read(fd_barometer, buf_barometer, sizeof(buf_barometer)); + if (fd_bma180 > 0)(void)read(fd_bma180, buf_accelerometer, sizeof(buf_accelerometer)); + if (fd_gyro > 0)(void)read(fd_gyro, &buf_gyro, sizeof(buf_gyro)); + if (fd_accelerometer > 0)(void)read(fd_accelerometer, &buf_accel_report, sizeof(buf_accel_report)); + (void)read(fd_magnetometer, &buf_magnetometer, sizeof(buf_magnetometer)); + if (fd_barometer > 0)(void)read(fd_barometer, &buf_barometer, sizeof(buf_barometer)); struct sensor_combined_s raw = { .timestamp = hrt_absolute_time(), - .gyro_raw = {buf_gyro[0], buf_gyro[1], buf_gyro[2]}, + .gyro_raw = {buf_gyro.x_raw, buf_gyro.y_raw, buf_gyro.z_raw}, .gyro_raw_counter = 0, - .gyro_rad_s = {0, 0, 0}, - .accelerometer_raw = {buf_accelerometer[0], buf_accelerometer[1], buf_accelerometer[2]}, + .gyro_rad_s = {buf_gyro.x, buf_gyro.y, buf_gyro.z}, + .accelerometer_raw = {buf_accel_report.x_raw, buf_accel_report.y_raw, buf_accel_report.z_raw}, .accelerometer_raw_counter = 0, - .accelerometer_m_s2 = {0, 0, 0}, - .magnetometer_raw = {buf_magnetometer[0], buf_magnetometer[1], buf_magnetometer[2]}, + .accelerometer_m_s2 = {buf_accel_report.x, buf_accel_report.y, buf_accel_report.z}, + .magnetometer_raw = {buf_magnetometer.x_raw, buf_magnetometer.y_raw, buf_magnetometer.z_raw}, .magnetometer_raw_counter = 0, - .baro_pres_mbar = 0, - .baro_alt_meter = 0, - .baro_temp_celcius = 0, - .battery_voltage_v = BAT_VOL_INITIAL, - .adc_voltage_v = {0, 0 , 0}, + .baro_pres_mbar = buf_barometer.pressure, + .baro_alt_meter = buf_barometer.altitude, + .baro_temp_celcius = buf_barometer.temperature, .baro_raw_counter = 0, + .battery_voltage_v = BAT_VOL_INITIAL, + .adc_voltage_v = {0, 0, 0}, .battery_voltage_counter = 0, .battery_voltage_valid = false, }; @@ -822,7 +826,7 @@ int sensors_thread_main(int argc, char *argv[]) if (fd_gyro > 0) { /* try reading gyro */ uint64_t start_gyro = hrt_absolute_time(); - ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro)); + ret_gyro = read(fd_gyro, &buf_gyro, sizeof(buf_gyro)); int gyrotime = hrt_absolute_time() - start_gyro; if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime); @@ -832,7 +836,7 @@ int sensors_thread_main(int argc, char *argv[]) gyro_fail_count++; if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { - fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + fprintf(stderr, "[sensors] GYRO ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) { @@ -859,6 +863,46 @@ int sensors_thread_main(int argc, char *argv[]) if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime); } + // if (fd_gyro_l3gd20 > 0) { + // /* try reading gyro */ + // uint64_t start_gyro = hrt_absolute_time(); + // ret_gyro = read(fd_gyro, buf_gyro_l3gd20, sizeof(buf_gyro_l3gd20)); + // int gyrotime = hrt_absolute_time() - start_gyro; + + // if (gyrotime > 500) printf("L3GD20 GYRO (pure read): %d us\n", gyrotime); + + // /* GYROSCOPE */ + // if (ret_gyro != sizeof(buf_gyro)) { + // gyro_fail_count++; + + // if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { + // fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + // } + + // if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) { + // // global_data_send_subsystem_info(&gyro_present_enabled); + // gyro_healthy = false; + // gyro_success_count = 0; + // } + + // } else { + // gyro_success_count++; + + // if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) { + // // global_data_send_subsystem_info(&gyro_present_enabled_healthy); + // gyro_healthy = true; + // gyro_fail_count = 0; + + // } + + // gyro_updated = true; + // } + + // gyrotime = hrt_absolute_time() - start_gyro; + + // if (gyrotime > 500) printf("L3GD20 GYRO (complete): %d us\n", gyrotime); + // } + /* read MPU-6000 */ if (fd_accelerometer > 0) { /* try reading acc */ @@ -869,7 +913,7 @@ int sensors_thread_main(int argc, char *argv[]) if (ret_accelerometer != sizeof(struct accel_report)) { acc_fail_count++; - if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) { + if ((acc_fail_count % 500) == 0 || (acc_fail_count > 20 && acc_fail_count < 40)) { fprintf(stderr, "[sensors] MPU-6000 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } @@ -908,7 +952,7 @@ int sensors_thread_main(int argc, char *argv[]) if (ret_accelerometer != 6) { acc_fail_count++; - if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) { + if ((acc_fail_count % 500) == 0 || (acc_fail_count > 20 && acc_fail_count < 40)) { fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } @@ -939,31 +983,31 @@ int sensors_thread_main(int argc, char *argv[]) /* MAGNETOMETER */ if (magcounter == 4) { /* 120 Hz */ uint64_t start_mag = hrt_absolute_time(); - /* start calibration mode if requested */ - if (!mag_calibration_enabled && vstatus.preflight_mag_calibration) { - ioctl(fd_magnetometer, HMC5883L_CALIBRATION_ON, 0); - printf("[sensors] enabling mag calibration mode\n"); - mag_calibration_enabled = true; - } else if (mag_calibration_enabled && !vstatus.preflight_mag_calibration) { - ioctl(fd_magnetometer, HMC5883L_CALIBRATION_OFF, 0); - printf("[sensors] disabling mag calibration mode\n"); - mag_calibration_enabled = false; - } - - ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer)); + // /* start calibration mode if requested */ + // if (!mag_calibration_enabled && vstatus.preflight_mag_calibration) { + // ioctl(fd_magnetometer, HMC5883L_CALIBRATION_ON, 0); + // printf("[sensors] enabling mag calibration mode\n"); + // mag_calibration_enabled = true; + // } else if (mag_calibration_enabled && !vstatus.preflight_mag_calibration) { + // ioctl(fd_magnetometer, HMC5883L_CALIBRATION_OFF, 0); + // printf("[sensors] disabling mag calibration mode\n"); + // mag_calibration_enabled = false; + // } + + ret_magnetometer = read(fd_magnetometer, &buf_magnetometer, sizeof(buf_magnetometer)); int errcode_mag = (int) * get_errno_ptr(); int magtime = hrt_absolute_time() - start_mag; if (magtime > 2000) { - printf("MAG (pure read): %d us\n", magtime); + printf("[sensors] WARN: MAG (pure read): %d us\n", magtime); } if (ret_magnetometer != sizeof(buf_magnetometer)) { mag_fail_count++; - if ((mag_fail_count % 20) == 0 || (mag_fail_count > 20 && mag_fail_count < 100)) { - fprintf(stderr, "[sensors] HMC5883L ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + if ((mag_fail_count % 200) == 0 || (mag_fail_count > 20 && mag_fail_count < 40)) { + fprintf(stderr, "[sensors] MAG ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } if (magn_healthy && mag_fail_count >= MAGN_HEALTH_COUNTER_LIMIT_ERROR) { @@ -987,8 +1031,8 @@ int sensors_thread_main(int argc, char *argv[]) magtime = hrt_absolute_time() - start_mag; if (magtime > 2000) { - printf("MAG (overall time): %d us\n", magtime); - fprintf(stderr, "[sensors] TIMEOUT HMC5883L ERROR #%d: %s\n", errcode_mag, strerror(errcode_mag)); + printf("[sensors] WARN: MAG (overall time): %d us\n", magtime); + fprintf(stderr, "[sensors] TIMEOUT MAG ERROR #%d: %s\n", errcode_mag, strerror(errcode_mag)); } magcounter = 0; @@ -1000,12 +1044,12 @@ int sensors_thread_main(int argc, char *argv[]) if (barocounter == 5 && (fd_barometer > 0)) { /* 100 Hz */ uint64_t start_baro = hrt_absolute_time(); *get_errno_ptr() = 0; - ret_barometer = read(fd_barometer, buf_barometer, sizeof(buf_barometer)); + ret_barometer = read(fd_barometer, &buf_barometer, sizeof(buf_barometer)); if (ret_barometer != sizeof(buf_barometer)) { baro_fail_count++; - if (((baro_fail_count % 20) == 0 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { + if (((baro_fail_count % 200) == 0 || (baro_fail_count > 20 && baro_fail_count < 40)) && (int)*get_errno_ptr() != EAGAIN) { fprintf(stderr, "[sensors] MS5611 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } @@ -1133,20 +1177,39 @@ int sensors_thread_main(int argc, char *argv[]) /* Copy values of gyro, acc, magnetometer & barometer */ /* GYROSCOPE */ - if (gyro_updated) { + // if (gyro_updated) { + // /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ + + // raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32768 : buf_gyro[1]); // x of the board is y of the sensor + // /* assign negated value, except for -SHORT_MAX, as it would wrap there */ + // raw.gyro_raw[1] = ((buf_gyro[0] == -32768) ? 32767 : -buf_gyro[0]); // y on the board is -x of the sensor + // raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32768 : buf_gyro[2]); // z of the board is z of the sensor + + // /* scale measurements */ + // // XXX request scaling from driver instead of hardcoding it + // /* scaling calculated as: raw * (1/(32768*(500/180*PI))) */ + // raw.gyro_rad_s[0] = (raw.gyro_raw[0] - rcp.gyro_offset[0]) * 0.000266316109f; + // raw.gyro_rad_s[1] = (raw.gyro_raw[1] - rcp.gyro_offset[1]) * 0.000266316109f; + // raw.gyro_rad_s[2] = (raw.gyro_raw[2] - rcp.gyro_offset[2]) * 0.000266316109f; + + // raw.gyro_raw_counter++; + // } + + /* MPU-6000 update */ + if (gyro_updated && fd_accelerometer > 0) { /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ - raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32768 : buf_gyro[1]); // x of the board is y of the sensor + raw.gyro_raw[0] = ((buf_gyro.y_raw == -32768) ? -32768 : buf_gyro.y_raw); // x of the board is y of the sensor /* assign negated value, except for -SHORT_MAX, as it would wrap there */ - raw.gyro_raw[1] = ((buf_gyro[0] == -32768) ? 32767 : -buf_gyro[0]); // y on the board is -x of the sensor - raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32768 : buf_gyro[2]); // z of the board is z of the sensor + raw.gyro_raw[1] = ((buf_gyro.x_raw == -32768) ? 32767 : -buf_gyro.x_raw); // y on the board is -x of the sensor + raw.gyro_raw[2] = ((buf_gyro.z_raw == -32768) ? -32768 : buf_gyro.z_raw); // z of the board is z of the sensor /* scale measurements */ // XXX request scaling from driver instead of hardcoding it /* scaling calculated as: raw * (1/(32768*(500/180*PI))) */ - raw.gyro_rad_s[0] = (raw.gyro_raw[0] - rcp.gyro_offset[0]) * 0.000266316109f; - raw.gyro_rad_s[1] = (raw.gyro_raw[1] - rcp.gyro_offset[1]) * 0.000266316109f; - raw.gyro_rad_s[2] = (raw.gyro_raw[2] - rcp.gyro_offset[2]) * 0.000266316109f; + raw.gyro_rad_s[0] = buf_gyro.y; + raw.gyro_rad_s[1] = buf_gyro.x; + raw.gyro_rad_s[2] = buf_gyro.z; raw.gyro_raw_counter++; } @@ -1159,14 +1222,13 @@ int sensors_thread_main(int argc, char *argv[]) /* MPU-6000 values */ /* scale from 14 bit to m/s2 */ - raw.accelerometer_m_s2[0] = buf_accel_report.x - rcp.acc_offset[0] / 1000.0f; - raw.accelerometer_m_s2[1] = buf_accel_report.y - rcp.acc_offset[1] / 1000.0f; - raw.accelerometer_m_s2[2] = buf_accel_report.z - rcp.acc_offset[2] / 1000.0f; + raw.accelerometer_m_s2[0] = buf_accel_report.x - rcp.acc_offset[0] * buf_accel_report.scaling; + raw.accelerometer_m_s2[1] = buf_accel_report.y - rcp.acc_offset[1] * buf_accel_report.scaling; + raw.accelerometer_m_s2[2] = buf_accel_report.z - rcp.acc_offset[2] * buf_accel_report.scaling; - /* assign negated value, except for -SHORT_MAX, as it would wrap there */ - raw.accelerometer_raw[0] = buf_accel_report.x * 1000; // x of the board is -y of the sensor - raw.accelerometer_raw[1] = buf_accel_report.y * 1000; // y on the board is x of the sensor - raw.accelerometer_raw[2] = buf_accel_report.z * 1000; // z of the board is z of the sensor + raw.accelerometer_raw[0] = buf_accel_report.x_raw; + raw.accelerometer_raw[1] = buf_accel_report.y_raw; + raw.accelerometer_raw[2] = buf_accel_report.z_raw; raw.accelerometer_raw_counter++; } else if (fd_bma180 > 0) { @@ -1187,19 +1249,16 @@ int sensors_thread_main(int argc, char *argv[]) raw.accelerometer_raw_counter++; } - /* L3GD20 is not available, use MPU-6000 */ - if (fd_accelerometer > 0 && fd_gyro < 0) { - raw.gyro_raw[0] = ((buf_accelerometer[3] == -32768) ? -32767 : buf_accelerometer[3]); // x of the board is y of the sensor - /* assign negated value, except for -SHORT_MAX, as it would wrap there */ - raw.gyro_raw[1] = ((buf_accelerometer[4] == -32768) ? 32767 : -buf_accelerometer[4]); // y on the board is -x of the sensor - raw.gyro_raw[2] = ((buf_accelerometer[5] == -32768) ? -32767 : buf_accelerometer[5]); // z of the board is -z of the sensor + /* Use MPU-6000 */ + if (fd_accelerometer > 0) { + raw.gyro_raw[0] = buf_gyro.x_raw; + raw.gyro_raw[1] = buf_gyro.y_raw; + raw.gyro_raw[2] = buf_gyro.z_raw; - /* scale measurements */ - // XXX request scaling from driver instead of hardcoding it - /* scaling calculated as: raw * (1/(32768*(500/180*PI))) */ - raw.gyro_rad_s[0] = (raw.gyro_raw[0] - rcp.gyro_offset[0]) * 0.000266316109f; - raw.gyro_rad_s[1] = (raw.gyro_raw[1] - rcp.gyro_offset[1]) * 0.000266316109f; - raw.gyro_rad_s[2] = (raw.gyro_raw[2] - rcp.gyro_offset[2]) * 0.000266316109f; + /* scaled measurements */ + raw.gyro_rad_s[0] = (buf_gyro.x - rcp.gyro_offset[0]) * buf_gyro.scaling; + raw.gyro_rad_s[1] = (buf_gyro.y - rcp.gyro_offset[1]) * buf_gyro.scaling; + raw.gyro_rad_s[2] = (buf_gyro.z - rcp.gyro_offset[2]) * buf_gyro.scaling; raw.gyro_raw_counter++; /* mark as updated */ @@ -1212,17 +1271,17 @@ int sensors_thread_main(int argc, char *argv[]) /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ /* assign negated value, except for -SHORT_MAX, as it would wrap there */ - raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : buf_magnetometer[1]; // x of the board is y of the sensor - raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? 32767 : -buf_magnetometer[0]; // y on the board is -x of the sensor - raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32768 : buf_magnetometer[2]; // z of the board is z of the sensor + raw.magnetometer_raw[0] = buf_magnetometer.x_raw; + raw.magnetometer_raw[1] = buf_magnetometer.y_raw; + raw.magnetometer_raw[2] = buf_magnetometer.z_raw; // XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here - raw.magnetometer_ga[0] = ((raw.magnetometer_raw[0] - rcp.mag_offset[0]) / 4096.0f) * 0.88f; - raw.magnetometer_ga[1] = ((raw.magnetometer_raw[1] - rcp.mag_offset[1]) / 4096.0f) * 0.88f; - raw.magnetometer_ga[2] = ((raw.magnetometer_raw[2] - rcp.mag_offset[2]) / 4096.0f) * 0.88f; + raw.magnetometer_ga[0] = buf_magnetometer.x - rcp.mag_offset[0] * buf_magnetometer.scaling; + raw.magnetometer_ga[1] = buf_magnetometer.y - rcp.mag_offset[1] * buf_magnetometer.scaling; + raw.magnetometer_ga[2] = buf_magnetometer.z - rcp.mag_offset[2] * buf_magnetometer.scaling; /* store mode */ - raw.magnetometer_mode = buf_magnetometer[3]; + raw.magnetometer_mode = 0; raw.magnetometer_raw_counter++; } @@ -1231,9 +1290,9 @@ int sensors_thread_main(int argc, char *argv[]) if (baro_updated) { /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ - raw.baro_pres_mbar = buf_barometer[0]; // Pressure in mbar - raw.baro_alt_meter = buf_barometer[1]; // Altitude in meters - raw.baro_temp_celcius = buf_barometer[2]; // Temperature in degrees celcius + raw.baro_pres_mbar = buf_barometer.pressure; // Pressure in mbar + raw.baro_alt_meter = buf_barometer.altitude; // Altitude in meters + raw.baro_temp_celcius = buf_barometer.temperature; // Temperature in degrees celcius raw.baro_raw_counter++; } |