diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c
index faec7c13b..d214b9e9d 100644
--- a/os/ex/ST/l3gd20.c
+++ b/os/ex/ST/l3gd20.c
@@ -264,24 +264,6 @@ static msg_t set_full_scale(void *ip, l3gd20_fs_t fs) {
   return MSG_OK;
 }
 
-static msg_t set_meas_unit(void *ip, l3gd20_unit_t unit) {
-  unsigned i;
-  if(unit != ((L3GD20Driver *)ip)->meas_unit) {
-    ((L3GD20Driver *)ip)->meas_unit = unit;
-    /* From RPS to DPS */
-    if(unit == L3GD20_UNIT_DPS)
-      for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-        ((L3GD20Driver *)ip)->sensitivity[i] *= (2 * PI);
-    /* From DPS to RPS */
-    else if(unit == L3GD20_UNIT_RPS)
-      for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-        ((L3GD20Driver *)ip)->sensitivity[i] /= (2 * PI);
-    else
-      return MSG_RESET;
-  }
-  return MSG_OK;
-}
-
 static const struct BaseSensorVMT vmt_basesensor = {
   get_axes_number, read_raw, read_cooked
 };
@@ -296,7 +278,7 @@ static const struct L3GD20VMT vmt_l3gd20 = {
   get_axes_number, read_raw, read_cooked,
   sample_bias, set_bias, reset_bias,
   set_sensivity, reset_sensivity,
-  set_full_scale, set_meas_unit
+  set_full_scale
 };
 
 /*===========================================================================*/
@@ -395,50 +377,23 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
 #if	L3GD20_SHARED_SPI
   spiReleaseBus((devp)->config->spip);
 #endif /* L3GD20_SHARED_SPI */
-#endif /* L3GD20_USE_SPI */
+#endif /* L3GD20_USE_SPI */
   
-  /* Storing sensitivity information according to full scale and unit value.*/
+  /* Storing sensitivity information according to full scale.*/
   if(devp->config->fullscale == L3GD20_FS_250DPS) {
     devp->fullscale = L3GD20_250DPS;
-    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
-      if(devp->meas_unit == L3GD20_UNIT_DPS) {
-        devp->sensitivity[i] = L3GD20_SENS_250DPS;
-      }
-      else if (devp->meas_unit == L3GD20_UNIT_RPS) {
-        devp->sensitivity[i] = L3GD20_SENS_250DPS / (2 * PI);
-      }
-      else {
-        devp->sensitivity[i] = 1.0;
-      }
-    }
+    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
+      devp->sensitivity[i] = L3GD20_SENS_250DPS;
   }
   else if(devp->config->fullscale == L3GD20_FS_500DPS) {
     devp->fullscale = L3GD20_500DPS;
-    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
-      if(devp->meas_unit == L3GD20_UNIT_DPS) {
-        devp->sensitivity[i] = L3GD20_SENS_500DPS;
-      }
-      else if (devp->meas_unit == L3GD20_UNIT_RPS) {
-        devp->sensitivity[i] = L3GD20_SENS_500DPS / (2 * PI);
-      }
-      else {
-        devp->sensitivity[i] = 1.0;
-      }
-    }
+    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
+      devp->sensitivity[i] = L3GD20_SENS_500DPS;
   }
   else if(devp->config->fullscale == L3GD20_FS_2000DPS) {
     devp->fullscale = L3GD20_2000DPS;
-    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
-      if(devp->meas_unit == L3GD20_UNIT_DPS) {
-        devp->sensitivity[i] = L3GD20_SENS_500DPS;
-      }
-      else if (devp->meas_unit == L3GD20_UNIT_RPS) {
-        devp->sensitivity[i] = L3GD20_SENS_500DPS / (2 * PI);
-      }
-      else {
-        devp->sensitivity[i] = 1.0;
-      }
-    }
+    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
+      devp->sensitivity[i] = L3GD20_SENS_2000DPS;
   }
   else
     osalDbgAssert(FALSE, "l3gd20Start(), full scale issue");
