MPU6000: Add regdump command

Add mpu6000 regdump command for debugging mpu6000.

1 files changed, 42 insertions, 2 deletions

diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index a95e041a1..c9c27892f 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -194,6 +194,8 @@ public:
 	 */
 	void			print_info();
 
+	void			print_registers();
+
 protected:
 	virtual int		probe();
 
@@ -1414,6 +1416,21 @@ MPU6000::print_info()
 	_gyro_reports->print_info("gyro queue");
 }
 
+void
+MPU6000::print_registers()
+{
+	printf("MPU6000 registers\n");
+	for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) {
+		uint8_t v = read_reg(reg);
+		printf("%02x:%02x ",(unsigned)reg, (unsigned)v);
+		if ((reg - (MPUREG_PRODUCT_ID-1)) % 13 == 0) {
+			printf("\n");
+		}
+	}
+	printf("\n");
+}
+
+
 MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
 	CDev("MPU6000_gyro", path),
 	_parent(parent),
@@ -1479,6 +1496,7 @@ void	start(bool, enum Rotation);
 void	test(bool);
 void	reset(bool);
 void	info(bool);
+void	regdump(bool);
 void	usage();
 
 /**
@@ -1654,10 +1672,26 @@ info(bool external_bus)
 	exit(0);
 }
 
+/**
+ * Dump the register information
+ */
+void
+regdump(bool external_bus)
+{
+	MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	if (*g_dev_ptr == nullptr)
+		errx(1, "driver not running");
+
+	printf("regdump @ %p\n", *g_dev_ptr);
+	(*g_dev_ptr)->print_registers();
+
+	exit(0);
+}
+
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -1714,5 +1748,11 @@ mpu6000_main(int argc, char *argv[])
 	if (!strcmp(verb, "info"))
 		mpu6000::info(external_bus);
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	/*
+	 * Print register information.
+	 */
+	if (!strcmp(verb, "regdump"))
+		mpu6000::regdump(external_bus);
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
 }