__EXPORT void up_wspeedinit()
{
// Initialize interrupts (GPIO 1-4)
stm32_gpiosetevent(GPIO_GPIO0_INPUT, true, true, false, sensor_int1);
stm32_gpiosetevent(GPIO_GPIO1_INPUT, true, true, false, sensor_int2);
stm32_gpiosetevent(GPIO_GPIO2_INPUT, true, true, false, sensor_int3);
stm32_gpiosetevent(GPIO_GPIO3_INPUT, true, true, false, sensor_int4);
// Initialize timer 5
timer = stm32_tim_init(5);
// Configure timer 5 for a frequency of 1000000 Hz (overflow after 4295 s)
STM32_TIM_SETCLOCK(timer, 1000000);
STM32_TIM_SETMODE(timer, STM32_TIM_MODE_UP | STM32_TIM_MODE_CK_INT);
// Initialize timer for filter interrupt at a interval of 5ms
timer2 = stm32_tim_init(4);
STM32_TIM_SETISR(timer2, filter_interrupt, 0);
STM32_TIM_ENABLEINT(timer2, 0);
STM32_TIM_SETPERIOD(timer2, 4096);
STM32_TIM_SETCOMPARE(timer2, 1, 5000);
STM32_TIM_SETCLOCK(timer2, 1000000); // 1000000 Hz -> period: 1us
STM32_TIM_SETMODE(timer2, STM32_TIM_MODE_UP | STM32_TIM_MODE_CK_INT);
printf("[WSPEED] initialized\n");
}
static void zcross_enable(FAR const struct zc_lowerhalf_s *lower,
zc_interrupt_t handler, FAR void *arg)
{
irqstate_t flags;
bool rising = false;
bool falling = true;
int i;
/* Start with all interrupts disabled */
flags = irqsave();
zcross_disable();
snllvdbg("handler: %p arg: %p\n", handler, arg);
if (handler)
{
g_zcrosshandler = handler;
g_zcrossarg = arg;
}
(void)stm32_gpiosetevent(GPIO_ZEROCROSS, rising, falling,
true, zcross_interrupt);
irqrestore(flags);
}
static int stm32tiny_wl_irq_attach(xcpt_t isr)
{
_info("Attach IRQ\n");
g_isr = isr;
stm32_gpiosetevent(GPIO_NRF24L01_IRQ, false, true, false, g_isr);
return OK;
}
__EXPORT int nsh_archinitialize(void)
{
/* the interruption subsystem is not initialized when stm32_boardinitialize() is called */
stm32_gpiosetevent(GPIO_FORCE_BOOTLOADER, true, false, false, _bootloader_force_pin_callback);
/* configure power supply control/sense pins */
stm32_configgpio(GPIO_VDD_5V_SENSORS_EN);
/* configure the high-resolution time/callout interface */
hrt_init();
/* configure the DMA allocator */
dma_alloc_init();
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* set up the serial DMA polling */
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
/* initial LED state */
drv_led_start();
led_off(LED_AMBER);
led_off(LED_BLUE);
/* Configure SPI-based devices */
spi1 = up_spiinitialize(1);
if (!spi1) {
message("[boot] FAILED to initialize SPI port 1\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* Default SPI1 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
return OK;
}
static void ajoy_enable(FAR const struct ajoy_lowerhalf_s *lower,
ajoy_buttonset_t press, ajoy_buttonset_t release,
ajoy_handler_t handler, FAR void *arg)
{
irqstate_t flags;
ajoy_buttonset_t either = press | release;
ajoy_buttonset_t bit;
bool rising;
bool falling;
int i;
/* Start with all interrupts disabled */
flags = enter_critical_section();
ajoy_disable();
iinfo("press: %02x release: %02x handler: %p arg: %p\n",
press, release, handler, arg);
/* If no events are indicated or if no handler is provided, then this
* must really be a request to disable interrupts.
*/
if (either && handler)
{
/* Save the new the handler and argument */
g_ajoyhandler = handler;
g_ajoyarg = arg;
/* Check each GPIO. */
for (i = 0; i < AJOY_NGPIOS; i++)
{
/* Enable interrupts on each pin that has either a press or
* release event associated with it.
*/
bit = (1 << i);
if ((either & bit) != 0)
{
/* Active low so a press corresponds to a falling edge and
* a release corresponds to a rising edge.
*/
falling = ((press & bit) != 0);
rising = ((release & bit) != 0);
iinfo("GPIO %d: rising: %d falling: %d\n",
i, rising, falling);
(void)stm32_gpiosetevent(g_joygpio[i], rising, falling,
true, ajoy_interrupt, NULL);
}
}
}
leave_critical_section(flags);
}
int stm32_sdio_initialize(void)
{
int ret;
#ifdef HAVE_NCD
/* Card detect */
bool cd_status;
/* Configure the card detect GPIO */
stm32_configgpio(GPIO_SDMMC1_NCD);
/* Register an interrupt handler for the card detect pin */
(void)stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true,
stm32_ncd_interrupt, NULL);
#endif
/* Mount the SDIO-based MMC/SD block driver */
/* First, get an instance of the SDIO interface */
finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO);
g_sdio_dev = sdio_initialize(SDIO_SLOTNO);
if (!g_sdio_dev)
{
ferr("ERROR: Failed to initialize SDIO slot %d\n", SDIO_SLOTNO);
return -ENODEV;
}
/* Now bind the SDIO interface to the MMC/SD driver */
finfo("Bind SDIO to the MMC/SD driver, minor=%d\n", SDIO_MINOR);
ret = mmcsd_slotinitialize(SDIO_MINOR, g_sdio_dev);
if (ret != OK)
{
ferr("ERROR: Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
return ret;
}
finfo("Successfully bound SDIO to the MMC/SD driver\n");
#ifdef HAVE_NCD
/* Use SD card detect pin to check if a card is g_sd_inserted */
cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD);
finfo("Card detect : %d\n", cd_status);
sdio_mediachange(g_sdio_dev, cd_status);
#else
/* Assume that the SD card is inserted. What choice do we have? */
sdio_mediachange(g_sdio_dev, true);
#endif
return OK;
}
xcpt_t up_irqio(int id, xcpt_t irqhandler)
{
xcpt_t oldhandler = NULL;
/* The following should be atomic */
if (id == 0)
{
oldhandler = stm32_gpiosetevent(GPIO_D14, true, true, true, irqhandler);
}
else if (id == 1)
{
oldhandler = stm32_gpiosetevent(GPIO_D15, true, true, true, irqhandler);
}
return oldhandler;
}
xcpt_t board_button_irq(int id, xcpt_t irqhandler)
{
xcpt_t oldhandler = NULL;
if (id == 0)
oldhandler = stm32_gpiosetevent(GPIO_BTN_0, true, true, true, irqhandler);
return oldhandler;
}
int board_button_irq(int id, xcpt_t irqhandler, FAR void *arg)
{
int ret = -EINVAL;
if (id == 0)
{
ret = stm32_gpiosetevent(GPIO_BTN_0, true, true, true, irqhandler, arg);
}
return ret;
}
xcpt_t board_button_irq(int id, xcpt_t irqhandler)
{
xcpt_t oldhandler = NULL;
if (id == BUTTON_USER)
{
oldhandler = stm32_gpiosetevent(GPIO_BTN_USER, true, true, true, irqhandler);
}
return oldhandler;
}
void board_pwr_init(int stage)
{
if (stage == 0) {
stm32_configgpio(POWER_ON_GPIO);
stm32_configgpio(KEY_AD_GPIO);
}
if (stage == 1) {
stm32_gpiosetevent(KEY_AD_GPIO, true, true, true, board_button_irq);
}
}
xcpt_t board_button_irq(int id, xcpt_t irqhandler)
{
xcpt_t oldhandler = NULL;
/* The following should be atomic */
if (id == BUTTON_USER)
{
oldhandler = stm32_gpiosetevent(GPIO_BTN, true, true, true, irqhandler);
}
return oldhandler;
}
xcpt_t board_button_irq(int id, xcpt_t irqhandler)
{
xcpt_t oldhandler = NULL;
/* The following should be atomic */
if (id >= MIN_IRQBUTTON && id <= MAX_IRQBUTTON)
{
oldhandler = stm32_gpiosetevent(g_buttons[id], true, true, true, irqhandler);
}
return oldhandler;
}
int nsh_archinitialize(void)
{
#ifdef NSH_HAVEMMCSD
int ret;
/* Card detect */
bool cd_status;
/* Configure the card detect GPIO */
stm32_configgpio(GPIO_SD_CD);
/* Register an interrupt handler for the card detect pin */
stm32_gpiosetevent(GPIO_SD_CD, true, true, true, nsh_cdinterrupt);
/* Mount the SDIO-based MMC/SD block driver */
/* First, get an instance of the SDIO interface */
message("nsh_archinitialize: Initializing SDIO slot %d\n",
CONFIG_NSH_MMCSDSLOTNO);
g_sdiodev = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
if (!g_sdiodev)
{
message("nsh_archinitialize: Failed to initialize SDIO slot %d\n",
CONFIG_NSH_MMCSDSLOTNO);
return -ENODEV;
}
/* Now bind the SDIO interface to the MMC/SD driver */
message("nsh_archinitialize: Bind SDIO to the MMC/SD driver, minor=%d\n",
CONFIG_NSH_MMCSDMINOR);
ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, g_sdiodev);
if (ret != OK)
{
message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
return ret;
}
dbg("nsh_archinitialize: Successfully bound SDIO to the MMC/SD driver\n");
/* Use SD card detect pin to check if a card is inserted */
cd_status = !stm32_gpioread(GPIO_SD_CD);
vdbg("Card detect : %hhu\n", cd_status);
sdio_mediachange(g_sdiodev, cd_status);
#endif
return OK;
}
int board_button_irq(int id, xcpt_t irqhandler, FAR void *arg)
{
int ret = -EINVAL;
/* The following should be atomic */
if (id == IRQBUTTON)
{
ret = stm32_gpiosetevent(BUTTON_BOOT0n, true, true, true, irqhandler,
arg);
}
return ret;
}
int board_button_irq(int id, xcpt_t irqhandler, FAR void *arg)
{
int ret = -EINVAL;
/* The following should be atomic */
if (id >= MIN_IRQBUTTON && id <= MAX_IRQBUTTON)
{
ret = stm32_gpiosetevent(g_buttons[id], true, true, true, irqhandler,
arg);
}
return ret;
}
static void zcross_disable(void)
{
irqstate_t flags;
int i;
/* Disable zero cross pin interrupt */
flags = irqsave();
(void)stm32_gpiosetevent(GPIO_ZEROCROSS, false, false, false, NULL);
irqrestore(flags);
/* Nullify the handler and argument */
g_zcrosshandler = NULL;
g_zcrossarg = NULL;
}
xcpt_t up_irqbutton(int id, xcpt_t irqhandler)
{
uint16_t gpio;
if (id == BUTTON_KEY1)
{
gpio = GPIO_KEY1;
}
else if (id == BUTTON_KEY2)
{
gpio = GPIO_KEY2;
}
else
{
return NULL;
}
return stm32_gpiosetevent(gpio, true, true, true, irqhandler);
}
static void ajoy_disable(void)
{
irqstate_t flags;
int i;
/* Disable each joystick interrupt */
flags = enter_critical_section();
for (i = 0; i < AJOY_NGPIOS; i++)
{
(void)stm32_gpiosetevent(g_joygpio[i], false, false, false, NULL, NULL);
}
leave_critical_section(flags);
/* Nullify the handler and argument */
g_ajoyhandler = NULL;
g_ajoyarg = NULL;
}
xcpt_t stm32_lm75attach(xcpt_t irqhandler)
{
return stm32_gpiosetevent(GPIO_LM75_OSINT, true, true, true, irqhandler);
}
xcpt_t stm32_setup_overcurrent(xcpt_t handler)
{
return stm32_gpiosetevent(GPIO_OTGFS_OVER, true, true, true, handler);
}
void up_boot_standby_mode(void)
{
uint32_t gpio_off_mask;
uint32_t regval;
int i;
DEBUGASSERT((getreg32(STM32_PWR_CR) & PWR_CR_DBP) != 0);
/*
* This function is called from up_boot.c, stm32_boardinitialize(). OS is not
* running yet. Regulators and chip-selects have been initialized. IWDG not
* enabled. If board has HWWDG, we must enable it from this function before
* going to standby mode.
*/
regval = getreg32(CONFIG_STANDBYMODE_MAGIC_BKREG);
if (regval != CONFIG_STANDBYMODE_MAGIC)
{
g_board_wakeup_from_standby = (regval == CONFIG_STANDBYMODE_MAGIC + 1);
/* Standby mode was not requested, continue with regular boot. */
putreg32(0, CONFIG_STANDBYMODE_MAGIC_BKREG);
for (i = 0; i < 4; i++)
up_lowputc('r');
return;
}
#if defined(CONFIG_STM32_DFU) || defined (CONFIG_BOARD_HALTIAN_HWWDG)
up_irqinitialize();
#endif
/* Go into standby mode. */
putreg32(CONFIG_STANDBYMODE_MAGIC + 1, CONFIG_STANDBYMODE_MAGIC_BKREG);
for (i = 0; i < 4; i++)
up_lowputc('s');
/* Setup bootloader to jump directly to firmware. */
putreg32(BOARD_FIRMWARE_BASE_ADDR, CONFIG_BOOTLOADER_ADDR_BKREG);
/* Now disable backup register access. If following interrupt handlers
* access backup registers, they need to make sure to re-enable access. */
stm32_pwr_enablebkp(false);
while (true)
{
/* Configure SDcard pins. */
gpio_initialize_sdcard_pins();
/* Configure display GPIOs. */
gpio_off_mask = ~(GPIO_PUPD_MASK | GPIO_MODE_MASK | GPIO_OUTPUT_SET);
stm32_configgpio(GPIO_CHIP_SELECT_DISPLAY);
stm32_configgpio(GPIO_LCD_SSD1309_CMDDATA);
stm32_configgpio(GPIO_LCD_SSD1309_RESET);
stm32_configgpio((GPIO_SPI2_MOSI & gpio_off_mask) | GPIO_OUTPUT_CLEAR | GPIO_OUTPUT);
stm32_configgpio((GPIO_SPI2_SCK & gpio_off_mask) | GPIO_OUTPUT_CLEAR | GPIO_OUTPUT);
/* Reconfigure GPIO's for stop mode (most pins are setup as analog input). */
up_reconfigure_gpios_for_pmstop();
/* We must kick HWWDG even from standby mode, else it resets device. */
board_wdginitialize_autokick(hwwdg_irq_in_standby);
/* Setup 'power'-button as EXTI for wake-up. */
stm32_configgpio(GPIO_BTN_POWERKEY);
stm32_gpiosetevent(GPIO_BTN_POWERKEY, true, false, true,
button_pressed_down_handler);
/* Our standby-mode is really ARM core's stop-mode.
Enter stop-mode with MCU internal regulator in low-power mode. */
(void)stm32_pmstop(true);
standby_do_trace('p');
}
}
int sif_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "init")) {
return sif_init();
}
else if (!strcmp(argv[1], "gpio") && argc == 4) {
vsn_sif.gpio[0] = atoi(argv[2]);
vsn_sif.gpio[1] = atoi(argv[3]);
sif_gpio1_update();
sif_gpio2_update();
printf("GPIO States: %2x %2x\n", vsn_sif.gpio[0], vsn_sif.gpio[1]);
return 0;
}
else if (!strcmp(argv[1], "pwr") && argc == 3) {
int val = atoi(argv[2]);
//STM32_TIM_SETCOMPARE(vsn_sif.tim8, GPIO_OUT_PWRPWM_TIM8_CH, val);
STM32_TIM_SETCOMPARE(vsn_sif.tim3, GPIO_OUT_PWM_TIM3_CH, val);
return 0;
}
else if (!strcmp(argv[1], "time") && argc == 3) {
struct timespec t_set;
t_set.tv_sec = atoi(argv[2]);
clock_settime(CLOCK_REALTIME, &t_set);
}
else if (!strcmp(argv[1], "free")) {
size_t page = 0, stpage = 0xFFFF;
ssize_t status;
do {
status = up_progmem_ispageerased(page++);
/* Is this beginning of new free space section */
if (status == 0) {
if (stpage == 0xFFFF) stpage = page-1;
}
else if (status != 0) {
if (stpage != 0xFFFF) {
printf("Free Range:\t%lu\t-\t%lu\n",
(unsigned long)stpage, (unsigned long)(page-2));
stpage = 0xFFFF;
}
}
}
while (status >= 0);
return 0;
}
else if (!strcmp(argv[1], "erase") && argc == 3) {
size_t page = atoi(argv[2]);
printf("Erase result: %d\n", up_progmem_erasepage(page));
return 0;
}
else if (!strcmp(argv[1], "flash") && argc == 3) {
size_t page = atoi(argv[2]);
size_t addr = page * up_progmem_pagesize(page);
printf("Write result: %d (writing to address %lxh)\n",
up_progmem_write(addr, "Test", 4), (unsigned long)addr);
return 0;
}
else if (!strcmp(argv[1], "i2c") && argc == 3) {
int val = atoi(argv[2]);
I2C_SETFREQUENCY(vsn_sif.i2c1, 100000);
struct lis331dl_dev_s * lis = lis331dl_init(vsn_sif.i2c1, val);
if (lis) {
const struct lis331dl_vector_s * a;
int i;
uint32_t time_stamp = clock_systimer();
/* Set to 400 Hz : 3 = 133 Hz/axis */
lis331dl_setconversion(lis, false, true);
/* Sample some values */
for (i=0; i<1000;) {
if ((a = lis331dl_getreadings(lis))) {
i++;
printf("%d %d %d\n", a->x, a->y, a->z);
}
else if (errno != 11) {
printf("Readings errno %d\n", errno);
break;
}
}
printf("Time diff = %d\n", clock_systimer() - time_stamp);
lis331dl_deinit(lis);
}
else printf("Exit point: errno=%d\n", errno);
return 0;
}
else if (!strcmp(argv[1], "pga")) {
int gain = atoi(argv[2]);
gain = vsn_muxbus_setpgagain(gain);
printf("Gain changed: %d\n", gain);
return 0;
}
else if (!strcmp(argv[1], "cc")) {
struct cc1101_dev_s * cc;
uint8_t buf[64];
int sta;
cc = cc1101_init(vsn_sif.spi2, CC1101_PIN_GDO0, GPIO_CC1101_GDO0,
&cc1101_rfsettings_ISM1_868MHzGFSK100kbps);
if (cc) {
/* Work-around: enable falling edge, event and interrupt */
stm32_gpiosetevent(GPIO_CC1101_GDO0, false, true, true, cc1101_eventcb);
/* Enable clock to ARM PLL, allowing to speed-up to 72 MHz */
cc1101_setgdo(cc, CC1101_PIN_GDO2, CC1101_GDO_CLK_XOSC3);
cc1101_setchannel(cc, 0); /* AV Test Hex, receive on that channel */
cc1101_receive(cc); /* Enter RX mode */
while (1)
{
fflush(stdout);
sta = cc1101_read(cc, buf, 64);
if (sta > 0) {
printf("Received %d bytes: rssi=%d [dBm], LQI=%d (CRC %s)\n",
sta, cc1101_calcRSSIdBm(buf[sta-2]), buf[sta-1]&0x7F,
(buf[sta-1]&0x80)?"OK":"BAD");
cc1101_write(cc, buf, 61);
cc1101_send(cc);
printf("Packet send back\n");
cc1101_receive(cc);
}
}
}
}
}
fprintf(stderr, "%s:\tinit\n\tgpio\tA B\n\tpwr\tval\n", argv[0]);
fprintf(stderr, "rtc time = %u, time / systick = %u / %u\n",
up_rtc_time(), time(NULL), clock_systimer());
return -1;
}
int board_app_initialize(uintptr_t arg)
{
int ret;
#ifdef NSH_HAVEMMCSD
/* Card detect */
bool cd_status;
/* Configure the card detect GPIO */
stm32_configgpio(GPIO_SD_CD);
/* Register an interrupt handler for the card detect pin */
(void)stm32_gpiosetevent(GPIO_SD_CD, true, true, true, nsh_cdinterrupt, NULL);
/* Mount the SDIO-based MMC/SD block driver */
/* First, get an instance of the SDIO interface */
syslog(LOG_INFO, "Initializing SDIO slot %d\n",
CONFIG_NSH_MMCSDSLOTNO);
g_sdiodev = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
if (!g_sdiodev)
{
syslog(LOG_ERR, "ERROR: Failed to initialize SDIO slot %d\n",
CONFIG_NSH_MMCSDSLOTNO);
return -ENODEV;
}
/* Now bind the SDIO interface to the MMC/SD driver */
syslog(LOG_INFO, "Bind SDIO to the MMC/SD driver, minor=%d\n",
CONFIG_NSH_MMCSDMINOR);
ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, g_sdiodev);
if (ret != OK)
{
syslog(LOG_ERR, "ERROR: Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
return ret;
}
syslog(LOG_INFO, "Successfully bound SDIO to the MMC/SD driver\n");
/* Use SD card detect pin to check if a card is inserted */
cd_status = !stm32_gpioread(GPIO_SD_CD);
_info("Card detect : %hhu\n", cd_status);
sdio_mediachange(g_sdiodev, cd_status);
#endif
#ifdef CONFIG_INPUT
/* Initialize the touchscreen */
ret = stm32_tsc_setup(0);
if (ret < 0)
{
syslog(LOG_ERR, "ERROR: stm32_tsc_setup failed: %d\n", ret);
}
#endif
UNUSED(ret);
return OK;
}
/* using the interrupt way */
void interruptGPIO()
{
gpio_fd = open(gpio_dev, O_RDWR);
float distance = 0;
hrt_abstime t,t_start;
hrt_abstime temp_t;
t = hrt_absolute_time();
t_start = t;
uint32_t falling_count_1,falling_count_2;
/*
* register to the nuttx of the gpio event
* we must set the PIN to GPIO_EXTI for external interrupt
* the GPIO_PIN4 <-> GPIO_EXT_1 and GPIO_PIN5 <-> GPIO_EXT_2
* see "board_config.h"
*/
uint32_t pinset = (GPIO_INPUT | GPIO_FLOAT | GPIO_EXTI | GPIO_PORTC | GPIO_PIN5);
xcpt_t oldhandler = stm32_gpiosetevent(pinset ,false ,true , false, GPIO_EXT_2_falling_handler);
if(oldhandler != NULL)
{
printf("WARNING: oldhandler:%p is not NULL! Button events may be lost or aliased!\n",oldhandler);
}
/* don't know is it that if ioctl is set from INPUT to OUTPUT or OUTPUT to input
* will trigger a pusle */
/* run for 50s */
while(t < t_start + 1000000 * 5)
{
/* trigger measure */
printf("measure started\n");
printf("falling_count 0 = %d\n",falling_count);
echo_start_time = hrt_absolute_time();
/*
* the GPIO_SET_OUTPUT will trigger a pulse of the GPIO_EXT_2 !!
* so we don't need to do ioctl(gpio_fd, GPIO_SET, GPIO_EXT_1)
* and ioctl(gpio_fd, GPIO_CLEAR, GPIO_EXT_1) anymore...
*
* this function takes about 20us. seems a little dangerous,
* is it possible that it won't trigger the sonar?? haven't seem yet...
*/
printf("gpio_fd = %d, GPIO_SET_OUTPUT = %d, GPIO_EXT_1 = %d\n",gpio_fd,GPIO_SET_OUTPUT,GPIO_EXT_1);
ioctl(gpio_fd, GPIO_SET_OUTPUT, GPIO_EXT_1);
falling_count_1 = falling_count;
printf("falling_count 1 = %d\n",falling_count);
/*
ioctl(gpio_fd, GPIO_SET, GPIO_EXT_1);
usleep(10); // this function takes about 2000us ,but the ioctl only takes 20us
ioctl(gpio_fd, GPIO_CLEAR, GPIO_EXT_1);
*/
/*
* must be set to GPIO_SET_INPUT, or we can't get the value of GPIO_EXT_2
* this will trigger the sonar to send a ultrasonic wave which will set the 'Echo'(GPIO_EXT_2) pin
* seems this will trigger a pusle too...
*/
printf("gpio_fd = %d, GPIO_SET_INPUT = %d, GPIO_EXT_2 = %d\n",gpio_fd,GPIO_SET_INPUT,GPIO_EXT_2);
ioctl(gpio_fd, GPIO_SET_INPUT, GPIO_EXT_2);
falling_count_2 = falling_count;
printf("falling_count 2 = %d\n",falling_count);
/*
* sleep some time for getting the interrupt.
* this determines the frequency of collecting data
*/
usleep(200000);
if(echo_stop_time < echo_start_time)
{
echo_stop_time = hrt_absolute_time();
printf("ERROR: we didn't get the falling! \n");
}
if(falling_count_1 == falling_count_2)
{
printf("SONAR ERROR: please check the SONAR or the connection!!\n");
}
distance = (float)(echo_stop_time - echo_start_time) * 170.0f / 1000000.0f;
printf("distance :%.2f m\n",distance);
printf("start :%llu , stop: %llu\n",echo_start_time,echo_stop_time);
printf("falling_count 3 = %d\n",falling_count);
t = hrt_absolute_time();
}
printf("total interrupt count :%d\n",falling_count);
}
