andreika-git
/
ChibiOS
mirror of https://github.com/andreika-git/ChibiOS.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Removed measurement unit management from L3GD20 driver. 

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9699 35acf78f-673a-0410-8e92-d51de3d6d3f4
This commit is contained in:
Rocco Marco Guglielmi 2016-07-10 20:01:00 +00:00
parent 724eef04f3
commit d5917172bc
5 changed files with 62 additions and 246 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
os/ex/ST/l3gd20.c
View File

@ -264,24 +264,6 @@ static msg_t set_full_scale(void *ip, l3gd20_fs_t fs) {
return MSG_OK; 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 = { static const struct BaseSensorVMT vmt_basesensor = {
get_axes_number, read_raw, read_cooked get_axes_number, read_raw, read_cooked
}; };
@ -296,7 +278,7 @@ static const struct L3GD20VMT vmt_l3gd20 = {
get_axes_number, read_raw, read_cooked, get_axes_number, read_raw, read_cooked,
sample_bias, set_bias, reset_bias, sample_bias, set_bias, reset_bias,
set_sensivity, reset_sensivity, set_sensivity, reset_sensivity,
set_full_scale, set_meas_unit set_full_scale
}; };
/*===========================================================================*/ /*===========================================================================*/
@ -397,48 +379,21 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
#endif /* L3GD20_SHARED_SPI */ #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) { if(devp->config->fullscale == L3GD20_FS_250DPS) {
devp->fullscale = L3GD20_250DPS; devp->fullscale = L3GD20_250DPS;
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) { for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
if(devp->meas_unit == L3GD20_UNIT_DPS) {
devp->sensitivity[i] = L3GD20_SENS_250DPS; 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;
}
}
}
else if(devp->config->fullscale == L3GD20_FS_500DPS) { else if(devp->config->fullscale == L3GD20_FS_500DPS) {
devp->fullscale = L3GD20_500DPS; devp->fullscale = L3GD20_500DPS;
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) { for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
if(devp->meas_unit == L3GD20_UNIT_DPS) {
devp->sensitivity[i] = L3GD20_SENS_500DPS; 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;
}
}
}
else if(devp->config->fullscale == L3GD20_FS_2000DPS) { else if(devp->config->fullscale == L3GD20_FS_2000DPS) {
devp->fullscale = L3GD20_2000DPS; devp->fullscale = L3GD20_2000DPS;
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) { for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
if(devp->meas_unit == L3GD20_UNIT_DPS) { devp->sensitivity[i] = L3GD20_SENS_2000DPS;
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;
}
}
} }
else else
osalDbgAssert(FALSE, "l3gd20Start(), full scale issue"); osalDbgAssert(FALSE, "l3gd20Start(), full scale issue");

34
os/ex/ST/l3gd20.h
View File

@ -392,14 +392,6 @@ typedef enum {
L3GD20_END_BIG = 0x40 /**< Big endian. */ L3GD20_END_BIG = 0x40 /**< Big endian. */
}l3gd20_end_t; }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. * @brief Driver state machine possible states.
*/ */
@ -442,10 +434,6 @@ typedef struct {
* @brief L3GD20 initial bias. * @brief L3GD20 initial bias.
*/ */
float bias[L3GD20_NUMBER_OF_AXES]; float bias[L3GD20_NUMBER_OF_AXES];
/**
* @brief L3GD20 initial measurement unit.
*/
l3gd20_unit_t unit;
/** /**
* @brief L3GD20 initial full scale value. * @brief L3GD20 initial full scale value.
*/ */
@ -494,9 +482,7 @@ typedef struct L3GD20Driver L3GD20Driver;
#define _l3gd20_methods \ #define _l3gd20_methods \
_base_gyroscope_methods \ _base_gyroscope_methods \
/* Change full scale value of L3GD20 .*/ \ /* Change full scale value of L3GD20 .*/ \
msg_t (*set_full_scale)(void *instance, l3gd20_fs_t fs); \ 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); \
/** /**
* @extends BaseGyroscopeVMT * @extends BaseGyroscopeVMT
@ -521,9 +507,7 @@ struct L3GD20VMT {
/* Current Bias data.*/ \ /* Current Bias data.*/ \
float bias[L3GD20_NUMBER_OF_AXES]; \ float bias[L3GD20_NUMBER_OF_AXES]; \
/* Current full scale value.*/ \ /* Current full scale value.*/ \
float fullscale; \ float fullscale;
/* Measurement unit.*/ \
l3gd20_unit_t meas_unit;
/** /**
* @extends BaseGyroscope * @extends BaseGyroscope
@ -561,20 +545,6 @@ struct L3GD20Driver {
#define gyroscopeSetFullScale(ip, fs) \ #define gyroscopeSetFullScale(ip, fs) \
(ip)->vmt_l3gd20->set_full_scale(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. */ /* External declarations. */
/*===========================================================================*/ /*===========================================================================*/

