From 0fd11b78ebf027a7bf13c65c8e3ff9bd71754cc6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Feb 2015 12:30:39 +0100 Subject: Fix PX4IO startup --- src/drivers/px4io/module.mk | 2 +- src/drivers/px4io/px4io.cpp | 9 ++++++++- src/drivers/px4io/px4io_serial.cpp | 11 ++++++++++- src/drivers/px4io/px4io_uploader.cpp | 2 +- src/drivers/px4io/uploader.h | 2 +- 5 files changed, 21 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index 924283356..55f803709 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cf1b89c4c..7845f8bf8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -622,8 +622,15 @@ PX4IO::init() return ret; /* get some parameters */ - unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + unsigned protocol; + hrt_abstime start_try_time = hrt_absolute_time(); + do { + usleep(2000); + protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + } while (protocol == _io_reg_get_error && (hrt_elapsed_time(&start_try_time) < 700U * 1000U)); + + /* if the error still persists after timing out, we give up */ if (protocol == _io_reg_get_error) { mavlink_and_console_log_emergency(_mavlink_fd, "Failed to communicate with IO, abort."); return -1; diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index d227e15d5..1a848cbc0 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -217,6 +217,9 @@ PX4IO_serial::~PX4IO_serial() stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); + /* Disable APB clock for the USART peripheral */ + modifyreg32(STM32_RCC_APB2ENR, RCC_APB2ENR_USART6EN, 0); + /* and kill our semaphores */ sem_destroy(&_completion_semaphore); sem_destroy(&_bus_semaphore); @@ -239,12 +242,17 @@ PX4IO_serial::~PX4IO_serial() int PX4IO_serial::init() { + /* allocate DMA */ _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) return -1; + + /* Enable the APB clock for the USART peripheral */ + modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_USART6EN); + /* configure pins for serial use */ stm32_configgpio(PX4IO_SERIAL_TX_GPIO); stm32_configgpio(PX4IO_SERIAL_RX_GPIO); @@ -258,6 +266,7 @@ PX4IO_serial::init() (void)rSR; (void)rDR; + /* configure line speed */ uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); uint32_t mantissa = usartdiv32 >> 5; diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 02e527695..8f98ac92d 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index e17523413..def10c039 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions -- cgit v1.2.3