diff --git a/os/ex/ST/l3gd20.h b/os/ex/ST/l3gd20.h
index f2c3907da..f6b1780a7 100644
--- a/os/ex/ST/l3gd20.h
+++ b/os/ex/ST/l3gd20.h
@@ -392,14 +392,6 @@ typedef enum {
   L3GD20_END_BIG = 0x40             /**< Big endian.                        */
 }l3gd20_end_t;
 
-/**
- * @brief   L3GD20 measurement unit.
- */
-typedef enum {
-  L3GD20_UNIT_DPS = 0x00,           /**< Cooked data in degrees per seconds.*/
-  L3GD20_UNIT_RPS = 0x01,           /**< Cooked data in radians per seconds.*/
-} l3gd20_unit_t;
-
 /**
  * @brief   Driver state machine possible states.
  */
@@ -442,10 +434,6 @@ typedef struct {
    * @brief L3GD20 initial bias.
    */
   float                     bias[L3GD20_NUMBER_OF_AXES];
-  /**
-   * @brief   L3GD20 initial measurement unit.
-   */
-  l3gd20_unit_t             unit;
   /**
    * @brief L3GD20 initial full scale value.
    */
@@ -494,9 +482,7 @@ typedef struct L3GD20Driver L3GD20Driver;
 #define _l3gd20_methods                                                     \
   _base_gyroscope_methods                                                   \
   /* Change full scale value of L3GD20 .*/                                  \
-  msg_t (*set_full_scale)(void *instance, l3gd20_fs_t fs);                  \
-  /* Change measurement unit of L3GD20 .*/                                  \
-  msg_t (*set_meas_unit)(void *instance, l3gd20_unit_t unit);               \
+  msg_t (*set_full_scale)(void *instance, l3gd20_fs_t fs);
 
 /**
  * @extends BaseGyroscopeVMT
@@ -521,9 +507,7 @@ struct L3GD20VMT {
   /* Current Bias data.*/                                                   \
   float                     bias[L3GD20_NUMBER_OF_AXES];                    \
   /* Current full scale value.*/                                            \
-  float                     fullscale;                                      \
-  /* Measurement unit.*/                                                    \
-  l3gd20_unit_t             meas_unit;  
+  float                     fullscale;
 
 /**
  * @extends BaseGyroscope
@@ -560,20 +544,6 @@ struct L3GD20Driver {
  */
 #define gyroscopeSetFullScale(ip, fs)                                       \
         (ip)->vmt_l3gd20->set_full_scale(ip, fs)
-
-/**
- * @brief   Set gyroscope cooked data measurement unit.
- *
- * @param[in] ip        pointer to a @p BaseGyroscope class.
- * @param[in] unit      the MEMS measurement unit.
- *
- * @return              The operation status.
- * @retval MSG_OK       if the function succeeded.
- * @retval MSG_RESET    if one or more errors occurred.
- * @api
- */
-#define gyroscopeSetMeasurementUnit(ip, unit)                               \
-        (ip)->vmt_l3gd20->set_meas_unit(ip, unit)
         
 /*===========================================================================*/
 /* External declarations.                                                    */
diff --git a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
index e76b62441..fb21b0af8 100644
--- a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
+++ b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
@@ -35,7 +35,6 @@ static int32_t rawdata[L3GD20_NUMBER_OF_AXES];
 static float cookeddata[L3GD20_NUMBER_OF_AXES];
 
 static char axisID[L3GD20_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};
-static char unit[4] = "dps";
 static uint32_t i;
 
 static const SPIConfig spicfg = {
@@ -51,7 +50,6 @@ static L3GD20Config l3gd20cfg = {
   &spicfg,                                   /* Pointer to SPI Configuration.*/
   {0, 0, 0},                                 /* Use default sensitivity.*/
   {0, 0, 0},                                 /* Use default bias.*/
-  L3GD20_UNIT_DPS,                           /* Measurement unit DPS.*/
   L3GD20_FS_250DPS,                          /* Full scale value.*/
   L3GD20_ODR_760HZ,                          /* Output data rate.*/
 #if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
@@ -104,7 +102,7 @@ static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
       gyroscopeReadCooked(&L3GD20D1, cookeddata);
       chprintf(chp, "L3GD20 Gyroscope cooked data...\r\n");
       for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
-        chprintf(chp, "%c-axis: %.4f %s\r\n", axisID[i], cookeddata[i], unit);
+        chprintf(chp, "%c-axis: %.4f DPS\r\n", axisID[i], cookeddata[i]);
       }
     }
     else {
@@ -115,62 +113,30 @@ static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
   chprintf(chp, "Stopped\r\n");
 }
 
-static void cmd_set(BaseSequentialStream *chp, int argc, char *argv[]) {
+static void cmd_fullscale(BaseSequentialStream *chp, int argc, char *argv[]) {
   (void)argv;
-  if (argc < 1) {
-    chprintf(chp, "Usage: set [fs|unit] [value]\r\n");
+  if (argc != 1) {
+    chprintf(chp, "Usage: fullscale [250|500|2000]\r\n");
     return;
   }
-  if (!strcmp (argv[0], "fs")) {
 #if CHPRINTF_USE_ANSI_CODE
     chprintf(chp, "\033[2J\033[1;1H");
 #endif
-    if (argc != 2) {
-      chprintf(chp, "Usage: set fs [250|500|2000]\r\n");
-      return;
-    }
-    if(!strcmp (argv[1], "250")) {
-      gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_250DPS);
-      chprintf(chp, "L3GD20 Gyroscope full scale set to 250 dps...\r\n");
-    }
-    else if(!strcmp (argv[1], "500")) {
-      gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_500DPS);
-      chprintf(chp, "L3GD20 Gyroscope full scale set to 500 dps...\r\n");
-    }
-    else if(!strcmp (argv[1], "2000")) {
-      gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_2000DPS);
-      chprintf(chp, "L3GD20 Gyroscope full scale set to 2000 dps...\r\n");
-    }
-    else {
-      chprintf(chp, "Usage: set fs [250|500|2000]\r\n");
-      return;
-    }
+  if(!strcmp (argv[0], "250")) {
+    gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_250DPS);
+    chprintf(chp, "L3GD20 Gyroscope full scale set to 250 dps...\r\n");
   }
-  else if (!strcmp (argv[0], "unit")) {
-#if CHPRINTF_USE_ANSI_CODE
-    chprintf(chp, "\033[2J\033[1;1H");
-#endif
-    if (argc != 2) {
-      chprintf(chp, "Usage: set unit [dps|rps]\r\n");
-      return;
-    }
-    if(!strcmp (argv[1], "dps")) {
-      gyroscopeSetMeasurementUnit(&L3GD20D1, L3GD20_UNIT_DPS);
-      strcpy(unit, "dps");
-      chprintf(chp, "L3GD20 Gyroscope unit set to degrees per second...\r\n");
-    }
-    else if(!strcmp (argv[1], "rps")) {
-      gyroscopeSetMeasurementUnit(&L3GD20D1, L3GD20_UNIT_RPS);
-      strcpy(unit, "rps");
-      chprintf(chp, "L3GD20 Gyroscope unit set to radians per second...\r\n");
-    }
-    else {
-      chprintf(chp, "Usage: set unit [dps|rps]\r\n");
-      return;
-    }
+  else if(!strcmp (argv[0], "500")) {
+    gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_500DPS);
+    chprintf(chp, "L3GD20 Gyroscope full scale set to 500 dps...\r\n");
+  }
+  else if(!strcmp (argv[0], "2000")) {
+    gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_2000DPS);
+    chprintf(chp, "L3GD20 Gyroscope full scale set to 2000 dps...\r\n");
   }
   else {
-    chprintf(chp, "Usage: set [fs|unit] [value]\r\n");
+    chprintf(chp, "Usage: fullscale [250|500|2000]\r\n");
+    return;
   }
 }
 