52
testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
View File

@ -35,7 +35,6 @@ static int32_t rawdata[L3GD20_NUMBER_OF_AXES];
static float cookeddata[L3GD20_NUMBER_OF_AXES]; static float cookeddata[L3GD20_NUMBER_OF_AXES];
static char axisID[L3GD20_NUMBER_OF_AXES] = {'X', 'Y', 'Z'}; static char axisID[L3GD20_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};
static char unit[4] = "dps";
static uint32_t i; static uint32_t i;
static const SPIConfig spicfg = { static const SPIConfig spicfg = {
@ -51,7 +50,6 @@ static L3GD20Config l3gd20cfg = {
&spicfg, /* Pointer to SPI Configuration.*/ &spicfg, /* Pointer to SPI Configuration.*/
{0, 0, 0}, /* Use default sensitivity.*/ {0, 0, 0}, /* Use default sensitivity.*/
{0, 0, 0}, /* Use default bias.*/ {0, 0, 0}, /* Use default bias.*/
L3GD20_UNIT_DPS, /* Measurement unit DPS.*/
L3GD20_FS_250DPS, /* Full scale value.*/ L3GD20_FS_250DPS, /* Full scale value.*/
L3GD20_ODR_760HZ, /* Output data rate.*/ L3GD20_ODR_760HZ, /* Output data rate.*/
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__) #if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
@ -104,7 +102,7 @@ static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
gyroscopeReadCooked(&L3GD20D1, cookeddata); gyroscopeReadCooked(&L3GD20D1, cookeddata);
chprintf(chp, "L3GD20 Gyroscope cooked data...\r\n"); chprintf(chp, "L3GD20 Gyroscope cooked data...\r\n");
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) { 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 { else {
@ -115,64 +113,32 @@ static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
chprintf(chp, "Stopped\r\n"); 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; (void)argv;
if (argc < 1) { if (argc != 1) {
chprintf(chp, "Usage: set [fs|unit] [value]\r\n"); chprintf(chp, "Usage: fullscale [250|500|2000]\r\n");
return; return;
} }
if (!strcmp (argv[0], "fs")) {
#if CHPRINTF_USE_ANSI_CODE #if CHPRINTF_USE_ANSI_CODE
chprintf(chp, "\033[2J\033[1;1H"); chprintf(chp, "\033[2J\033[1;1H");
#endif #endif
if (argc != 2) { if(!strcmp (argv[0], "250")) {
chprintf(chp, "Usage: set fs [250|500|2000]\r\n");
return;
}
if(!strcmp (argv[1], "250")) {
gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_250DPS); gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_250DPS);
chprintf(chp, "L3GD20 Gyroscope full scale set to 250 dps...\r\n"); chprintf(chp, "L3GD20 Gyroscope full scale set to 250 dps...\r\n");
} }
else if(!strcmp (argv[1], "500")) { else if(!strcmp (argv[0], "500")) {
gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_500DPS); gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_500DPS);
chprintf(chp, "L3GD20 Gyroscope full scale set to 500 dps...\r\n"); chprintf(chp, "L3GD20 Gyroscope full scale set to 500 dps...\r\n");
} }
else if(!strcmp (argv[1], "2000")) { else if(!strcmp (argv[0], "2000")) {
gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_2000DPS); gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_2000DPS);
chprintf(chp, "L3GD20 Gyroscope full scale set to 2000 dps...\r\n"); chprintf(chp, "L3GD20 Gyroscope full scale set to 2000 dps...\r\n");
} }
else { else {
chprintf(chp, "Usage: set fs [250|500|2000]\r\n"); chprintf(chp, "Usage: fullscale [250|500|2000]\r\n");
return; return;
} }
} }
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 {
chprintf(chp, "Usage: set [fs|unit] [value]\r\n");
}
}
static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) { static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) {
(void)argv; (void)argv;
@ -211,7 +177,7 @@ static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) {
static const ShellCommand commands[] = { static const ShellCommand commands[] = {
{"read", cmd_read}, {"read", cmd_read},
{"set", cmd_set}, {"fullscale", cmd_fullscale},
{"bias", cmd_bias}, {"bias", cmd_bias},
{NULL, NULL} {NULL, NULL}
}; };

59
testhal/STM32/STM32F4xx/SPI-L3GD20/main.c
View File

@ -35,7 +35,6 @@ static int32_t rawdata[L3GD20_NUMBER_OF_AXES];
static float cookeddata[L3GD20_NUMBER_OF_AXES]; static float cookeddata[L3GD20_NUMBER_OF_AXES];
static char axisID[L3GD20_NUMBER_OF_AXES] = {'X', 'Y', 'Z'}; static char axisID[L3GD20_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};
static char unit[4] = "dps";
static uint32_t i; static uint32_t i;
static const SPIConfig spicfg = { static const SPIConfig spicfg = {
@ -51,7 +50,6 @@ static L3GD20Config l3gd20cfg = {
&spicfg, /* Pointer to SPI Configuration.*/ &spicfg, /* Pointer to SPI Configuration.*/
{0, 0, 0}, /* Use default sensitivity.*/ {0, 0, 0}, /* Use default sensitivity.*/
{0, 0, 0}, /* Use default bias.*/ {0, 0, 0}, /* Use default bias.*/
L3GD20_UNIT_DPS, /* Measurement unit DPS.*/
L3GD20_FS_250DPS, /* Full scale value.*/ L3GD20_FS_250DPS, /* Full scale value.*/
L3GD20_ODR_760HZ, /* Output data rate.*/ L3GD20_ODR_760HZ, /* Output data rate.*/
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__) #if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
@ -68,13 +66,6 @@ static L3GD20Config l3gd20cfg = {
/* Command line related. */ /* Command line related. */
/*===========================================================================*/ /*===========================================================================*/
/* Enable use of special ANSI escape sequences.*/ /* Enable use of special ANSI escape sequences.*/
#define CHPRINTF_USE_ANSI_CODE TRUE #define CHPRINTF_USE_ANSI_CODE TRUE
#define SHELL_WA_SIZE THD_WORKING_AREA_SIZE(2048) #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); gyroscopeReadCooked(&L3GD20D1, cookeddata);
chprintf(chp, "L3GD20 Gyroscope cooked data...\r\n"); chprintf(chp, "L3GD20 Gyroscope cooked data...\r\n");
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) { 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 { else {
@ -115,64 +106,32 @@ static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
chprintf(chp, "Stopped\r\n"); 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; (void)argv;
if (argc < 1) { if (argc != 1) {
chprintf(chp, "Usage: set [fs|unit] [value]\r\n"); chprintf(chp, "Usage: fullscale [250|500|2000]\r\n");
return; return;
} }
if (!strcmp (argv[0], "fs")) {
#if CHPRINTF_USE_ANSI_CODE #if CHPRINTF_USE_ANSI_CODE
chprintf(chp, "\033[2J\033[1;1H"); chprintf(chp, "\033[2J\033[1;1H");
#endif #endif
if (argc != 2) { if(!strcmp (argv[0], "250")) {
chprintf(chp, "Usage: set fs [250|500|2000]\r\n");
return;
}
if(!strcmp (argv[1], "250")) {
gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_250DPS); gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_250DPS);
chprintf(chp, "L3GD20 Gyroscope full scale set to 250 dps...\r\n"); chprintf(chp, "L3GD20 Gyroscope full scale set to 250 dps...\r\n");
} }
else if(!strcmp (argv[1], "500")) { else if(!strcmp (argv[0], "500")) {
gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_500DPS); gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_500DPS);
chprintf(chp, "L3GD20 Gyroscope full scale set to 500 dps...\r\n"); chprintf(chp, "L3GD20 Gyroscope full scale set to 500 dps...\r\n");
} }
else if(!strcmp (argv[1], "2000")) { else if(!strcmp (argv[0], "2000")) {
gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_2000DPS); gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_2000DPS);
chprintf(chp, "L3GD20 Gyroscope full scale set to 2000 dps...\r\n"); chprintf(chp, "L3GD20 Gyroscope full scale set to 2000 dps...\r\n");
} }
else { else {
chprintf(chp, "Usage: set fs [250|500|2000]\r\n"); chprintf(chp, "Usage: fullscale [250|500|2000]\r\n");
return; return;
} }
} }
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 {
chprintf(chp, "Usage: set [fs|unit] [value]\r\n");
}
}
static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) { static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) {
(void)argv; (void)argv;
@ -212,7 +171,7 @@ static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) {
static const ShellCommand commands[] = { static const ShellCommand commands[] = {
{"read", cmd_read}, {"read", cmd_read},
{"set", cmd_set}, {"fullscale", cmd_fullscale},
{"bias", cmd_bias}, {"bias", cmd_bias},
{NULL, NULL} {NULL, NULL}
}; };

52
testhal/STM32/STM32L4xx/SPI-L3GD20/main.c
View File

@ -34,7 +34,6 @@ static int32_t rawdata[L3GD20_NUMBER_OF_AXES];
static float cookeddata[L3GD20_NUMBER_OF_AXES]; static float cookeddata[L3GD20_NUMBER_OF_AXES];
static char axisID[L3GD20_NUMBER_OF_AXES] = {'X', 'Y', 'Z'}; static char axisID[L3GD20_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};
static char unit[4] = "dps";
static uint32_t i; static uint32_t i;
static const SPIConfig spicfg = { static const SPIConfig spicfg = {
@ -50,7 +49,6 @@ static L3GD20Config l3gd20cfg = {
&spicfg, /* Pointer to SPI Configuration.*/ &spicfg, /* Pointer to SPI Configuration.*/
{0, 0, 0}, /* Use default sensitivity.*/ {0, 0, 0}, /* Use default sensitivity.*/
{0, 0, 0}, /* Use default bias.*/ {0, 0, 0}, /* Use default bias.*/
L3GD20_UNIT_DPS, /* Measurement unit DPS.*/
L3GD20_FS_250DPS, /* Full scale value.*/ L3GD20_FS_250DPS, /* Full scale value.*/
L3GD20_ODR_760HZ, /* Output data rate.*/ L3GD20_ODR_760HZ, /* Output data rate.*/
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__) #if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
@ -103,7 +101,7 @@ static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
gyroscopeReadCooked(&L3GD20D1, cookeddata); gyroscopeReadCooked(&L3GD20D1, cookeddata);
chprintf(chp, "L3GD20 Gyroscope cooked data...\r\n"); chprintf(chp, "L3GD20 Gyroscope cooked data...\r\n");
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) { 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 { else {
@ -114,64 +112,32 @@ static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
chprintf(chp, "Stopped\r\n"); 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; (void)argv;
if (argc < 1) { if (argc != 1) {
chprintf(chp, "Usage: set [fs|unit] [value]\r\n"); chprintf(chp, "Usage: fullscale [250|500|2000]\r\n");
return; return;
} }
if (!strcmp (argv[0], "fs")) {
#if CHPRINTF_USE_ANSI_CODE #if CHPRINTF_USE_ANSI_CODE
chprintf(chp, "\033[2J\033[1;1H"); chprintf(chp, "\033[2J\033[1;1H");
#endif #endif
if (argc != 2) { if(!strcmp (argv[0], "250")) {
chprintf(chp, "Usage: set fs [250|500|2000]\r\n");
return;
}
if(!strcmp (argv[1], "250")) {
gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_250DPS); gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_250DPS);
chprintf(chp, "L3GD20 Gyroscope full scale set to 250 dps...\r\n"); chprintf(chp, "L3GD20 Gyroscope full scale set to 250 dps...\r\n");
} }
else if(!strcmp (argv[1], "500")) { else if(!strcmp (argv[0], "500")) {
gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_500DPS); gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_500DPS);
chprintf(chp, "L3GD20 Gyroscope full scale set to 500 dps...\r\n"); chprintf(chp, "L3GD20 Gyroscope full scale set to 500 dps...\r\n");
} }
else if(!strcmp (argv[1], "2000")) { else if(!strcmp (argv[0], "2000")) {
gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_2000DPS); gyroscopeSetFullScale(&L3GD20D1, L3GD20_FS_2000DPS);
chprintf(chp, "L3GD20 Gyroscope full scale set to 2000 dps...\r\n"); chprintf(chp, "L3GD20 Gyroscope full scale set to 2000 dps...\r\n");
} }
else { else {
chprintf(chp, "Usage: set fs [250|500|2000]\r\n"); chprintf(chp, "Usage: fullscale [250|500|2000]\r\n");
return; return;
} }
} }
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 {
chprintf(chp, "Usage: set [fs|unit] [value]\r\n");
}
}
static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) { static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) {
(void)argv; (void)argv;
@ -208,7 +174,7 @@ static void cmd_bias(BaseSequentialStream *chp, int argc, char *argv[]) {
static const ShellCommand commands[] = { static const ShellCommand commands[] = {
{"read", cmd_read}, {"read", cmd_read},
{"set", cmd_set}, {"fullscale", cmd_fullscale},
{"bias", cmd_bias}, {"bias", cmd_bias},
{NULL, NULL} {NULL, NULL}
}; };