zcy
/
tinySA
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Improved Gyroscope methods. Updated L3GD20 files and demos. 

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9126 35acf78f-673a-0410-8e92-d51de3d6d3f4
master
Rocco Marco Guglielmi 2016-03-16 11:45:23 +00:00
parent 1914bcbbf4
commit 94bbc4d19c
6 changed files with 90 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -244,26 +244,12 @@ static msg_t read_cooked(void *ip, float axes[]) {
msg = read_raw(ip, raw);
for(i = 0; i < L3GD20_NUMBER_OF_AXES ; i++){
axes[i] = raw[i] * ((L3GD20Driver *)ip)->sensitivity;
axes[i] = raw[i] * ((L3GD20Driver *)ip)->sensitivity[i];
}
return msg;
}
static msg_t reset_calibration(void *ip) {
uint32_t i;
osalDbgCheck(ip != NULL);
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY) ||
(((L3GD20Driver *)ip)->state == L3GD20_STOP),
"reset_calibration(), invalid state");
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
((L3GD20Driver *)ip)->bias[i] = 0;
return MSG_OK;
}
static msg_t calibrate(void *ip) {
static msg_t sample_bias(void *ip) {
uint32_t i, j;
int32_t raw[L3GD20_NUMBER_OF_AXES];
int32_t buff[L3GD20_NUMBER_OF_AXES] = {0, 0, 0};
@ -287,13 +273,27 @@ static msg_t calibrate(void *ip) {
return MSG_OK;
}
static msg_t reset_bias(void *ip) {
uint32_t i;
osalDbgCheck(ip != NULL);
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY) ||
(((L3GD20Driver *)ip)->state == L3GD20_STOP),
"reset_calibration(), invalid state");
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
((L3GD20Driver *)ip)->bias[i] = 0;
return MSG_OK;
}
static const struct BaseSensorVMT vmt_basesensor = {
get_axes_number, read_raw, read_cooked
};
static const struct BaseGyroscopeVMT vmt_basegyroscope = {
get_axes_number, read_raw, read_cooked,
reset_calibration, calibrate
sample_bias, reset_bias
};
@ -329,7 +329,7 @@ void l3gd20ObjectInit(L3GD20Driver *devp) {
* @api
*/
void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
uint8_t i;
osalDbgCheck((devp != NULL) && (config != NULL));
osalDbgAssert((devp->state == L3GD20_STOP) || (devp->state == L3GD20_READY),
@ -358,11 +358,14 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
/* Storing sensitivity information according to full scale value */
if(devp->config->fullscale == L3GD20_FS_250DPS)
devp->sensitivity = L3GD20_SENS_250DPS;
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
devp->sensitivity[i] = L3GD20_SENS_250DPS;
else if(devp->config->fullscale == L3GD20_FS_500DPS)
devp->sensitivity = L3GD20_SENS_500DPS;
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
devp->sensitivity[i] = L3GD20_SENS_500DPS;
else if(devp->config->fullscale == L3GD20_FS_2000DPS)
devp->sensitivity = L3GD20_SENS_2000DPS;
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
devp->sensitivity[i] = L3GD20_SENS_2000DPS;
else
osalDbgAssert(FALSE, "l3gd20Start(), full scale issue");
/* This is the Gyroscope transient recovery time */

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

@ -266,7 +266,7 @@ struct L3GD20VMT {
/* Current configuration data.*/ \
const L3GD20Config *config; \
/* Current sensitivity.*/ \
float sensitivity; \
float sensitivity[L3GD20_NUMBER_OF_AXES]; \
/* Bias data.*/ \
int32_t bias[L3GD20_NUMBER_OF_AXES];

43
os/hal/lib/peripherals/sensors/hal_gyroscope.h
View File

@ -47,10 +47,10 @@
* @brief BaseGyroscope specific methods.
*/
#define _base_gyroscope_methods_alone \
/* Remove the calibration data.*/ \
msg_t (*reset_calibration)(void *instance); \
/* Invokes the calibration procedure.*/ \
msg_t (*calibrate)(void *instance);
/* Invoke the sample bias procedure.*/ \
msg_t (*sample_bias)(void *instance); \
/* Remove bias stored data.*/ \
msg_t (*reset_bias)(void *instance);
/**
* @brief BaseGyroscope specific methods with inherited ones.
@ -132,7 +132,24 @@ typedef struct {
(ip)->vmt_basegyroscope->read_cooked(ip, dp)
/**
* @brief Delete calibration data.
* @brief Gyroscope bias sampling procedure.
* @note During this procedure gyroscope must be kept hold in the rest
* position. Sampled bias will be automatically removed after calling
* this procedure.
*
* @param[in] ip pointer to a @p BaseGyroscope class.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
* @retval MSG_RESET if one or more errors occurred.
*
* @api
*/
#define gyroscopeSampleBias(ip) \
(ip)->vmt_basegyroscope->sample_bias(ip)
/**
* @brief Reset bias data restoring it to zero.
*
* @param[in] ip pointer to a @p BaseGyroscope class.
*
@ -143,21 +160,7 @@ typedef struct {
* @api
*/
#define gyroscopeResetCalibration(ip) \
(ip)->vmt_basegyroscope->reset_calibration(ip)
/**
* @brief Gyroscope calibration procedure.
*
* @param[in] ip pointer to a @p BaseGyroscope class.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
* @retval MSG_RESET if one or more errors occurred.
*
* @api
*/
#define gyroscopeCalibrate(ip) \
(ip)->vmt_basegyroscope->calibrate(ip)
(ip)->vmt_basegyroscope->reset_bias(ip)
/** @} */
/*===========================================================================*/

38
testhal/STM32/STM32F3xx/SPI-L3GD20/.project Normal file
View File

@ -0,0 +1,38 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>STM32F3xx-SPI-L3GD20</name>
<comment></comment>
<projects>
</projects>
<buildSpec>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
<triggers>clean,full,incremental,</triggers>
<arguments>
</arguments>
</buildCommand>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
<triggers>full,incremental,</triggers>
<arguments>
</arguments>
</buildCommand>
</buildSpec>
<natures>
<nature>org.eclipse.cdt.core.cnature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
</natures>
<linkedResources>
<link>
<name>board</name>
<type>2</type>
<location>C:/ChibiStudio/chibios_trunk/os/hal/boards/ST_STM32F3_DISCOVERY</location>
</link>
<link>
<name>os</name>
<type>2</type>
<locationURI>CHIBIOS/os</locationURI>
</link>
</linkedResources>
</projectDescription>

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

@ -146,14 +146,14 @@ int main(void) {
}
palClearLine(LINE_LED10_RED);
chprintf(chp, "Calibrating Gyroscope...\r\n");
chprintf(chp, "Calibrating Gyroscope sampling bias...\r\n");
chprintf(chp, "Keep it in the rest position while red LED is on\r\n");
chThdSleepMilliseconds(3000);
palSetLine(LINE_LED10_RED);
chThdSleepMilliseconds(1000);
gyroscopeCalibrate(&L3GD20D1);
gyroscopeSampleBias(&L3GD20D1);
palClearLine(LINE_LED10_RED);
#if CHPRINTF_USE_ANSI_CODE
chprintf(chp, "\033[2J\033[1;1H");

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

@ -119,14 +119,14 @@ int main(void) {
}
palClearLine(LINE_LED_RED);
chprintf(chp, "Calibrating Gyroscope...\r\n");
chprintf(chp, "Calibrating Gyroscope sampling bias...\r\n");
chprintf(chp, "Keep it in the rest position while red LED is on\r\n");
chThdSleepMilliseconds(3000);
palSetLine(LINE_LED_RED);
chThdSleepMilliseconds(1000);
gyroscopeCalibrate(&L3GD20D1);
gyroscopeSampleBias(&L3GD20D1);
palClearLine(LINE_LED_RED);
#if CHPRINTF_USE_ANSI_CODE
chprintf(chp, "\033[2J\033[1;1H");