void hcsr04_init(const sonarHardware_t *initialSonarHardware)
{
gpio_config_t gpio;
EXTI_InitTypeDef EXTIInit;
sonarHardware = initialSonarHardware;
#ifdef STM32F10X
// enable AFIO for EXTI support
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
#endif
#ifdef STM32F303xC
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
#endif
// trigger pin
gpio.pin = sonarHardware->trigger_pin;
gpio.mode = Mode_Out_PP;
gpio.speed = Speed_2MHz;
gpioInit(GPIOB, &gpio);
// echo pin
gpio.pin = sonarHardware->echo_pin;
gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOB, &gpio);
#ifdef STM32F10X
// setup external interrupt on echo pin
gpioExtiLineConfig(GPIO_PortSourceGPIOB, sonarHardware->exti_pin_source);
#endif
#ifdef STM32F303xC
gpioExtiLineConfig(EXTI_PortSourceGPIOB, sonarHardware->exti_pin_source);
#endif
EXTI_ClearITPendingBit(sonarHardware->exti_line);
EXTIInit.EXTI_Line = sonarHardware->exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = sonarHardware->exti_irqn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SONAR_ECHO);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SONAR_ECHO);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance()
}
void hcsr04_set_sonar_hardware(void)
{
#if !defined(UNIT_TEST)
#ifdef STM32F10X
// enable AFIO for EXTI support
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
#endif
#ifdef STM32F303xC
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
#endif
gpio_config_t gpio;
// trigger pin
gpio.pin = sonarHcsr04Hardware.trigger_pin;
gpio.mode = Mode_Out_PP;
gpio.speed = Speed_2MHz;
gpioInit(sonarHcsr04Hardware.trigger_gpio, &gpio);
// echo pin
gpio.pin = sonarHcsr04Hardware.echo_pin;
gpio.mode = Mode_IN_FLOATING;
gpioInit(sonarHcsr04Hardware.echo_gpio, &gpio);
#ifdef STM32F10X
// setup external interrupt on echo pin
gpioExtiLineConfig(GPIO_PortSourceGPIOB, sonarHcsr04Hardware.exti_pin_source);
#endif
#ifdef STM32F303xC
gpioExtiLineConfig(EXTI_PortSourceGPIOB, sonarHcsr04Hardware.exti_pin_source);
#endif
EXTI_ClearITPendingBit(sonarHcsr04Hardware.exti_line);
EXTI_InitTypeDef EXTIInit;
EXTIInit.EXTI_Line = sonarHcsr04Hardware.exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = sonarHcsr04Hardware.exti_irqn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SONAR_ECHO);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SONAR_ECHO);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#endif
}
static void hmc5883lConfigureDataReadyInterruptHandling(void)
{
#ifdef USE_MAG_DATA_READY_SIGNAL
if (!(hmc5883Config->exti_port_source && hmc5883Config->exti_pin_source)) {
return;
}
#ifdef STM32F10X
// enable AFIO for EXTI support
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
#endif
#ifdef STM32F303xC
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
#endif
#ifdef STM32F10X
gpioExtiLineConfig(hmc5883Config->exti_port_source, hmc5883Config->exti_pin_source);
#endif
#ifdef STM32F303xC
gpioExtiLineConfig(hmc5883Config->exti_port_source, hmc5883Config->exti_pin_source);
#endif
#ifdef ENSURE_MAG_DATA_READY_IS_HIGH
uint8_t status = GPIO_ReadInputDataBit(hmc5883Config->gpioPort, hmc5883Config->gpioPin);
if (!status) {
return;
}
#endif
registerExti15_10_CallbackHandler(MAG_DATA_READY_EXTI_Handler);
EXTI_ClearITPendingBit(hmc5883Config->exti_line);
EXTI_InitTypeDef EXTIInit;
EXTIInit.EXTI_Line = hmc5883Config->exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Falling;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = hmc5883Config->exti_irqn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MAG_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MAG_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#endif
}
void configureMPUDataReadyInterruptHandling(void)
{
#ifdef USE_MPU_DATA_READY_SIGNAL
#ifdef STM32F10X
// enable AFIO for EXTI support
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
#endif
#ifdef STM32F303xC
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
#endif
#ifdef STM32F10X
gpioExtiLineConfig(mpu6000Config->exti_port_source, mpu6000Config->exti_pin_source);
#endif
#ifdef STM32F303xC
gpioExtiLineConfig(mpu6000Config->exti_port_source, mpu6000Config->exti_pin_source);
#endif
#ifdef ENSURE_MPU_DATA_READY_IS_LOW
uint8_t status = GPIO_ReadInputDataBit(mpu6000Config->gpioPort, mpu6000Config->gpioPin);
if (status) {
return;
}
#endif
EXTI_ClearITPendingBit(mpu6000Config->exti_line);
EXTI_InitTypeDef EXTIInit;
EXTIInit.EXTI_Line = mpu6000Config->exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = mpu6000Config->exti_irqn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MPU_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MPU_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#endif
}
void hcsr04_init(sonar_config_t config)
{
gpio_config_t gpio;
EXTI_InitTypeDef EXTIInit;
// enable AFIO for EXTI support - already done is drv_system.c
// RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph, ENABLE);
switch(config) {
case sonar_pwm56:
trigger_pin = Pin_8; // PWM5 (PB8) - 5v tolerant
echo_pin = Pin_9; // PWM6 (PB9) - 5v tolerant
exti_line = EXTI_Line9;
exti_pin_source = GPIO_PinSource9;
exti_irqn = EXTI9_5_IRQn;
break;
case sonar_rc78:
trigger_pin = Pin_0; // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
echo_pin = Pin_1; // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
exti_line = EXTI_Line1;
exti_pin_source = GPIO_PinSource1;
exti_irqn = EXTI1_IRQn;
break;
}
// tp - trigger pin
gpio.pin = trigger_pin;
gpio.mode = Mode_Out_PP;
gpio.speed = Speed_2MHz;
gpioInit(GPIOB, &gpio);
// ep - echo pin
gpio.pin = echo_pin;
gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOB, &gpio);
// setup external interrupt on echo pin
gpioExtiLineConfig(GPIO_PortSourceGPIOB, exti_pin_source);
EXTI_ClearITPendingBit(exti_line);
EXTIInit.EXTI_Line = exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_EnableIRQ(exti_irqn);
last_measurement = millis() - 60; // force 1st measurement in hcsr04_get_distance()
}
void hcsr04_init(const sonarHardware_t *initialSonarHardware)
{
gpio_config_t gpio;
EXTI_InitTypeDef EXTIInit;
sonarHardware = initialSonarHardware;
// enable AFIO for EXTI support
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
// tp - trigger pin
gpio.pin = sonarHardware->trigger_pin;
gpio.mode = Mode_Out_PP;
gpio.speed = Speed_2MHz;
gpioInit(GPIOB, &gpio);
// ep - echo pin
gpio.pin = sonarHardware->echo_pin;
gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOB, &gpio);
// setup external interrupt on echo pin
gpioExtiLineConfig(GPIO_PortSourceGPIOB, sonarHardware->exti_pin_source);
EXTI_ClearITPendingBit(sonarHardware->exti_line);
EXTIInit.EXTI_Line = sonarHardware->exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_EnableIRQ(sonarHardware->exti_irqn);
last_measurement = millis() - 60; // force 1st measurement in hcsr04_get_distance()
}
void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sonarRange)
{
sonarHardware = initialSonarHardware;
sonarRange->maxRangeCm = HCSR04_MAX_RANGE_CM;
sonarRange->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
sonarRange->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
#if !defined(UNIT_TEST)
gpio_config_t gpio;
EXTI_InitTypeDef EXTIInit;
#ifdef STM32F10X
// enable AFIO for EXTI support
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
#endif
#ifdef STM32F303xC
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
#endif
// trigger pin
gpio.pin = sonarHardware->trigger_pin;
gpio.mode = Mode_Out_PP;
gpio.speed = Speed_2MHz;
gpioInit(GPIOB, &gpio);
// echo pin
gpio.pin = sonarHardware->echo_pin;
gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOB, &gpio);
#ifdef STM32F10X
// setup external interrupt on echo pin
gpioExtiLineConfig(GPIO_PortSourceGPIOB, sonarHardware->exti_pin_source);
#endif
#ifdef STM32F303xC
gpioExtiLineConfig(EXTI_PortSourceGPIOB, sonarHardware->exti_pin_source);
#endif
EXTI_ClearITPendingBit(sonarHardware->exti_line);
EXTIInit.EXTI_Line = sonarHardware->exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = sonarHardware->exti_irqn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SONAR_ECHO);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SONAR_ECHO);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance()
#else
lastMeasurementAt = 0; // to avoid "unused" compiler warning
#endif
}
