mirror of https://github.com/rusefi/ChibiOS.git
Improved LSM303DLHC driver
git-svn-id: https://svn.code.sf.net/p/chibios/svn2/trunk@11639 110e8d01-0319-4d1e-a829-52ad28d1bb01
This commit is contained in:
parent
6ed6b094b3
commit
d1d8e8bb3a
|
@ -97,7 +97,7 @@ static msg_t lsm303dlhcI2CWriteRegister(I2CDriver *i2cp, lsm303dlhc_sad_t sad,
|
|||
/**
|
||||
* @brief Return the number of axes of the BaseAccelerometer.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseAccelerometer interface
|
||||
* @param[in] ip pointer to @p BaseAccelerometer interface.
|
||||
*
|
||||
* @return the number of axes.
|
||||
*/
|
||||
|
@ -195,7 +195,7 @@ static msg_t acc_read_cooked(void *ip, float axes[]) {
|
|||
"acc_read_cooked(), channel not ready");
|
||||
|
||||
msg = acc_read_raw(ip, raw);
|
||||
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES ; i++) {
|
||||
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
|
||||
axes[i] = raw[i] * devp->accsensitivity[i];
|
||||
axes[i] -= devp->accbias[i];
|
||||
}
|
||||
|
@ -217,6 +217,7 @@ static msg_t acc_read_cooked(void *ip, float axes[]) {
|
|||
static msg_t acc_set_bias(void *ip, float *bp) {
|
||||
LSM303DLHCDriver* devp;
|
||||
uint32_t i;
|
||||
msg_t msg = MSG_OK;
|
||||
|
||||
osalDbgCheck((ip != NULL) && (bp != NULL));
|
||||
|
||||
|
@ -229,7 +230,7 @@ static msg_t acc_set_bias(void *ip, float *bp) {
|
|||
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
|
||||
devp->accbias[i] = bp[i];
|
||||
}
|
||||
return MSG_OK;
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -245,6 +246,7 @@ static msg_t acc_set_bias(void *ip, float *bp) {
|
|||
static msg_t acc_reset_bias(void *ip) {
|
||||
LSM303DLHCDriver* devp;
|
||||
uint32_t i;
|
||||
msg_t msg = MSG_OK;
|
||||
|
||||
osalDbgCheck(ip != NULL);
|
||||
|
||||
|
@ -256,7 +258,7 @@ static msg_t acc_reset_bias(void *ip) {
|
|||
|
||||
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
|
||||
devp->accbias[i] = 0.0;
|
||||
return MSG_OK;
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -274,6 +276,7 @@ static msg_t acc_reset_bias(void *ip) {
|
|||
static msg_t acc_set_sensivity(void *ip, float *sp) {
|
||||
LSM303DLHCDriver* devp;
|
||||
uint32_t i;
|
||||
msg_t msg = MSG_OK;
|
||||
|
||||
/* Getting parent instance pointer.*/
|
||||
devp = objGetInstance(LSM303DLHCDriver*, (BaseAccelerometer*)ip);
|
||||
|
@ -286,7 +289,7 @@ static msg_t acc_set_sensivity(void *ip, float *sp) {
|
|||
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
|
||||
devp->accsensitivity[i] = sp[i];
|
||||
}
|
||||
return MSG_OK;
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -551,7 +554,8 @@ static msg_t comp_read_cooked(void *ip, float axes[]) {
|
|||
static msg_t comp_set_bias(void *ip, float *bp) {
|
||||
LSM303DLHCDriver* devp;
|
||||
uint32_t i;
|
||||
|
||||
msg_t msg = MSG_OK;
|
||||
|
||||
osalDbgCheck((ip != NULL) && (bp != NULL));
|
||||
|
||||
/* Getting parent instance pointer.*/
|
||||
|
@ -563,7 +567,7 @@ static msg_t comp_set_bias(void *ip, float *bp) {
|
|||
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
|
||||
devp->compbias[i] = bp[i];
|
||||
}
|
||||
return MSG_OK;
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -579,6 +583,7 @@ static msg_t comp_set_bias(void *ip, float *bp) {
|
|||
static msg_t comp_reset_bias(void *ip) {
|
||||
LSM303DLHCDriver* devp;
|
||||
uint32_t i;
|
||||
msg_t msg = MSG_OK;
|
||||
|
||||
osalDbgCheck(ip != NULL);
|
||||
|
||||
|
@ -590,7 +595,7 @@ static msg_t comp_reset_bias(void *ip) {
|
|||
|
||||
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++)
|
||||
devp->compbias[i] = 0.0;
|
||||
return MSG_OK;
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -608,7 +613,8 @@ static msg_t comp_reset_bias(void *ip) {
|
|||
static msg_t comp_set_sensivity(void *ip, float *sp) {
|
||||
LSM303DLHCDriver* devp;
|
||||
uint32_t i;
|
||||
|
||||
msg_t msg = MSG_OK;
|
||||
|
||||
/* Getting parent instance pointer.*/
|
||||
devp = objGetInstance(LSM303DLHCDriver*, (BaseCompass*)ip);
|
||||
|
||||
|
@ -620,7 +626,7 @@ static msg_t comp_set_sensivity(void *ip, float *sp) {
|
|||
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
|
||||
devp->compsensitivity[i] = sp[i];
|
||||
}
|
||||
return MSG_OK;
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -1138,7 +1144,8 @@ void lsm303dlhcStop(LSM303DLHCDriver *devp) {
|
|||
uint8_t cr[2];
|
||||
osalDbgCheck(devp != NULL);
|
||||
|
||||
osalDbgAssert((devp->state == LSM303DLHC_STOP) || (devp->state == LSM303DLHC_READY),
|
||||
osalDbgAssert((devp->state == LSM303DLHC_STOP) ||
|
||||
(devp->state == LSM303DLHC_READY),
|
||||
"lsm303dlhcStop(), invalid state");
|
||||
|
||||
if (devp->state == LSM303DLHC_READY) {
|
||||
|
|
|
@ -287,11 +287,8 @@
|
|||
#define LSM303DLHC_MR_REG_M_MASK 0x03
|
||||
#define LSM303DLHC_MR_REG_M_MD0 (1 << 0)
|
||||
#define LSM303DLHC_MR_REG_M_MD1 (1 << 1)
|
||||
|
||||
/** @} */
|
||||
|
||||
//TODO: ADD more LSM303DLHC register bits definitions
|
||||
|
||||
/*===========================================================================*/
|
||||
/* Driver pre-compile time settings. */
|
||||
/*===========================================================================*/
|
||||
|
@ -500,7 +497,7 @@ typedef struct {
|
|||
*/
|
||||
float *accsensitivity;
|
||||
/**
|
||||
* @brief LSM303DLHC initial bias.
|
||||
* @brief LSM303DLHC accelerometer subsystem initial bias.
|
||||
*/
|
||||
float *accbias;
|
||||
/**
|
||||
|
@ -554,7 +551,7 @@ typedef struct {
|
|||
} LSM303DLHCConfig;
|
||||
|
||||
/**
|
||||
* @brief @p LSM303DLHC subsystem specific methods.
|
||||
* @brief @p LSM303DLHC specific methods.
|
||||
*/
|
||||
#define _lsm303dlhc_methods_alone \
|
||||
/* Change full scale value of LSM303DLHC accelerometer subsystem .*/ \
|
||||
|
@ -565,7 +562,7 @@ typedef struct {
|
|||
lsm303dlhc_comp_fs_t fs); \
|
||||
|
||||
/**
|
||||
* @brief @p LSM303DLHC subsystem specific methods with inherited ones.
|
||||
* @brief @p LSM303DLHC specific methods with inherited ones.
|
||||
*/
|
||||
#define _lsm303dlhc_methods \
|
||||
_base_object_methods \
|
||||
|
@ -589,21 +586,21 @@ struct LSM303DLHCVMT {
|
|||
lsm303dlhc_state_t state; \
|
||||
/* Current configuration data.*/ \
|
||||
const LSM303DLHCConfig *config; \
|
||||
/* Axes number of the accelerometer subsystem.*/ \
|
||||
/* Accelerometer subsystem axes number.*/ \
|
||||
size_t accaxes; \
|
||||
/* Axes number of the compass subsystem.*/ \
|
||||
size_t compaxes; \
|
||||
/* Current accelerometer sensitivity.*/ \
|
||||
/* Accelerometer subsystem current sensitivity.*/ \
|
||||
float accsensitivity[LSM303DLHC_ACC_NUMBER_OF_AXES]; \
|
||||
/* Accelerometer bias data.*/ \
|
||||
/* Accelerometer subsystem current bias .*/ \
|
||||
float accbias[LSM303DLHC_ACC_NUMBER_OF_AXES]; \
|
||||
/* Current accelerometer full scale value.*/ \
|
||||
/* Accelerometer subsystem current full scale value.*/ \
|
||||
float accfullscale; \
|
||||
/* Current compass sensitivity.*/ \
|
||||
/* Compass subsystem axes number.*/ \
|
||||
size_t compaxes; \
|
||||
/* Compass subsystem current sensitivity.*/ \
|
||||
float compsensitivity[LSM303DLHC_COMP_NUMBER_OF_AXES];\
|
||||
/* Compass bias data.*/ \
|
||||
/* Compass subsystem current bias.*/ \
|
||||
float compbias[LSM303DLHC_COMP_NUMBER_OF_AXES]; \
|
||||
/* Current compass full scale value.*/ \
|
||||
/* Compass subsystem current full scale value.*/ \
|
||||
float compfullscale;
|
||||
|
||||
/**
|
||||
|
@ -612,9 +609,9 @@ struct LSM303DLHCVMT {
|
|||
struct LSM303DLHCDriver {
|
||||
/** @brief Virtual Methods Table.*/
|
||||
const struct LSM303DLHCVMT *vmt;
|
||||
/** @brief Accelerometer interface.*/
|
||||
/** @brief Base accelerometer interface.*/
|
||||
BaseAccelerometer acc_if;
|
||||
/** @brief Compass interface.*/
|
||||
/** @brief Base compass interface.*/
|
||||
BaseCompass comp_if;
|
||||
_lsm303dlhc_data
|
||||
};
|
||||
|
@ -623,10 +620,11 @@ struct LSM303DLHCDriver {
|
|||
/*===========================================================================*/
|
||||
/* Driver macros. */
|
||||
/*===========================================================================*/
|
||||
|
||||
/**
|
||||
* @brief Return the number of axes of the BaseAccelerometer.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseAccelerometer interface
|
||||
* @param[in] devp pointer to @p LSM303DLHCDriver.
|
||||
*
|
||||
* @return the number of axes.
|
||||
*
|
||||
|
@ -642,7 +640,7 @@ struct LSM303DLHCDriver {
|
|||
* @note The axes array must be at least the same size of the
|
||||
* BaseAccelerometer axes number.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseAccelerometer interface.
|
||||
* @param[in] devp pointer to @p LSM303DLHCDriver.
|
||||
* @param[out] axes a buffer which would be filled with raw data.
|
||||
*
|
||||
* @return The operation status.
|
||||
|
@ -664,7 +662,7 @@ struct LSM303DLHCDriver {
|
|||
* @note The axes array must be at least the same size of the
|
||||
* BaseAccelerometer axes number.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseAccelerometer interface.
|
||||
* @param[in] devp pointer to @p LSM303DLHCDriver.
|
||||
* @param[out] axes a buffer which would be filled with cooked data.
|
||||
*
|
||||
* @return The operation status.
|
||||
|
@ -684,7 +682,7 @@ struct LSM303DLHCDriver {
|
|||
* @note The bias buffer must be at least the same size of the
|
||||
* BaseAccelerometer axes number.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseAccelerometer interface.
|
||||
* @param[in] devp pointer to @p LSM303DLHCDriver.
|
||||
* @param[in] bp a buffer which contains biases.
|
||||
*
|
||||
* @return The operation status.
|
||||
|
@ -700,7 +698,7 @@ struct LSM303DLHCDriver {
|
|||
* @note Default biases value are obtained from device datasheet when
|
||||
* available otherwise they are considered zero.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseAccelerometer interface.
|
||||
* @param[in] devp pointer to @p LSM303DLHCDriver.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
|
@ -716,7 +714,7 @@ struct LSM303DLHCDriver {
|
|||
* @note The sensitivity buffer must be at least the same size of the
|
||||
* BaseAccelerometer axes number.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseAccelerometer interface.
|
||||
* @param[in] devp pointer to @p LSM303DLHCDriver.
|
||||
* @param[in] sp a buffer which contains sensitivities.
|
||||
*
|
||||
* @return The operation status.
|
||||
|
@ -731,7 +729,7 @@ struct LSM303DLHCDriver {
|
|||
* @brief Reset sensitivity values for the BaseAccelerometer.
|
||||
* @note Default sensitivities value are obtained from device datasheet.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseAccelerometer interface.
|
||||
* @param[in] devp pointer to @p LSM303DLHCDriver.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
|
@ -748,7 +746,7 @@ struct LSM303DLHCDriver {
|
|||
* previous and next fullscale value.
|
||||
* @note A recalibration is highly suggested after calling this function.
|
||||
*
|
||||
* @param[in] ip pointer to @p LSM303DLHCDriver interface.
|
||||
* @param[in] devp pointer to @p LSM303DLHCDriver.
|
||||
* @param[in] fs new fullscale value.
|
||||
*
|
||||
* @return The operation status.
|
||||
|
@ -763,7 +761,7 @@ struct LSM303DLHCDriver {
|
|||
/**
|
||||
* @brief Return the number of axes of the BaseCompass.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseCompass interface
|
||||
* @param[in] devp pointer to @p LSM303DLHCDriver.
|
||||
*
|
||||
* @return the number of axes.
|
||||
*
|
||||
|
@ -837,7 +835,7 @@ struct LSM303DLHCDriver {
|
|||
* @note Default biases value are obtained from device datasheet when
|
||||
* available otherwise they are considered zero.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseCompass interface.
|
||||
* @param[in] devp pointer to @p LSM303DLHCDriver.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
|
@ -853,7 +851,7 @@ struct LSM303DLHCDriver {
|
|||
* @note The sensitivity buffer must be at least the same size of the
|
||||
* BaseCompass axes number.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseCompass interface.
|
||||
* @param[in] devp pointer to @p LSM303DLHCDriver.
|
||||
* @param[in] sp a buffer which contains sensitivities.
|
||||
*
|
||||
* @return The operation status.
|
||||
|
@ -868,7 +866,7 @@ struct LSM303DLHCDriver {
|
|||
* @brief Reset sensitivity values for the BaseCompass.
|
||||
* @note Default sensitivities value are obtained from device datasheet.
|
||||
*
|
||||
* @param[in] ip pointer to @p BaseCompass interface.
|
||||
* @param[in] devp pointer to @p LSM303DLHCDriver.
|
||||
*
|
||||
* @return The operation status.
|
||||
* @retval MSG_OK if the function succeeded.
|
||||
|
@ -885,7 +883,7 @@ struct LSM303DLHCDriver {
|
|||
* previous and next fullscale value.
|
||||
* @note A recalibration is highly suggested after calling this function.
|
||||
*
|
||||
* @param[in] ip pointer to @p LSM303DLHCDriver interface.
|
||||
* @param[in] devp pointer to @p LSM303DLHCDriver.
|
||||
* @param[in] fs new fullscale value.
|
||||
*
|
||||
* @return The operation status.
|
||||
|
@ -896,6 +894,7 @@ struct LSM303DLHCDriver {
|
|||
*/
|
||||
#define lsm303dlhcCompassSetFullScale(devp, fs) \
|
||||
(devp)->vmt->comp_set_full_scale(devp, fs)
|
||||
|
||||
/*===========================================================================*/
|
||||
/* External declarations. */
|
||||
/*===========================================================================*/
|
||||
|
|
Loading…
Reference in New Issue