@@ -211,7 +177,7 @@ static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) {
 
 static const ShellCommand commands[] = {
   {"read", cmd_read},
-  {"set", cmd_set},
+  {"fullscale", cmd_fullscale},
   {"bias", cmd_bias},
   {NULL, NULL}
 };
diff --git a/testhal/STM32/STM32F4xx/SPI-L3GD20/main.c b/testhal/STM32/STM32F4xx/SPI-L3GD20/main.c
index f43ffcbe8..ae30c6c98 100644
--- a/testhal/STM32/STM32F4xx/SPI-L3GD20/main.c
+++ b/testhal/STM32/STM32F4xx/SPI-L3GD20/main.c
@@ -35,7 +35,6 @@ static int32_t rawdata[L3GD20_NUMBER_OF_AXES];
 static float cookeddata[L3GD20_NUMBER_OF_AXES];
 
 static char axisID[L3GD20_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};
-static char unit[4] = "dps";
 static uint32_t i;
 
 static const SPIConfig spicfg = {
@@ -51,7 +50,6 @@ static L3GD20Config l3gd20cfg = {
   &spicfg,                                   /* Pointer to SPI Configuration.*/
   {0, 0, 0},                                 /* Use default sensitivity.*/
   {0, 0, 0},                                 /* Use default bias.*/
-  L3GD20_UNIT_DPS,                           /* Measurement unit DPS.*/
   L3GD20_FS_250DPS,                          /* Full scale value.*/
   L3GD20_ODR_760HZ,                          /* Output data rate.*/
 #if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
@@ -68,13 +66,6 @@ static L3GD20Config l3gd20cfg = {
 /* Command line related.                                                     */
 /*===========================================================================*/
 
-
-
-
-
-
-
-
 /* Enable use of special ANSI escape sequences.*/
 #define CHPRINTF_USE_ANSI_CODE      TRUE
 #define SHELL_WA_SIZE               THD_WORKING_AREA_SIZE(2048)
@@ -104,7 +95,7 @@ static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
       gyroscopeReadCooked(&L3GD20D1, cookeddata);
       chprintf(chp, "L3GD20 Gyroscope cooked data...\r\n");
       for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
-        chprintf(chp, "%c-axis: %.4f %s\r\n", axisID[i], cookeddata[i], unit);
+        chprintf(chp, "%c-axis: %.4f DPS\r\n", axisID[i], cookeddata[i]);
       }
     }
     else {
@@ -115,62 +106,30 @@ static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
   chprintf(chp, "Stopped\r\n");
 }
 
-static void cmd_set(BaseSequentialStream *chp, int argc, char *argv[]) {
+static void cmd_fullscale(BaseSequentialStream *chp, int argc, char *argv[]) {
   (void)argv;
-  if (argc < 1) {
-    chprintf(chp, "Usage: set [fs|unit] [value]\r\n");
+  if (argc != 1) {
+    chprintf(chp, "Usage: fullscale [250|500|2000]\r\n");
     return;
   }
-  if (!strcmp (argv[0], "fs")) {
 #if CHPRINTF_USE_ANSI_CODE
     chprintf(chp, "\033[2J\033[1;1H");
 #endif
-    if (argc != 2) {
-      chprintf(chp, "Usage: set fs [250|500|2000]\r\n");
-      return;
-    }
-    if(!strcmp (argv[1], "250")) {
-      gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_250DPS);
-      chprintf(chp, "L3GD20 Gyroscope full scale set to 250 dps...\r\n");
-    }
-    else if(!strcmp (argv[1], "500")) {
-      gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_500DPS);
-      chprintf(chp, "L3GD20 Gyroscope full scale set to 500 dps...\r\n");
-    }
-    else if(!strcmp (argv[1], "2000")) {
-      gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_2000DPS);
-      chprintf(chp, "L3GD20 Gyroscope full scale set to 2000 dps...\r\n");
-    }
-    else {
-      chprintf(chp, "Usage: set fs [250|500|2000]\r\n");
-      return;
-    }
+  if(!strcmp (argv[0], "250")) {
+    gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_250DPS);
+    chprintf(chp, "L3GD20 Gyroscope full scale set to 250 dps...\r\n");
   }
-  else if (!strcmp (argv[0], "unit")) {
-#if CHPRINTF_USE_ANSI_CODE
-    chprintf(chp, "\033[2J\033[1;1H");
-#endif
-    if (argc != 2) {
-      chprintf(chp, "Usage: set unit [dps|rps]\r\n");
-      return;
-    }
-    if(!strcmp (argv[1], "dps")) {
-      gyroscopeSetMeasurementUnit(&L3GD20D1, L3GD20_UNIT_DPS);
-      strcpy(unit, "dps");
-      chprintf(chp, "L3GD20 Gyroscope unit set to degrees per second...\r\n");
-    }
-    else if(!strcmp (argv[1], "rps")) {
-      gyroscopeSetMeasurementUnit(&L3GD20D1, L3GD20_UNIT_RPS);
-      strcpy(unit, "rps");
-      chprintf(chp, "L3GD20 Gyroscope unit set to radians per second...\r\n");
-    }
-    else {
-      chprintf(chp, "Usage: set unit [dps|rps]\r\n");
-      return;
-    }
+  else if(!strcmp (argv[0], "500")) {
+    gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_500DPS);
+    chprintf(chp, "L3GD20 Gyroscope full scale set to 500 dps...\r\n");
+  }
+  else if(!strcmp (argv[0], "2000")) {
+    gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_2000DPS);
+    chprintf(chp, "L3GD20 Gyroscope full scale set to 2000 dps...\r\n");
   }
   else {
-    chprintf(chp, "Usage: set [fs|unit] [value]\r\n");
+    chprintf(chp, "Usage: fullscale [250|500|2000]\r\n");
+    return;
   }
 }
 
