improvements / debugging on I2C drivers

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");
 }