From 6be7926ed33f678244bb997fcbc12ea1f3279176 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 16 Aug 2021 20:39:51 -0400 Subject: [PATCH] px4io: add watchdog - F1 iwdg:Support optional configuable init Co-authored-by: David Sidrane --- .../nuttx/src/px4/stm/stm32f1/CMakeLists.txt | 2 + .../px4/stm/stm32f1/watchdog/CMakeLists.txt | 36 +++++ .../nuttx/src/px4/stm/stm32f1/watchdog/iwdg.c | 128 ++++++++++++++++++ src/drivers/drv_watchdog.h | 1 + src/modules/px4iofirmware/CMakeLists.txt | 1 + src/modules/px4iofirmware/px4io.c | 5 + 6 files changed, 173 insertions(+) create mode 100644 platforms/nuttx/src/px4/stm/stm32f1/watchdog/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/stm/stm32f1/watchdog/iwdg.c diff --git a/platforms/nuttx/src/px4/stm/stm32f1/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f1/CMakeLists.txt index 65766e3d23..5659f4e502 100644 --- a/platforms/nuttx/src/px4/stm/stm32f1/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32f1/CMakeLists.txt @@ -36,3 +36,5 @@ add_subdirectory(../stm32_common/hrt hrt) add_subdirectory(../stm32_common/board_critmon board_critmon) add_subdirectory(../stm32_common/board_reset board_reset) add_subdirectory(../stm32_common/version version) + +add_subdirectory(watchdog) diff --git a/platforms/nuttx/src/px4/stm/stm32f1/watchdog/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f1/watchdog/CMakeLists.txt new file mode 100644 index 0000000000..3c80878d02 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f1/watchdog/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2021 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(arch_watchdog_iwdg + iwdg.c +) diff --git a/platforms/nuttx/src/px4/stm/stm32f1/watchdog/iwdg.c b/platforms/nuttx/src/px4/stm/stm32f1/watchdog/iwdg.c new file mode 100644 index 0000000000..100b55cc56 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f1/watchdog/iwdg.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include "chip.h" +#include "stm32.h" +#include +#include "nvic.h" + +/**************************************************************************** + * Name: watchdog_pet() + * + * Description: + * This function resets the Independent watchdog (IWDG) + * + * + * Input Parameters: + * none. + * + * Returned value: + * none. + * + ****************************************************************************/ + +void watchdog_pet(void) +{ + putreg32(IWDG_KR_KEY_RELOAD, STM32_IWDG_KR); +} + +/**************************************************************************** + * Name: watchdog_init_ex() + * + * Description: + * This function initialize the Independent watchdog (IWDG) + * + * + * Input Parameters: + * prescale - 0 - 7. + * reload - 0 - 0xfff. + * + * Returned value: + * none. + * + ****************************************************************************/ + +void watchdog_init_ex(int prescale, int reload) +{ + +#if defined(CONFIG_STM32_JTAG_FULL_ENABLE) || \ + defined(CONFIG_STM32_JTAG_NOJNTRST_ENABLE) || \ + defined(CONFIG_STM32_JTAG_SW_ENABLE) + putreg32(getreg32(STM32_DBGMCU_CR) | DBGMCU_CR_IWDGSTOP, STM32_DBGMCU_CR); +#endif + + /* unlock */ + + putreg32(IWDG_KR_KEY_ENABLE, STM32_IWDG_KR); + + /* Set the prescale value */ + + putreg32((prescale << IWDG_PR_SHIFT) & IWDG_PR_MASK, STM32_IWDG_PR); + + /* Set the reload value */ + + putreg32((reload << IWDG_RLR_RL_SHIFT) & IWDG_RLR_RL_MASK, STM32_IWDG_RLR); + + /* Start the watch dog */ + + putreg32(IWDG_KR_KEY_START, STM32_IWDG_KR); + + watchdog_pet(); + +} + + +/**************************************************************************** + * Name: watchdog_init() + * + * Description: + * This function initialize the Independent watchdog (IWDG) + * + * + * Input Parameters: + * none. + * + * Returned value: + * none. + * + ****************************************************************************/ + + +void watchdog_init(void) +{ + // max prescaler and max timeout ~26214.4 ms / 4 ~= 6.5 seconds + watchdog_init_ex(IWDG_PR_DIV256 >> IWDG_PR_SHIFT, IWDG_RLR_MAX >> IWDG_RLR_RL_SHIFT >> 2); +} diff --git a/src/drivers/drv_watchdog.h b/src/drivers/drv_watchdog.h index 9baeeb063d..129eee0aa6 100644 --- a/src/drivers/drv_watchdog.h +++ b/src/drivers/drv_watchdog.h @@ -45,5 +45,6 @@ __BEGIN_DECLS void watchdog_init(void); +void watchdog_init_ex(int prescale, int reload); // Optional interface void watchdog_pet(void); __END_DECLS diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt index 3b76556342..1f0ebc3f57 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -47,6 +47,7 @@ set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES px4iofirmware) target_link_libraries(px4iofirmware PUBLIC arch_io_pins + arch_watchdog_iwdg nuttx_apps nuttx_arch nuttx_c diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 520a4621fe..d0fcebc608 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -53,6 +53,7 @@ #include #include +#include #if defined(PX4IO_PERF) # include @@ -398,7 +399,11 @@ user_start(int argc, char *argv[]) uint64_t last_heartbeat_time = 0; uint64_t last_loop_time = 0; + watchdog_init(); + for (;;) { + watchdog_pet(); + dt = (hrt_absolute_time() - last_loop_time) / 1000000.0f; last_loop_time = hrt_absolute_time();