diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 7576fc1d96..1123ddf3ab 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -288,9 +288,9 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), - _accel_range_scale(1.0f), + _accel_range_scale(0.02f), _accel_topic(-1), - _gyro_range_scale(1.0f), + _gyro_range_scale(0.02f), _gyro_topic(-1), _reads(0), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")) @@ -826,7 +826,7 @@ test() fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { - reason = "can't open driver"; + reason = "can't open driver, use first"; break; } @@ -841,9 +841,10 @@ test() printf("single read\n"); fflush(stdout); printf("time: %lld\n", report.timestamp); - printf("x: %f\n", report.x); - printf("y: %f\n", report.y); - printf("z: %f\n", report.z); + 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); } while (0); diff --git a/apps/sensors/Makefile b/apps/sensors/Makefile index dadc2993d1..2692ade645 100644 --- a/apps/sensors/Makefile +++ b/apps/sensors/Makefile @@ -37,6 +37,6 @@ APPNAME = sensors PRIORITY = SCHED_PRIORITY_MAX-5 -STACKSIZE = 2048 +STACKSIZE = 3096 include $(APPDIR)/mk/app.mk diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index d847c4ffc1..6caeb06e5b 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -57,6 +57,7 @@ #include #include #include +#include #include #include @@ -105,11 +106,12 @@ static pthread_mutex_t sensors_read_ready_mutex; static int sensors_timer_loop_counter = 0; /* File descriptors for all sensors */ -static int fd_gyro = -1; -static int fd_accelerometer = -1; -static int fd_magnetometer = -1; -static int fd_barometer = -1; -static int fd_adc = -1; +static int fd_gyro = -1; +static int fd_bma180 = -1; +static int fd_magnetometer = -1; +static int fd_barometer = -1; +static int fd_adc = -1; +static int fd_accelerometer = -1; /* Private functions declared static */ static void sensors_timer_loop(void *arg); @@ -169,30 +171,70 @@ static int sensors_init(void) /* open gyro */ fd_gyro = open("/dev/l3gd20", O_RDONLY); + int errno_gyro = (int)*get_errno_ptr(); - if (fd_gyro < 0) { - fprintf(stderr, "[sensors] L3GD20 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 { + if (!(fd_gyro < 0)) { printf("[sensors] L3GD20 open ok\n"); } - /* open accelerometer */ - fd_accelerometer = open("/dev/bma180", O_RDONLY); + /* open accelerometer, prefer the MPU-6000 */ + fd_accelerometer = open("/dev/accel", O_RDONLY); + int errno_accelerometer = (int)*get_errno_ptr(); + + if (!(fd_accelerometer < 0)) { + printf("[sensors] Accelerometer open ok\n"); + } + + /* only attempt to use BMA180 if MPU-6000 is not available */ + int errno_bma180 = 0; if (fd_accelerometer < 0) { - fprintf(stderr, "[sensors] BMA180: 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 */ + fd_bma180 = open("/dev/bma180", O_RDONLY); + errno_bma180 = (int)*get_errno_ptr(); + + if (!(fd_bma180 < 0)) { + printf("[sensors] Accelerometer (BMA180) open ok\n"); + } + } else { + fd_bma180 = -1; + } + + /* fail if no accelerometer is available */ + 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)); + fflush(stderr); + /* this sensor is redundant with BMA180 */ + } + + if (fd_bma180 < 0) { + fprintf(stderr, "[sensors] BMA180: open fail (err #%d): %s\n", errno_bma180, strerror(errno_bma180)); + fflush(stderr); + /* this sensor is redundant with MPU-6000 */ + } + errno = ENOSYS; return ERROR; + } - } else { - printf("[sensors] BMA180 open ok\n"); + /* fail if no gyro is available */ + 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)); + fflush(stderr); + /* this sensor is redundant with BMA180 */ + } + + if (fd_gyro < 0) { + fprintf(stderr, "[sensors] L3GD20 open fail (err #%d): %s\n", errno_gyro, strerror(errno_gyro)); + fflush(stderr); + /* this sensor is critical, exit on failed init */ + } + + errno = ENOSYS; + return ERROR; } /* open adc */ @@ -209,16 +251,18 @@ static int sensors_init(void) printf("[sensors] ADC open ok\n"); } - /* configure gyro */ - if (ioctl(fd_gyro, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) { - fprintf(stderr, "[sensors] L3GD20 configuration (ioctl) 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] L3GD20 configuration ok\n"); + /* configure gyro - if its not available and we got here the MPU-6000 is for sure available */ + if (fd_gyro > 0) { + if (ioctl(fd_gyro, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) { + fprintf(stderr, "[sensors] L3GD20 configuration (ioctl) 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] L3GD20 configuration ok\n"); + } } /* XXX Add IOCTL configuration of remaining sensors */ @@ -255,7 +299,7 @@ int sensors_main(int argc, char *argv[]) fprintf(stderr, "[sensors] ERROR: Failed to initialize all sensors\n"); /* Clean up */ close(fd_gyro); - close(fd_accelerometer); + close(fd_bma180); close(fd_magnetometer); close(fd_barometer); close(fd_adc); @@ -311,6 +355,7 @@ int sensors_main(int argc, char *argv[]) int16_t buf_gyro[3]; int16_t buf_accelerometer[3]; + struct accel_report buf_accel_report; int16_t buf_magnetometer[7]; float buf_barometer[3]; @@ -354,8 +399,9 @@ int sensors_main(int argc, char *argv[]) /* Empty sensor buffers, avoid junk values */ /* Read first two values of each sensor into void */ - (void)read(fd_gyro, buf_gyro, sizeof(buf_gyro)); - (void)read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer)); + 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)); @@ -455,8 +501,12 @@ int sensors_main(int argc, char *argv[]) if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) { hil_enabled = true; publishing = false; - int ret = close(sensor_pub); - printf("[sensors] Closing sensor pub: %i \n", ret); + int sens_ret = close(sensor_pub); + if (sens_ret == OK) { + printf("[sensors] Closing sensor pub OK\n"); + } else { + printf("[sensors] FAILED Closing sensor pub, result: %i \n", sens_ret); + } /* switching from HIL to non-HIL mode */ @@ -508,79 +558,121 @@ int sensors_main(int argc, char *argv[]) paramcounter++; - /* try reading gyro */ - uint64_t start_gyro = hrt_absolute_time(); - ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro)); - int gyrotime = hrt_absolute_time() - start_gyro; + if (fd_gyro > 0) { + /* try reading gyro */ + uint64_t start_gyro = hrt_absolute_time(); + 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); + if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime); - /* GYROSCOPE */ - if (ret_gyro != sizeof(buf_gyro)) { - gyro_fail_count++; + /* 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_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; - } + 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++; + } 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; + 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; } - gyro_updated = true; - } + gyrotime = hrt_absolute_time() - start_gyro; - gyrotime = hrt_absolute_time() - start_gyro; + if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime); + } - if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime); + /* read MPU-6000 */ + if (fd_accelerometer > 0) { + /* try reading acc */ + uint64_t start_acc = hrt_absolute_time(); + ret_accelerometer = read(fd_accelerometer, &buf_accel_report, sizeof(struct accel_report)); - /* try reading acc */ - uint64_t start_acc = hrt_absolute_time(); - ret_accelerometer = read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer)); + /* ACCELEROMETER */ + if (ret_accelerometer != sizeof(struct accel_report)) { + acc_fail_count++; - /* ACCELEROMETER */ - if (ret_accelerometer != sizeof(buf_accelerometer)) { - acc_fail_count++; + if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) { + fprintf(stderr, "[sensors] MPU-6000 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + } - if (acc_fail_count & 0b1000 || (acc_fail_count > 20 && acc_fail_count < 100)) { - fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); - } + if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) { + // global_data_send_subsystem_info(&acc_present_enabled); + gyro_healthy = false; + acc_success_count = 0; + } - if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) { - // global_data_send_subsystem_info(&acc_present_enabled); - gyro_healthy = false; - acc_success_count = 0; - } + } else { + acc_success_count++; - } else { - acc_success_count++; + if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) { - if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) { + // global_data_send_subsystem_info(&acc_present_enabled_healthy); + acc_healthy = true; + acc_fail_count = 0; - // global_data_send_subsystem_info(&acc_present_enabled_healthy); - acc_healthy = true; - acc_fail_count = 0; + } + acc_updated = true; } - acc_updated = true; + int acctime = hrt_absolute_time() - start_acc; + if (acctime > 500) printf("ACC: %d us\n", acctime); } - int acctime = hrt_absolute_time() - start_acc; + /* read BMA180. If the MPU-6000 is present, the BMA180 file descriptor won't be open */ + if (fd_bma180 > 0) { + /* try reading acc */ + uint64_t start_acc = hrt_absolute_time(); + ret_accelerometer = read(fd_bma180, buf_accelerometer, 6); + + /* ACCELEROMETER */ + if (ret_accelerometer != 6) { + acc_fail_count++; + + if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) { + fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + } + + if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) { + // global_data_send_subsystem_info(&acc_present_enabled); + gyro_healthy = false; + acc_success_count = 0; + } + + } else { + acc_success_count++; + + if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) { + + // global_data_send_subsystem_info(&acc_present_enabled_healthy); + acc_healthy = true; + acc_fail_count = 0; + + } - if (acctime > 500) printf("ACC: %d us\n", acctime); + acc_updated = true; + } + + int acctime = hrt_absolute_time() - start_acc; + if (acctime > 500) printf("ACC: %d us\n", acctime); + } /* MAGNETOMETER */ if (magcounter == 4) { /* 120 Hz */ @@ -607,7 +699,7 @@ int sensors_main(int argc, char *argv[]) if (ret_magnetometer != sizeof(buf_magnetometer)) { mag_fail_count++; - if (mag_fail_count & 0b1000 || (mag_fail_count > 20 && mag_fail_count < 100)) { + 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())); } @@ -650,7 +742,7 @@ int sensors_main(int argc, char *argv[]) if (ret_barometer != sizeof(buf_barometer)) { baro_fail_count++; - if ((baro_fail_count & 0b1000 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { + if (((baro_fail_count % 20) == 0 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { fprintf(stderr, "[sensors] MS5611 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } @@ -687,10 +779,10 @@ int sensors_main(int argc, char *argv[]) ret_adc = read(fd_adc, &buf_adc, adc_readsize); nsamples_adc = ret_adc / sizeof(struct adc_msg_s); - if (ret_adc < 0 || nsamples_adc * sizeof(struct adc_msg_s) != ret_adc) { + if (ret_adc < 0 || ((int)(nsamples_adc * sizeof(struct adc_msg_s))) != ret_adc) { adc_fail_count++; - if ((adc_fail_count & 0b1000 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) { + if (((adc_fail_count % 20) == 0 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) { fprintf(stderr, "[sensors] ADC ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } @@ -730,7 +822,7 @@ int sensors_main(int argc, char *argv[]) */ if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) { /* Read out values from HRT */ - for (int i = 0; i < ppm_decoded_channels; i++) { + for (unsigned int i = 0; i < ppm_decoded_channels; i++) { rc.chan[i].raw = ppm_buffer[i]; /* Set the range to +-, then scale up */ rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor * 10000; @@ -759,9 +851,9 @@ int sensors_main(int argc, char *argv[]) if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; /* throttle input */ - manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled; + manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled/2.0f; if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - if (manual_control.throttle > 2.0f) manual_control.throttle = 2.0f; + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; /* mode switch input */ manual_control.override_mode_switch = rc.chan[rc.function[OVERRIDE]].scaled; @@ -800,19 +892,56 @@ int sensors_main(int argc, char *argv[]) if (acc_updated) { /* 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.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor - raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor - raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor - - // XXX read range from sensor - float range_g = 4.0f; - /* scale from 14 bit to m/s2 */ - raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f; - raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f; - raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f; - - raw.accelerometer_raw_counter++; + if (fd_accelerometer > 0) { + /* MPU-6000 values */ + + /* scale from 14 bit to m/s2 */ + raw.accelerometer_m_s2[0] = buf_accel_report.x; + raw.accelerometer_m_s2[1] = buf_accel_report.y; + raw.accelerometer_m_s2[2] = buf_accel_report.z; + + /* 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_counter++; + } else if (fd_bma180 > 0) { + + /* assign negated value, except for -SHORT_MAX, as it would wrap there */ + raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor + raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor + raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor + + + // XXX read range from sensor + float range_g = 4.0f; + /* scale from 14 bit to m/s2 */ + raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f; + raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f; + raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f; + + 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 + + /* 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] - gyro_offset[0]) * 0.000266316109f; + raw.gyro_rad_s[1] = (raw.gyro_raw[1] - gyro_offset[1]) * 0.000266316109f; + raw.gyro_rad_s[2] = (raw.gyro_raw[2] - gyro_offset[2]) * 0.000266316109f; + + raw.gyro_raw_counter++; + /* mark as updated */ + gyro_updated = true; + } } /* MAGNETOMETER */ @@ -912,7 +1041,7 @@ int sensors_main(int argc, char *argv[]) printf("[sensors] sensor readout stopped\n"); close(fd_gyro); - close(fd_accelerometer); + close(fd_bma180); close(fd_magnetometer); close(fd_barometer); close(fd_adc); diff --git a/nuttx/configs/px4fmu/src/up_nsh.c b/nuttx/configs/px4fmu/src/up_nsh.c index 6e092e3b41..e7b9ce70a4 100644 --- a/nuttx/configs/px4fmu/src/up_nsh.c +++ b/nuttx/configs/px4fmu/src/up_nsh.c @@ -236,19 +236,6 @@ int nsh_archinitialize(void) if (acc_fail) message("[boot] FAILED to attach BMA180 accelerometer\r\n"); - int mpu_attempts = 0; - int mpu_fail = 0; - - while (mpu_attempts < 1) - { - mpu_fail = mpu6000_attach(spi1, PX4_SPIDEV_MPU); - mpu_attempts++; - if (mpu_fail == 0) break; - up_udelay(200); - } - - if (mpu_fail) message("[boot] FAILED to attach MPU 6000 gyro/acc\r\n"); - /* initialize I2C2 bus */ i2c2 = up_i2cinitialize(2);