@@ -212,7 +171,7 @@ static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) {
 
 static const ShellCommand commands[] = {
   {"read", cmd_read},
-  {"set", cmd_set},
+  {"fullscale", cmd_fullscale},
   {"bias", cmd_bias},
   {NULL, NULL}
 };
diff --git a/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c b/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c
index 7a54839e0..644a155f6 100644
--- a/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c
+++ b/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c
@@ -34,7 +34,6 @@ static int32_t rawdata[L3GD20_NUMBER_OF_AXES];
 static float cookeddata[L3GD20_NUMBER_OF_AXES];
 
 static char axisID[L3GD20_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};
-static char unit[4] = "dps";
 static uint32_t i;
 
 static const SPIConfig spicfg = {
@@ -50,7 +49,6 @@ static L3GD20Config l3gd20cfg = {
   &spicfg,                                   /* Pointer to SPI Configuration.*/
   {0, 0, 0},                                 /* Use default sensitivity.*/
   {0, 0, 0},                                 /* Use default bias.*/
-  L3GD20_UNIT_DPS,                           /* Measurement unit DPS.*/
   L3GD20_FS_250DPS,                          /* Full scale value.*/
   L3GD20_ODR_760HZ,                          /* Output data rate.*/
 #if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
@@ -103,7 +101,7 @@ static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
       gyroscopeReadCooked(&L3GD20D1, cookeddata);
       chprintf(chp, "L3GD20 Gyroscope cooked data...\r\n");
       for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
-        chprintf(chp, "%c-axis: %.4f %s\r\n", axisID[i], cookeddata[i], unit);
+        chprintf(chp, "%c-axis: %.4f DPS\r\n", axisID[i], cookeddata[i]);
       }
     }
     else {
@@ -114,62 +112,30 @@ static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
   chprintf(chp, "Stopped\r\n");
 }
 
-static void cmd_set(BaseSequentialStream *chp, int argc, char *argv[]) {
+static void cmd_fullscale(BaseSequentialStream *chp, int argc, char *argv[]) {
   (void)argv;
-  if (argc < 1) {
-    chprintf(chp, "Usage: set [fs|unit] [value]\r\n");
+  if (argc != 1) {
+    chprintf(chp, "Usage: fullscale [250|500|2000]\r\n");
     return;
   }
-  if (!strcmp (argv[0], "fs")) {
 #if CHPRINTF_USE_ANSI_CODE
     chprintf(chp, "\033[2J\033[1;1H");
 #endif
-    if (argc != 2) {
-      chprintf(chp, "Usage: set fs [250|500|2000]\r\n");
-      return;
-    }
-    if(!strcmp (argv[1], "250")) {
-      gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_250DPS);
-      chprintf(chp, "L3GD20 Gyroscope full scale set to 250 dps...\r\n");
-    }
-    else if(!strcmp (argv[1], "500")) {
-      gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_500DPS);
-      chprintf(chp, "L3GD20 Gyroscope full scale set to 500 dps...\r\n");
-    }
-    else if(!strcmp (argv[1], "2000")) {
-      gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_2000DPS);
-      chprintf(chp, "L3GD20 Gyroscope full scale set to 2000 dps...\r\n");
-    }
-    else {
-      chprintf(chp, "Usage: set fs [250|500|2000]\r\n");
-      return;
-    }
+  if(!strcmp (argv[0], "250")) {
+    gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_250DPS);
+    chprintf(chp, "L3GD20 Gyroscope full scale set to 250 dps...\r\n");
   }
-  else if (!strcmp (argv[0], "unit")) {
-#if CHPRINTF_USE_ANSI_CODE
-    chprintf(chp, "\033[2J\033[1;1H");
-#endif
-    if (argc != 2) {
-      chprintf(chp, "Usage: set unit [dps|rps]\r\n");
-      return;
-    }
-    if(!strcmp (argv[1], "dps")) {
-      gyroscopeSetMeasurementUnit(&L3GD20D1, L3GD20_UNIT_DPS);
-      strcpy(unit, "dps");
-      chprintf(chp, "L3GD20 Gyroscope unit set to degrees per second...\r\n");
-    }
-    else if(!strcmp (argv[1], "rps")) {
-      gyroscopeSetMeasurementUnit(&L3GD20D1, L3GD20_UNIT_RPS);
-      strcpy(unit, "rps");
-      chprintf(chp, "L3GD20 Gyroscope unit set to radians per second...\r\n");
-    }
-    else {
-      chprintf(chp, "Usage: set unit [dps|rps]\r\n");
-      return;
-    }
+  else if(!strcmp (argv[0], "500")) {
+    gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_500DPS);
+    chprintf(chp, "L3GD20 Gyroscope full scale set to 500 dps...\r\n");
+  }
+  else if(!strcmp (argv[0], "2000")) {
+    gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_2000DPS);
+    chprintf(chp, "L3GD20 Gyroscope full scale set to 2000 dps...\r\n");
   }
   else {
-    chprintf(chp, "Usage: set [fs|unit] [value]\r\n");
+    chprintf(chp, "Usage: fullscale [250|500|2000]\r\n");
+    return;
   }
 }
 
@@ -208,7 +174,7 @@ static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) {
 
 static const ShellCommand commands[] = {
   {"read", cmd_read},
-  {"set", cmd_set},
+  {"fullscale", cmd_fullscale},
   {"bias", cmd_bias},
   {NULL, NULL}
 };