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

Improved BaseGyroscope interface. Updated L3GD20. 

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9128 35acf78f-673a-0410-8e92-d51de3d6d3f4
master
Rocco Marco Guglielmi 2016-03-16 18:16:30 +00:00
parent 13375b4faa
commit 49dac4f825
2 changed files with 161 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -257,7 +257,7 @@ static msg_t sample_bias(void *ip) {
osalDbgCheck(ip != NULL);
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
"calibrate(), invalid state");
"sample_bias(), invalid state");
for(i = 0; i < L3GD20_BIAS_ACQ_TIMES; i++){
read_raw(ip, raw);
@ -273,6 +273,21 @@ static msg_t sample_bias(void *ip) {
return MSG_OK;
}
static msg_t set_bias(void *ip, int32_t *bp) {
uint32_t i;
osalDbgCheck((ip != NULL) && (bp !=NULL));
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY) ||
(((L3GD20Driver *)ip)->state == L3GD20_STOP),
"set_bias(), invalid state");
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
((L3GD20Driver *)ip)->bias[i] = bp[i];
}
return MSG_OK;
}
static msg_t reset_bias(void *ip) {
uint32_t i;
@ -287,16 +302,64 @@ static msg_t reset_bias(void *ip) {
return MSG_OK;
}
static msg_t set_sensivity(void *ip, float *sp) {
uint32_t i;
osalDbgCheck((ip != NULL) && (sp !=NULL));
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
"set_sensivity(), invalid state");
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
((L3GD20Driver *)ip)->sensitivity[i] = sp[i];
}
return MSG_OK;
}
static msg_t reset_sensivity(void *ip) {
uint32_t i;
osalDbgCheck(ip != NULL);
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
"reset_sensivity(), invalid state");
if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_250DPS)
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_250DPS;
else if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_500DPS)
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_500DPS;
else if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_2000DPS)
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_2000DPS;
return MSG_OK;
}
static msg_t enable_temperature_compensation(void *ip) {
(void) ip;
/* TODO complete this function */
return 0;
}
static msg_t disable_temperature_compensation(void *ip) {
(void) ip;
/* TODO complete this function */
return 0;
}
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,
sample_bias, reset_bias
sample_bias, set_bias, reset_bias,
set_sensivity, reset_sensivity,
enable_temperature_compensation,
disable_temperature_compensation
};
/*===========================================================================*/
/* Driver exported functions. */
/*===========================================================================*/

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

@ -49,8 +49,19 @@
#define _base_gyroscope_methods_alone \
/* Invoke the sample bias procedure.*/ \
msg_t (*sample_bias)(void *instance); \
/* Invoke the set bias procedure.*/ \
msg_t (*set_bias)(void *instance, int32_t biases[]); \
/* Remove bias stored data.*/ \
msg_t (*reset_bias)(void *instance);
msg_t (*reset_bias)(void *instance); \
/* Invoke the set sensitivity procedure.*/ \
msg_t (*set_sensitivity)(void *instance, float sensitivities[]); \
/* Restore sensitivity stored data to default.*/ \
msg_t (*reset_sensitivity)(void *instance); \
/* Enable temperature drift effect compensation.*/ \
msg_t (*enable_temperature_compensation)(void *instance); \
/* Disable temperature drift effect compensation.*/ \
msg_t (*disable_temperature_compensation)(void *instance);
/**
* @brief BaseGyroscope specific methods with inherited ones.
@ -134,8 +145,8 @@ typedef struct {
/**
* @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.
* position. Sampled bias will be automatically removed after
* calling this procedure.
*
* @param[in] ip pointer to a @p BaseGyroscope class.
*
@ -145,11 +156,28 @@ typedef struct {
*
* @api
*/
#define gyroscopeSampleBias(ip) \
#define gyroscopeSampleBias(ip) \
(ip)->vmt_basegyroscope->sample_bias(ip)
/**
* @brief Reset bias data restoring it to zero.
* @brief Updates gyroscope bias data from received buffer.
* @note The bias buffer must have the same length of the
* the gyroscope axes number.
*
* @param[in] ip pointer to a @p BaseGyroscope class.
* @param[in] bp pointer to a buffer of bias values.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
* @retval MSG_RESET if one or more errors occurred.
*
* @api
*/
#define gyroscopeSetBias(ip, bp) \
(ip)->vmt_basegyroscope->set_bias(ip, bp)
/**
* @brief Reset gyroscope bias data restoring it to zero.
*
* @param[in] ip pointer to a @p BaseGyroscope class.
*
@ -159,8 +187,68 @@ typedef struct {
*
* @api
*/
#define gyroscopeResetCalibration(ip) \
#define gyroscopeResetBias(ip) \
(ip)->vmt_basegyroscope->reset_bias(ip)
/**
* @brief Updates gyroscope sensitivity data from received buffer.
* @note The sensitivity buffer must have the same length of the
* the gyroscope axes number.
*
* @param[in] ip pointer to a @p BaseGyroscope class.
* @param[in] sp pointer to a buffer of sensitivity values.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
* @retval MSG_RESET if one or more errors occurred.
*
* @api
*/
#define gyroscopeSetSensitivity(ip, sp) \
(ip)->vmt_basegyroscope->set_sensitivity(ip, sp)
/**
* @brief Reset gyroscope sensitivity data restoring it to its typical
* value.
*
* @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 gyroscopeResetSensitivity(ip) \
(ip)->vmt_basegyroscope->reset_sensitivity(ip)
/**
* @brief Enables data compensation removing temperature drift.
*
* @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 gyroscopeEnableTempCompensation(ip) \
(ip)->vmt_basegyroscope->enable_temperature_compensation(ip)
/**
* @brief Disable data compensation.
*
* @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 gyroscopeDisableTempCompensation(ip) \
(ip)->vmt_basegyroscope->disable_temperature_compensation(ip)
/** @} */
/*===========================================================================*/