diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-24 14:58:14 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-24 14:58:14 +0200 |
commit | d12c09cc869c09de4cf670e58f900e42f7fc5f0f (patch) | |
tree | be4f2bc7c8943abd76bb19a12669d0b1c773bb84 /apps | |
parent | 45e178eaa3ba620dfc8364aa73a1deeb9b609a2b (diff) | |
download | px4-firmware-d12c09cc869c09de4cf670e58f900e42f7fc5f0f.tar.gz px4-firmware-d12c09cc869c09de4cf670e58f900e42f7fc5f0f.tar.bz2 px4-firmware-d12c09cc869c09de4cf670e58f900e42f7fc5f0f.zip |
improvements / debugging on I2C drivers
Diffstat (limited to 'apps')
-rw-r--r-- | apps/drivers/device/i2c.cpp | 7 | ||||
-rw-r--r-- | apps/drivers/hmc5883/hmc5883.cpp | 2 | ||||
-rw-r--r-- | apps/drivers/mpu6000/Makefile | 2 | ||||
-rw-r--r-- | apps/drivers/mpu6000/mpu6000.cpp | 30 | ||||
-rw-r--r-- | apps/drivers/ms5611/ms5611.cpp | 2 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 25 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 38 | ||||
-rw-r--r-- | apps/system/i2c/i2c_bus.c | 2 | ||||
-rw-r--r-- | apps/systemcmds/eeprom/eeprom.c | 2 |
9 files changed, 68 insertions, 42 deletions
diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index e403ce42b..42dfd7300 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -75,6 +75,9 @@ I2C::init() // attach to the i2c bus _dev = up_i2cinitialize(_bus); + // set the bus speed again to a reasonable number of 400 KHz + I2C_SETFREQUENCY(_dev, 400000); + if (_dev == nullptr) { debug("failed to init I2C"); @@ -118,7 +121,7 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len struct i2c_msg_s msgv[2]; unsigned msgs; int ret; - unsigned tries; + unsigned tries = 0; do { // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); @@ -144,6 +147,8 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len if (msgs == 0) return -EINVAL; + // set the bus speed again to a reasonable number of 400 KHz + I2C_SETFREQUENCY(_dev, 400000); ret = I2C_TRANSFER(_dev, &msgv[0], msgs); if (ret == OK) diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 0fa1f365d..f29e7dcb0 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -807,7 +807,9 @@ test(int fd) /* do a simple demand read */ + uint64_t start_mag = hrt_absolute_time(); sz = read(fd, &report, sizeof(report)); + test_note("time for read: %lld (should be below 2000!)", hrt_absolute_time() - start_mag); if (sz != sizeof(report)) return test_fail("immediate read failed: %d", errno); diff --git a/apps/drivers/mpu6000/Makefile b/apps/drivers/mpu6000/Makefile index e03eef67d..32df1bdae 100644 --- a/apps/drivers/mpu6000/Makefile +++ b/apps/drivers/mpu6000/Makefile @@ -37,6 +37,6 @@ APPNAME = mpu6000 PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 +STACKSIZE = 4096 include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index e49a2ed9f..017a4dd9e 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -181,11 +181,13 @@ private: struct accel_report _accel_report; struct accel_scale _accel_scale; float _accel_range_scale; + float _accel_range_m_s2; orb_advert_t _accel_topic; struct gyro_report _gyro_report; struct gyro_scale _gyro_scale; float _gyro_range_scale; + float _gyro_range_rad_s; orb_advert_t _gyro_topic; unsigned _reads; @@ -289,8 +291,10 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _product(0), _call_interval(0), _accel_range_scale(0.02f), + _accel_range_m_s2(0.0f), _accel_topic(-1), _gyro_range_scale(0.02f), + _gyro_range_rad_s(0.0f), _gyro_topic(-1), _reads(0), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")) @@ -383,6 +387,7 @@ MPU6000::init() _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); + _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; // product-specific scaling switch (_product) { @@ -417,6 +422,7 @@ MPU6000::init() _accel_scale.z_offset = 0; _accel_scale.z_scale = 1.0f / (4096.0f / 9.81f); _accel_range_scale = 1.0f / (4096.0f / 9.81f); + _accel_range_m_s2 = 8.0f * 9.81f; usleep(1000); @@ -779,6 +785,8 @@ MPU6000::measure() _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; + _accel_report.scaling = _accel_range_scale; + _accel_report.range_m_s2 = _accel_range_m_s2; _gyro_report.x_raw = report.gyro_x; _gyro_report.y_raw = report.gyro_y; @@ -787,6 +795,8 @@ MPU6000::measure() _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; + _gyro_report.scaling = _gyro_range_scale; + _gyro_report.range_rad_s = _gyro_range_rad_s; /* notify anyone waiting for data */ poll_notify(POLLIN); @@ -884,25 +894,25 @@ test() printf("single read\n"); fflush(stdout); 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, + printf("acc x: \t% 5.2f\tm/s^2\n", (double)report.x); + printf("acc y: \t% 5.2f\tm/s^2\n", (double)report.y); + printf("acc z: \t% 5.2f\tm/s^2\n", (double)report.z); + printf("acc range: %6.2f m/s^2 (%6.2f 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)); + sz = read(fd_gyro, &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)); + printf("gyro x: \t% 5.2f\trad/s\n", (double)g_report.x); + printf("gyro y: \t% 5.2f\trad/s\n", (double)g_report.y); + printf("gyro z: \t% 5.2f\trad/s\n", (double)g_report.z); + printf("gyro range: %6.3f rad/s (%8.1f 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/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 8f8f96217..872cf9fdb 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -833,7 +833,9 @@ test(int fd) /* do a simple demand read */ + uint64_t start_baro = hrt_absolute_time(); sz = read(fd, &report, sizeof(report)); + test_note("time for read: %lld (should be below 2000!)", hrt_absolute_time() - start_baro); if (sz != sizeof(report)) return test_fail("immediate read failed: %d", errno); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 4f3a8295d..aa7351153 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -94,7 +94,6 @@ static int mavlink_task; /* terminate MAVLink on user request - disabled by default */ static bool mavlink_link_termination_allowed = false; -static bool mavlink_exit_requested = false; mavlink_system_t mavlink_system = {100, 50, MAV_TYPE_FIXED_WING, 0, 0, 0}; // System ID, 1-255, Component/Subsystem ID, 1-255 static uint8_t chan = MAVLINK_COMM_0; @@ -459,8 +458,6 @@ static void *receiveloop(void *arg) while (!thread_should_exit) { - if (mavlink_exit_requested) break; - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; if (poll(fds, 1, timeout) > 0) { @@ -704,7 +701,6 @@ static void *uorb_receiveloop(void *arg) const int timeout = 1000; while (!thread_should_exit) { - if (mavlink_exit_requested) break; int poll_ret = poll(fds, fdsc_count, timeout); @@ -1090,12 +1086,8 @@ void handleMessage(mavlink_message_t *msg) fflush(stdout); usleep(50000); - /* terminate other threads */ - mavlink_exit_requested = true; - pthread_cancel(receive_thread); - pthread_cancel(uorb_receive_thread); - - pthread_exit(NULL); + /* terminate other threads and this thread */ + thread_should_exit = true; } else { @@ -1425,7 +1417,7 @@ int mavlink_thread_main(int argc, char *argv[]) if (uart < 0) { printf("[mavlink] FAILED to open %s, terminating.\n", uart_name); - return ERROR; + goto exit_cleanup; } /* Flush UART */ @@ -1511,9 +1503,9 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } - while (!thread_should_exit) { + thread_running = true; - if (mavlink_exit_requested) break; + while (!thread_should_exit) { /* get local and global position */ orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); @@ -1606,9 +1598,15 @@ int mavlink_thread_main(int argc, char *argv[]) printf("[mavlink] Restored original UART config, exiting..\n"); } +exit_cleanup: + /* close uart */ close(uart); + /* close subscriptions */ + close(mavlink_subs.global_pos_sub); + close(local_pos_sub); + fflush(stdout); fflush(stderr); @@ -1641,7 +1639,6 @@ int mavlink_main(int argc, char *argv[]) thread_should_exit = false; mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 4400, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; exit(0); } diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 9249e971e..a994e618c 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -113,6 +113,7 @@ static int sensors_timer_loop_counter = 0; /* File descriptors for all sensors */ static int fd_gyro = -1; +static int fd_gyro_l3gd20 = -1; static bool thread_should_exit = false; static bool thread_running = false; @@ -435,13 +436,21 @@ static int sensors_init(void) } /* open gyro */ - fd_gyro = open("/dev/l3gd20", O_RDONLY); + fd_gyro_l3gd20 = open("/dev/l3gd20", O_RDONLY); int errno_gyro = (int)*get_errno_ptr(); - if (!(fd_gyro < 0)) { + if (!(fd_gyro_l3gd20 < 0)) { printf("[sensors] L3GD20 open ok\n"); } + /* open gyro */ + fd_gyro = open("/dev/gyro", O_RDONLY); + errno_gyro = (int)*get_errno_ptr(); + + if (!(fd_gyro < 0)) { + printf("[sensors] GYRO open ok\n"); + } + /* open accelerometer, prefer the MPU-6000 */ fd_accelerometer = open("/dev/accel", O_RDONLY); int errno_accelerometer = (int)*get_errno_ptr(); @@ -516,8 +525,8 @@ static int sensors_init(void) } /* 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)) { + if (fd_gyro_l3gd20 > 0) { + if (ioctl(fd_gyro_l3gd20 , L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro_l3gd20 , 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 */ @@ -568,7 +577,7 @@ int sensors_thread_main(int argc, char *argv[]) close(fd_barometer); close(fd_adc); - fprintf(stderr, "[sensors] rebooting system.\n"); + fprintf(stderr, "[sensors] REBOOTING SYSTEM\n\n"); fflush(stderr); fflush(stdout); usleep(100000); @@ -631,7 +640,7 @@ int sensors_thread_main(int argc, char *argv[]) struct mag_report buf_magnetometer; struct baro_report buf_barometer; - bool mag_calibration_enabled = false; + // bool mag_calibration_enabled = false; #pragma pack(push,1) struct adc_msg4_s { @@ -835,7 +844,7 @@ int sensors_thread_main(int argc, char *argv[]) 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) { + if ((((gyro_fail_count % 500) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { fprintf(stderr, "[sensors] GYRO ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } @@ -981,7 +990,7 @@ int sensors_thread_main(int argc, char *argv[]) } /* MAGNETOMETER */ - if (magcounter == 4) { /* 120 Hz */ + if (magcounter == 210) { /* 120 Hz */ uint64_t start_mag = hrt_absolute_time(); // /* start calibration mode if requested */ // if (!mag_calibration_enabled && vstatus.preflight_mag_calibration) { @@ -993,7 +1002,7 @@ int sensors_thread_main(int argc, char *argv[]) // printf("[sensors] disabling mag calibration mode\n"); // mag_calibration_enabled = false; // } - + *get_errno_ptr() = 0; ret_magnetometer = read(fd_magnetometer, &buf_magnetometer, sizeof(buf_magnetometer)); int errcode_mag = (int) * get_errno_ptr(); int magtime = hrt_absolute_time() - start_mag; @@ -1007,7 +1016,7 @@ int sensors_thread_main(int argc, char *argv[]) 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())); + fprintf(stderr, "[sensors] MAG ERROR #%d: %s\n", errcode_mag, strerror(errcode_mag)); } if (magn_healthy && mag_fail_count >= MAGN_HEALTH_COUNTER_LIMIT_ERROR) { @@ -1030,9 +1039,8 @@ int sensors_thread_main(int argc, char *argv[]) magtime = hrt_absolute_time() - start_mag; - if (magtime > 2000) { - printf("[sensors] WARN: MAG (overall time): %d us\n", magtime); - fprintf(stderr, "[sensors] TIMEOUT MAG ERROR #%d: %s\n", errcode_mag, strerror(errcode_mag)); + if (magtime > 2000 && (read_loop_counter % 5) == 0) { + printf("[sensors] WARN: MAG (overall time): %d us, code:\n", magtime); } magcounter = 0; @@ -1041,7 +1049,7 @@ int sensors_thread_main(int argc, char *argv[]) magcounter++; /* BAROMETER */ - if (barocounter == 5 && (fd_barometer > 0)) { /* 100 Hz */ + if (barocounter == 200 && (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)); @@ -1076,7 +1084,7 @@ int sensors_thread_main(int argc, char *argv[]) barocounter = 0; int barotime = hrt_absolute_time() - start_baro; - if (barotime > 2000) printf("BARO: %d us\n", barotime); + if (barotime > 2000 && (read_loop_counter % 5) == 0) printf("[sensors] WARN: BARO %d us\n", barotime); } barocounter++; diff --git a/apps/system/i2c/i2c_bus.c b/apps/system/i2c/i2c_bus.c index 07e6d2da3..4bca13c66 100644 --- a/apps/system/i2c/i2c_bus.c +++ b/apps/system/i2c/i2c_bus.c @@ -84,6 +84,8 @@ int i2ccmd_bus(FAR struct i2ctool_s *i2ctool, int argc, char **argv) for (i = CONFIG_I2CTOOL_MINBUS; i <= CONFIG_I2CTOOL_MAXBUS; i++) { dev = up_i2cinitialize(i); + // set the bus speed again to a reasonable number of 400 KHz + I2C_SETFREQUENCY(_dev, 400000); if (dev) { i2ctool_printf(i2ctool, "Bus %d: YES\n", i); diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c index e59cb33e3..20727e764 100644 --- a/apps/systemcmds/eeprom/eeprom.c +++ b/apps/systemcmds/eeprom/eeprom.c @@ -99,7 +99,7 @@ int eeprom_main(int argc, char *argv[]) } } - errx(1, "expected a command, try 'start'"); + errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/params'\n\t'load_param /eeprom/params'\n\t'erase'\n"); } |