/**
* @brief Samples bias values for the BaseGyroscope.
* @note The L3GD20 shall not be moved during the whole procedure.
* @note After this function internal bias is automatically updated.
* @note The behavior of this function depends on @p L3GD20_BIAS_ACQ_TIMES
* and @p L3GD20_BIAS_SETTLING_US.
*
* @param[in] ip pointer to @p BaseGyroscope interface.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
*/
static msg_t gyro_sample_bias(void *ip) {
L3GD20Driver* devp;
uint32_t i, j;
int32_t raw[L3GD20_GYRO_NUMBER_OF_AXES];
int32_t buff[L3GD20_GYRO_NUMBER_OF_AXES] = {0, 0, 0};
msg_t msg;
osalDbgCheck(ip != NULL);
/* Getting parent instance pointer.*/
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
osalDbgAssert((devp->state == L3GD20_READY),
"gyro_sample_bias(), invalid state");
#if L3GD20_USE_SPI
osalDbgAssert((devp->config->spip->state == SPI_READY),
"gyro_sample_bias(), channel not ready");
#endif
for(i = 0; i < L3GD20_BIAS_ACQ_TIMES; i++){
msg = gyro_read_raw(ip, raw);
if(msg != MSG_OK)
return msg;
for(j = 0; j < L3GD20_GYRO_NUMBER_OF_AXES; j++){
buff[j] += raw[j];
}
osalThreadSleepMicroseconds(L3GD20_BIAS_SETTLING_US);
}
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++){
devp->gyrobias[i] = (buff[i] / L3GD20_BIAS_ACQ_TIMES);
devp->gyrobias[i] *= devp->gyrosensitivity[i];
}
return msg;
}
static msg_t gyro_sample_bias(void *ip) {
uint32_t i, j;
int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
int32_t buff[LSM6DS0_GYRO_NUMBER_OF_AXES] = {0, 0, 0};
msg_t msg;
osalDbgCheck(ip != NULL);
osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
"sample_bias(), invalid state");
for(i = 0; i < LSM6DS0_GYRO_BIAS_ACQ_TIMES; i++){
msg = gyro_read_raw(ip, raw);
if(msg != MSG_OK)
return msg;
for(j = 0; j < LSM6DS0_GYRO_NUMBER_OF_AXES; j++){
buff[j] += raw[j];
}
osalThreadSleepMicroseconds(LSM6DS0_GYRO_BIAS_SETTLING_uS);
}
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++){
((LSM6DS0Driver *)ip)->gyrobias[i] = buff[i] / LSM6DS0_GYRO_BIAS_ACQ_TIMES;
}
return msg;
}
/**
* @brief Generate reset pulse on bus.
*
* @param[in] owp pointer to the @p onewireDriver object
*
* @return Bool flag denoting device presence.
* @retval true There is at least one device on bus.
*/
bool onewireReset(onewireDriver *owp) {
PWMDriver *pwmd;
PWMConfig *pwmcfg;
size_t mch, sch;
osalDbgCheck(NULL != owp);
osalDbgAssert(owp->reg.state == ONEWIRE_READY, "Invalid state");
/* short circuit on bus or any other device transmit data */
if (PAL_LOW == ow_read_bit(owp))
return false;
pwmd = owp->config->pwmd;
pwmcfg = owp->config->pwmcfg;
mch = owp->config->master_channel;
sch = owp->config->sample_channel;
pwmcfg->period = ONEWIRE_RESET_LOW_WIDTH + ONEWIRE_RESET_SAMPLE_WIDTH;
pwmcfg->callback = NULL;
pwmcfg->channels[mch].callback = NULL;
pwmcfg->channels[mch].mode = owp->config->pwmmode;
pwmcfg->channels[sch].callback = pwm_reset_cb;
pwmcfg->channels[sch].mode = PWM_OUTPUT_ACTIVE_LOW;
ow_bus_active(owp);
osalSysLock();
pwmEnableChannelI(pwmd, mch, ONEWIRE_RESET_LOW_WIDTH);
pwmEnableChannelI(pwmd, sch, ONEWIRE_RESET_SAMPLE_WIDTH);
pwmEnableChannelNotificationI(pwmd, sch);
osalThreadSuspendS(&owp->thread);
osalSysUnlock();
ow_bus_idle(owp);
/* wait until slave release bus to discriminate short circuit condition */
osalThreadSleepMicroseconds(500);
return (PAL_HIGH == ow_read_bit(owp)) && (true == owp->reg.slave_present);
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
palSetPadMode(GPIOB, GPIOB_PIN0, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOB, GPIOB_PIN1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOB, GPIOB_PIN4, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOB, GPIOB_PIN5, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUDR_FLOATING);
eicuStart(&EICUD3, &eicucfg);
eicuEnable(&EICUD3);
osalThreadSleepMicroseconds(10); // need to stabilize input puns
chThdCreateStatic(PulseThreadWA_LED3, sizeof(PulseThreadWA_LED3),
NORMALPRIO+1, PulseThread, &pulse_led3);
chThdCreateStatic(PulseThreadWA_LED4, sizeof(PulseThreadWA_LED4),
NORMALPRIO+1, PulseThread, &pulse_led4);
chThdCreateStatic(PulseThreadWA_LED5, sizeof(PulseThreadWA_LED5),
NORMALPRIO+1, PulseThread, &pulse_led5);
chThdCreateStatic(PulseThreadWA_LED6, sizeof(PulseThreadWA_LED6),
NORMALPRIO+1, PulseThread, &pulse_led6);
while (true) {
osalThreadSleepMilliseconds(500);
}
return 0;
}
void time_keeper_sleep_us(uint64_t microseconds)
{
osalThreadSleepMicroseconds(microseconds);
}
