diff options
Diffstat (limited to 'os')
| -rw-r--r-- | os/ex/ST/l3gd20.c | 45 | ||||
| -rw-r--r-- | os/ex/ST/l3gd20.h | 2 | ||||
| -rw-r--r-- | os/hal/lib/peripherals/sensors/hal_gyroscope.h | 23 |
3 files changed, 38 insertions, 32 deletions
diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c index 4c6965875..63b902058 100644 --- a/os/ex/ST/l3gd20.c +++ b/os/ex/ST/l3gd20.c @@ -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 */ diff --git a/os/ex/ST/l3gd20.h b/os/ex/ST/l3gd20.h index 410e6e28a..e13dafba5 100644 --- a/os/ex/ST/l3gd20.h +++ b/os/ex/ST/l3gd20.h @@ -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]; diff --git a/os/hal/lib/peripherals/sensors/hal_gyroscope.h b/os/hal/lib/peripherals/sensors/hal_gyroscope.h index ac2901fe4..e61d06b0f 100644 --- a/os/hal/lib/peripherals/sensors/hal_gyroscope.h +++ b/os/hal/lib/peripherals/sensors/hal_gyroscope.h @@ -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,10 @@ 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.
*
@@ -142,11 +145,11 @@ typedef struct { *
* @api
*/
-#define gyroscopeResetCalibration(ip) \
- (ip)->vmt_basegyroscope->reset_calibration(ip)
+#define gyroscopeSampleBias(ip) \
+ (ip)->vmt_basegyroscope->sample_bias(ip)
/**
- * @brief Gyroscope calibration procedure.
+ * @brief Reset bias data restoring it to zero.
*
* @param[in] ip pointer to a @p BaseGyroscope class.
*
@@ -156,8 +159,8 @@ typedef struct { *
* @api
*/
-#define gyroscopeCalibrate(ip) \
- (ip)->vmt_basegyroscope->calibrate(ip)
+#define gyroscopeResetCalibration(ip) \
+ (ip)->vmt_basegyroscope->reset_bias(ip)
/** @} */
/*===========================================================================*/
|