From 67b83e80b6539c4cfef7ae575b9655ab193bef96 Mon Sep 17 00:00:00 2001
From: Rocco Marco Guglielmi <roccomarco.guglielmi@live.com>
Date: Fri, 17 Jun 2016 12:13:44 +0000
Subject: Improved STM32F3 L3GD20 test demo

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9639 35acf78f-673a-0410-8e92-d51de3d6d3f4
---
 testhal/STM32/STM32F3xx/SPI-L3GD20/main.c | 36 +++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)

(limited to 'testhal/STM32/STM32F3xx/SPI-L3GD20/main.c')

diff --git a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
index 6ac0c4f27..804f3522b 100644
--- a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
+++ b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
@@ -166,9 +166,45 @@ static void cmd_set(BaseSequentialStream *chp, int argc, char *argv[]) {
   }
 }
 
+static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) {
+  (void)argv;
+  if (argc != 1) {
+    chprintf(chp, "Usage: bias [sample|reset]\r\n");
+    return;
+  }
+  if(!strcmp (argv[0], "sample")) {
+#if CHPRINTF_USE_ANSI_CODE
+    chprintf(chp, "\033[2J\033[1;1H");
+#endif
+    chprintf(chp, "Please don't move the device while Green LEDs are on!\r\n");
+    chprintf(chp, "Press a key to start...\r\n");
+    while (chnGetTimeout((BaseChannel *)chp, TIME_IMMEDIATE) == Q_TIMEOUT)
+      ;
+    palSetLine(LINE_LED6_GREEN);
+    palSetLine(LINE_LED7_GREEN);
+    chThdSleepMilliseconds(1000);
+    gyroscopeSampleBias(&L3GD20D1);
+    palClearLine(LINE_LED6_GREEN);
+    palClearLine(LINE_LED7_GREEN);
+    chprintf(chp, "Procedure completed!\r\n");
+  }
+  else if(!strcmp (argv[0], "reset")) {
+#if CHPRINTF_USE_ANSI_CODE
+    chprintf(chp, "\033[2J\033[1;1H");
+#endif
+    gyroscopeResetBias(&L3GD20D1);
+    chprintf(chp, "Bias correction removed!\r\n");
+  }
+  else {
+    chprintf(chp, "Usage: bias [sample|reset]\r\n");
+    return;
+  }
+}
+
 static const ShellCommand commands[] = {
   {"get", cmd_get},
   {"set", cmd_set},
+  {"bias", cmd_bias},
   {NULL, NULL}
 };
 
-- 
cgit v1.2.3