diff --git a/fw/rbcx-coprocessor/include/Bsp.hpp b/fw/rbcx-coprocessor/include/Bsp.hpp index a2064963..b19dfaed 100644 --- a/fw/rbcx-coprocessor/include/Bsp.hpp +++ b/fw/rbcx-coprocessor/include/Bsp.hpp @@ -20,6 +20,66 @@ typedef uint32_t ADC_rank_t; inline const IRQn_Type usbLpIRQn = USB_LP_CAN1_RX0_IRQn; inline constexpr unsigned usbLpIRQnPrio = 8; +inline void clocksInit() { + RCC_OscInitTypeDef RCC_OscInitStruct {}; + RCC_ClkInitTypeDef RCC_ClkInitStruct {}; + RCC_PeriphCLKInitTypeDef PeriphClkInit {}; + + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; + RCC_OscInitStruct.HSEState = RCC_HSE_ON; + RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1; + RCC_OscInitStruct.HSIState = RCC_HSI_ON; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; + RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { + abort(); + } + + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK + | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) { + abort(); + } + + PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USB | RCC_PERIPHCLK_ADC; + PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_PLL_DIV1_5; + PeriphClkInit.AdcClockSelection = RCC_CFGR_ADCPRE_DIV6; + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) { + abort(); + } + + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOD_CLK_ENABLE(); + __HAL_RCC_GPIOE_CLK_ENABLE(); + __HAL_RCC_AFIO_CLK_ENABLE(); + __HAL_RCC_USART1_CLK_ENABLE(); + __HAL_RCC_USART2_CLK_ENABLE(); + __HAL_RCC_USART3_CLK_ENABLE(); + __HAL_RCC_UART4_CLK_ENABLE(); + __HAL_RCC_UART5_CLK_ENABLE(); + __HAL_RCC_DMA1_CLK_ENABLE(); + __HAL_RCC_DMA2_CLK_ENABLE(); + __HAL_RCC_PWR_CLK_ENABLE(); + __HAL_RCC_BKP_CLK_ENABLE(); + __HAL_RCC_RTC_ENABLE(); + __HAL_RCC_TIM1_CLK_ENABLE(); + __HAL_RCC_TIM2_CLK_ENABLE(); + __HAL_RCC_TIM3_CLK_ENABLE(); + __HAL_RCC_TIM4_CLK_ENABLE(); + __HAL_RCC_TIM5_CLK_ENABLE(); + __HAL_RCC_TIM6_CLK_ENABLE(); + __HAL_RCC_TIM7_CLK_ENABLE(); + __HAL_RCC_TIM8_CLK_ENABLE(); + __HAL_RCC_ADC1_CLK_ENABLE(); +} + inline void pinInit(GPIO_TypeDef* port, uint32_t pinMask, uint32_t mode, uint32_t pull, uint32_t speed, bool deInitFirst = false) { diff --git a/fw/rbcx-coprocessor/include/Bsp_v10.hpp b/fw/rbcx-coprocessor/include/Bsp_v10.hpp index 0f016815..3811076a 100644 --- a/fw/rbcx-coprocessor/include/Bsp_v10.hpp +++ b/fw/rbcx-coprocessor/include/Bsp_v10.hpp @@ -162,65 +162,6 @@ inline const IRQn_Type auxiliaryAndMotorAdcIRQn = ADC1_2_IRQn; inline const unsigned auxiliaryAndMotorAdcIrqPrio = 9; #define AUXILIARY_AND_MOTOR_ADC_IRQ_HANDLER ADC1_2_IRQHandler -inline void clocksInit() { - RCC_OscInitTypeDef RCC_OscInitStruct; - RCC_ClkInitTypeDef RCC_ClkInitStruct; - RCC_PeriphCLKInitTypeDef PeriphClkInit; - - RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; - RCC_OscInitStruct.HSEState = RCC_HSE_ON; - RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1; - RCC_OscInitStruct.HSIState = RCC_HSI_ON; - RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; - RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; - RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9; - if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { - abort(); - } - - RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK - | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2; - RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; - RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; - RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; - RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; - if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) { - abort(); - } - - PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USB; - PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_PLL_DIV1_5; - PeriphClkInit.AdcClockSelection = RCC_CFGR_ADCPRE_DIV6; - if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) { - abort(); - } - - __HAL_RCC_GPIOA_CLK_ENABLE(); - __HAL_RCC_GPIOB_CLK_ENABLE(); - __HAL_RCC_GPIOC_CLK_ENABLE(); - __HAL_RCC_GPIOD_CLK_ENABLE(); - __HAL_RCC_GPIOE_CLK_ENABLE(); - __HAL_RCC_AFIO_CLK_ENABLE(); - __HAL_RCC_USART1_CLK_ENABLE(); - __HAL_RCC_USART2_CLK_ENABLE(); - __HAL_RCC_USART3_CLK_ENABLE(); - __HAL_RCC_UART4_CLK_ENABLE(); - __HAL_RCC_UART5_CLK_ENABLE(); - __HAL_RCC_DMA1_CLK_ENABLE(); - __HAL_RCC_DMA2_CLK_ENABLE(); - __HAL_RCC_PWR_CLK_ENABLE(); - __HAL_RCC_BKP_CLK_ENABLE(); - __HAL_RCC_TIM1_CLK_ENABLE(); - __HAL_RCC_TIM2_CLK_ENABLE(); - __HAL_RCC_TIM3_CLK_ENABLE(); - __HAL_RCC_TIM4_CLK_ENABLE(); - __HAL_RCC_TIM5_CLK_ENABLE(); - __HAL_RCC_TIM6_CLK_ENABLE(); - __HAL_RCC_TIM7_CLK_ENABLE(); - __HAL_RCC_TIM8_CLK_ENABLE(); - __HAL_RCC_ADC1_CLK_ENABLE(); -} - // Set-up ESP32 strapping pins for the normal mode functions. Esp32Manager // handles the strapping process and ESP32 reset, and calls this function // after the reset is done so that normal function can be restored. @@ -237,10 +178,10 @@ inline void reinitEspStrappingPins() { } inline void pinsInit() { - pinInit(ledPins, GPIO_MODE_OUTPUT_PP, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW); + // pinInit(ledPins, GPIO_MODE_OUTPUT_PP, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW); - pinWrite(powerPin, 1); - pinInit(powerPin, GPIO_MODE_OUTPUT_OD, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW); + // pinWrite(powerPin, 1); + // pinInit(powerPin, GPIO_MODE_OUTPUT_OD, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW); for (auto button : buttonPin) pinInit(button, GPIO_MODE_INPUT, GPIO_PULLUP, GPIO_SPEED_FREQ_LOW); diff --git a/fw/rbcx-coprocessor/include/Bsp_v11.hpp b/fw/rbcx-coprocessor/include/Bsp_v11.hpp index 7c12cd87..eca95121 100644 --- a/fw/rbcx-coprocessor/include/Bsp_v11.hpp +++ b/fw/rbcx-coprocessor/include/Bsp_v11.hpp @@ -70,11 +70,11 @@ inline const PinDef userUartTxPin = std::make_pair(GPIOB, GPIO_PIN_6); inline const PinDef userUartRxPin = std::make_pair(GPIOB, GPIO_PIN_7); inline const PinDef tunnelUartTxPin = std::make_pair(GPIOD, GPIO_PIN_5); inline const PinDef tunnelUartRxPin = std::make_pair(GPIOD, GPIO_PIN_6); -inline const PinDef controlUartTxPin = std::make_pair(GPIOB, GPIO_PIN_10); -inline const PinDef controlUartRxPin = std::make_pair(GPIOB, GPIO_PIN_11); inline const PinDef debugUartTxPin = std::make_pair(GPIOC, GPIO_PIN_10); inline const PinDef debugUartRxPin = std::make_pair(GPIOC, GPIO_PIN_11); inline const PinDef servoUartTxRxPin = std::make_pair(GPIOC, GPIO_PIN_12); +inline const PinDef controlUartTxPin = std::make_pair(GPIOB, GPIO_PIN_10); +inline const PinDef controlUartRxPin = std::make_pair(GPIOB, GPIO_PIN_11); inline USART_TypeDef* const userUart = USART1; inline USART_TypeDef* const tunnelUart = USART2; @@ -159,65 +159,6 @@ inline const IRQn_Type auxiliaryAndMotorAdcIRQn = ADC1_2_IRQn; inline const unsigned auxiliaryAndMotorAdcIrqPrio = 9; #define AUXILIARY_AND_MOTOR_ADC_IRQ_HANDLER ADC1_2_IRQHandler -inline void clocksInit() { - RCC_OscInitTypeDef RCC_OscInitStruct; - RCC_ClkInitTypeDef RCC_ClkInitStruct; - RCC_PeriphCLKInitTypeDef PeriphClkInit; - - RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; - RCC_OscInitStruct.HSEState = RCC_HSE_ON; - RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1; - RCC_OscInitStruct.HSIState = RCC_HSI_ON; - RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; - RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; - RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9; - if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { - abort(); - } - - RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK - | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2; - RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; - RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; - RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; - RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; - if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) { - abort(); - } - - PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USB; - PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_PLL_DIV1_5; - PeriphClkInit.AdcClockSelection = RCC_CFGR_ADCPRE_DIV6; - if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) { - abort(); - } - - __HAL_RCC_GPIOA_CLK_ENABLE(); - __HAL_RCC_GPIOB_CLK_ENABLE(); - __HAL_RCC_GPIOC_CLK_ENABLE(); - __HAL_RCC_GPIOD_CLK_ENABLE(); - __HAL_RCC_GPIOE_CLK_ENABLE(); - __HAL_RCC_AFIO_CLK_ENABLE(); - __HAL_RCC_USART1_CLK_ENABLE(); - __HAL_RCC_USART2_CLK_ENABLE(); - __HAL_RCC_USART3_CLK_ENABLE(); - __HAL_RCC_UART4_CLK_ENABLE(); - __HAL_RCC_UART5_CLK_ENABLE(); - __HAL_RCC_DMA1_CLK_ENABLE(); - __HAL_RCC_DMA2_CLK_ENABLE(); - __HAL_RCC_PWR_CLK_ENABLE(); - __HAL_RCC_BKP_CLK_ENABLE(); - __HAL_RCC_TIM1_CLK_ENABLE(); - __HAL_RCC_TIM2_CLK_ENABLE(); - __HAL_RCC_TIM3_CLK_ENABLE(); - __HAL_RCC_TIM4_CLK_ENABLE(); - __HAL_RCC_TIM5_CLK_ENABLE(); - __HAL_RCC_TIM6_CLK_ENABLE(); - __HAL_RCC_TIM7_CLK_ENABLE(); - __HAL_RCC_TIM8_CLK_ENABLE(); - __HAL_RCC_ADC1_CLK_ENABLE(); -} - // Set-up ESP32 strapping pins for the normal mode functions. Esp32Manager // handles the strapping process and ESP32 reset, and calls this function // after the reset is done so that normal function can be restored. @@ -234,10 +175,10 @@ inline void reinitEspStrappingPins() { } inline void pinsInit() { - pinInit(ledPins, GPIO_MODE_OUTPUT_PP, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW); + // pinInit(ledPins, GPIO_MODE_OUTPUT_PP, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW); - pinWrite(powerPin, 1); - pinInit(powerPin, GPIO_MODE_OUTPUT_OD, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW); + // pinWrite(powerPin, 1); + // pinInit(powerPin, GPIO_MODE_OUTPUT_OD, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW); for (auto button : buttonPin) pinInit(button, GPIO_MODE_INPUT, GPIO_PULLUP, GPIO_SPEED_FREQ_LOW); diff --git a/fw/rbcx-coprocessor/include/Power.hpp b/fw/rbcx-coprocessor/include/Power.hpp index 5ed1e483..b7238d8e 100644 --- a/fw/rbcx-coprocessor/include/Power.hpp +++ b/fw/rbcx-coprocessor/include/Power.hpp @@ -1,5 +1,6 @@ #pragma once +void powerEarlyInit(); void powerInit(); void powerPoll(); diff --git a/fw/rbcx-coprocessor/include/RtcController.hpp b/fw/rbcx-coprocessor/include/RtcController.hpp new file mode 100644 index 00000000..626d2c45 --- /dev/null +++ b/fw/rbcx-coprocessor/include/RtcController.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include "rbcx.pb.h" + +#include + +void rtcInit(); +bool rtcInitReady(); +void rtcDispatch(const CoprocReq_RtcReq& req); + +uint32_t rtcGetTime(); +void rtcSetTime(uint32_t seconds); +uint32_t rtcGetAlarm(); +void rtcSetAlarm(uint32_t seconds); diff --git a/fw/rbcx-coprocessor/platformio.ini b/fw/rbcx-coprocessor/platformio.ini index 1dc1ddf8..87ce69ba 100644 --- a/fw/rbcx-coprocessor/platformio.ini +++ b/fw/rbcx-coprocessor/platformio.ini @@ -17,7 +17,7 @@ platform = ststm32@~7.0.0 board = genericSTM32F103VC framework = stm32cube lib_deps = - https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/039f2ae62d14ca7d05cdb194439514a92bbaaa78.zip + https://github.com/RoboticsBrno/RB3204-RBCX-coproc-comm/archive/81dae0effc201174b9e55c9702d58f80d51b720b.zip build_flags = -Iinclude diff --git a/fw/rbcx-coprocessor/src/DebugLink.cpp b/fw/rbcx-coprocessor/src/DebugLink.cpp index 86b15b8a..73896b0c 100644 --- a/fw/rbcx-coprocessor/src/DebugLink.cpp +++ b/fw/rbcx-coprocessor/src/DebugLink.cpp @@ -14,6 +14,7 @@ #include "BuzzerController.hpp" #include "Dispatcher.hpp" #include "Power.hpp" +#include "RtcController.hpp" #include "UsbCdcLink.h" #include "coproc_codec.h" #include "coproc_link_parser.h" @@ -322,6 +323,35 @@ static void debugLinkHandleCommand(const char* cmd) { }); }); + COMMAND("rtc", { + COMMAND("get", { + printf("RTC seconds: %lu\n", rtcGetTime()); + return; + }); + COMMAND("set", { + uint32_t val = 0; + if (sscanf(cmd, "%lu", &val) != 1) { + printf("Invalid parameters!\n"); + return; + } + rtcSetTime(val); + return; + }); + COMMAND("alarm get", { + printf("RTC alarm: %lu\n", rtcGetAlarm()); + return; + }); + COMMAND("alarm set", { + uint32_t val = 0; + if (sscanf(cmd, "%lu", &val) != 1) { + printf("Invalid parameters!\n"); + return; + } + rtcSetAlarm(val); + return; + }); + }); + printf("Invalid command.\n"); } diff --git a/fw/rbcx-coprocessor/src/Dispatcher.cpp b/fw/rbcx-coprocessor/src/Dispatcher.cpp index 5f078939..fa36db39 100644 --- a/fw/rbcx-coprocessor/src/Dispatcher.cpp +++ b/fw/rbcx-coprocessor/src/Dispatcher.cpp @@ -8,6 +8,7 @@ #include "Esp32Manager.hpp" #include "MotorController.hpp" #include "Power.hpp" +#include "RtcController.hpp" #include "StupidServoController.hpp" #include "UltrasoundController.hpp" #include "queue.h" @@ -97,6 +98,9 @@ static void dispatcherProcessReq(const CoprocReq& request) { case CoprocReq_motorReq_tag: motorDispatch(request.payload.motorReq); break; + case CoprocReq_rtcReq_tag: + rtcDispatch(request.payload.rtcReq); + break; } } diff --git a/fw/rbcx-coprocessor/src/Power.cpp b/fw/rbcx-coprocessor/src/Power.cpp index 3aacd7bc..0a028abc 100644 --- a/fw/rbcx-coprocessor/src/Power.cpp +++ b/fw/rbcx-coprocessor/src/Power.cpp @@ -1,10 +1,13 @@ #include "stm32f1xx_ll_adc.h" +#include "stm32f1xx_ll_pwr.h" #include "stm32f1xx_ll_rcc.h" +#include "stm32f1xx_ll_rtc.h" + +#include "Power.hpp" #include "Bsp.hpp" #include "BuzzerController.hpp" #include "Dispatcher.hpp" -#include "Power.hpp" #include "utils/Debug.hpp" #include "utils/Flash.hpp" #include "utils/TaskWrapper.hpp" @@ -94,6 +97,65 @@ static void loadCalibration() { calib.tempTypicalMv, calib.tempTypicalAtC); } +extern "C" void PVD_IRQHandler() { + // Enable ALARM -> powerPin override. + // Will generate a high pulse on alarm match, other times pulls power low. + LL_RTC_SetOutputSource(BKP, LL_RTC_CALIB_OUTPUT_ALARM); + + __disable_irq(); + + pinWrite(powerPin, 0); + + // Blink red LED "pretty fast" + // Only blink for a limited time because we don't want to cause a denial of service + // if we're powered from a different source (which?) + uint32_t leds = 0; + for (int blink = 0; blink < 10; blink++) { + leds ^= CoprocReq_LedsEnum_L3; + setLeds(leds); + + for (volatile int i = 0; i < 200000; i++) + ; + } + + // Reset program if power still present (powered from elsewhere) + HAL_NVIC_SystemReset(); +} + +void powerEarlyInit() { + pinWrite(powerPin, true); + __HAL_RCC_PWR_CLK_ENABLE(); + __HAL_RCC_BKP_CLK_ENABLE(); + + // Enable PVD - we want EXTI16(PVD_IRQn) interrupt when VDD drops. + // Enabled before disabling ALARM output to eliminate race + // that could result in ALARM deactivation. + LL_PWR_EnableBkUpAccess(); + // Level 7 is the highest and seems too intolerant. + LL_PWR_SetPVDLevel(LL_PWR_PVDLEVEL_4); + LL_PWR_EnablePVD(); + + setLeds(0x1); + + // Attempt to survive a transient power-up period when PVD output may fluctuate. + while (LL_PWR_IsActiveFlag_PVDO()) + ; + __HAL_PWR_PVD_EXTI_CLEAR_FLAG(); + __HAL_PWR_PVD_EXTI_ENABLE_RISING_EDGE(); + __HAL_PWR_PVD_EXTI_ENABLE_IT(); + + HAL_NVIC_ClearPendingIRQ(PVD_IRQn); + HAL_NVIC_SetPriority(PVD_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(PVD_IRQn); + + setLeds(0x3); + + // Disable potential RTC ALARM -> powerPin override. + // This way we keep VCC powered upon board power-up. + // This must be done early at power-up. + LL_RTC_SetOutputSource(BKP, LL_RTC_CALIB_OUTPUT_NONE); +} + void powerInit() { pinInit( batteryVoltagePin, GPIO_MODE_ANALOG, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW); @@ -264,19 +326,7 @@ void powerShutDown() { vTaskDelay(1); - __disable_irq(); - - pinWrite(powerPin, 0); - - // Blink red LED "pretty fast" - uint32_t leds = 0; - while (true) { - leds ^= CoprocReq_LedsEnum_L3; - setLeds(leds); - - for (volatile int i = 0; i < 200000; i++) - ; - } + HAL_NVIC_SetPendingIRQ(PVD_IRQn); } static void checkBatteryVoltage(uint16_t vccMv) { diff --git a/fw/rbcx-coprocessor/src/RtcController.cpp b/fw/rbcx-coprocessor/src/RtcController.cpp new file mode 100644 index 00000000..facaed72 --- /dev/null +++ b/fw/rbcx-coprocessor/src/RtcController.cpp @@ -0,0 +1,101 @@ +#include "RtcController.hpp" + +#include "ControlLink.hpp" +#include "utils/Debug.hpp" + +#include "stm32f1xx_ll_pwr.h" +#include "stm32f1xx_ll_rcc.h" +#include "stm32f1xx_ll_rtc.h" + +static void ensureInitialized() { + // We use DR3 to indicate that RTC was already initialized in a prior life. + bool initialized = LL_RTC_BKP_GetRegister(BKP, LL_RTC_BKP_DR3); + if (initialized) { + // RTC clock selected signifies RTC already initialized. + return; + } + + while (!LL_RCC_LSE_IsReady()) { + } + + LL_RTC_InitTypeDef init = { + .AsynchPrescaler = 0x7FFF, // 32kHz crystal -> 1s tick + .OutPutSource = LL_RTC_CALIB_OUTPUT_NONE, + }; + + LL_RCC_EnableRTC(); + LL_RCC_SetRTCClockSource(LL_RCC_RTC_CLKSOURCE_LSE); + LL_RTC_Init(RTC, &init); + LL_RTC_BKP_SetRegister(BKP, LL_RTC_BKP_DR3, 0x1); +} + +void rtcInit() { + LL_PWR_EnableBkUpAccess(); + + // Make sure the 32kHz crystal is started. + // We don't wait for LSERDY as it can take several seconds. + LL_RCC_LSE_Enable(); +} + +bool rtcInitReady() { return LL_RCC_LSE_IsReady(); } + +uint32_t rtcFlags() { + return (!rtcInitReady()) | (LL_RTC_IsActiveFlag_ALR(RTC) << 1); +} + +static void sendStatus() { + CoprocStat status; + status.which_payload = CoprocStat_rtcStat_tag, + status.payload.rtcStat = CoprocStat_RtcStat { + .time = rtcGetTime(), + .alarm = rtcGetAlarm(), + .flags = CoprocStat_RtcFlags(rtcFlags()), + }; + controlLinkTx(status); +} + +void rtcDispatch(const CoprocReq_RtcReq& req) { + switch (req.which_rtcCmd) { + case CoprocReq_RtcReq_get_tag: + sendStatus(); + break; + case CoprocReq_RtcReq_setTime_tag: + if (rtcInitReady()) { + rtcSetTime(req.rtcCmd.setTime); + } + sendStatus(); + break; + case CoprocReq_RtcReq_setAlarm_tag: + if (rtcInitReady()) { + rtcSetAlarm(req.rtcCmd.setAlarm); + } + sendStatus(); + break; + } +} + +uint32_t rtcGetTime() { return LL_RTC_TIME_Get(RTC); } + +void rtcSetTime(uint32_t seconds) { + ensureInitialized(); + LL_RTC_TIME_SetCounter(RTC, seconds); +} + +uint32_t rtcGetAlarm() { + uint32_t seconds = 0; + seconds |= LL_RTC_BKP_GetRegister(BKP, LL_RTC_BKP_DR1); + seconds |= LL_RTC_BKP_GetRegister(BKP, LL_RTC_BKP_DR2) << 16; + return seconds; +} + +void rtcSetAlarm(uint32_t seconds) { + ensureInitialized(); + LL_RTC_ClearFlag_ALR(RTC); + LL_RTC_ALARM_SetCounter(RTC, seconds); + + // We mirror the value into DR1 and DR2 16-bit regs + // because ALARM is write-only and LL_RTC_ALARM_Get + // returns garbage after reset. + LL_RTC_BKP_SetRegister(BKP, LL_RTC_BKP_DR1, seconds); + LL_RTC_BKP_SetRegister(BKP, LL_RTC_BKP_DR2, seconds >> 16); +} diff --git a/fw/rbcx-coprocessor/src/main.cpp b/fw/rbcx-coprocessor/src/main.cpp index 83658eda..b924d134 100644 --- a/fw/rbcx-coprocessor/src/main.cpp +++ b/fw/rbcx-coprocessor/src/main.cpp @@ -17,24 +17,33 @@ #include "Dispatcher.hpp" #include "Esp32Manager.hpp" #include "Power.hpp" +#include "RtcController.hpp" #include "UsbCdcLink.h" static TaskWrapper<3072> mainTask; int main() { - clocksInit(); - HAL_Init(); - #ifdef RBCX_VECT_TAB_OFFSET SCB->VTOR = FLASH_BASE | RBCX_VECT_TAB_OFFSET; #endif + // Allow POWER and LEDs drive in powerEarlyInit + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOC_CLK_ENABLE(); + pinInit(powerPin, GPIO_MODE_OUTPUT_PP, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW); + pinInit(ledPins, GPIO_MODE_OUTPUT_PP, GPIO_NOPULL, GPIO_SPEED_FREQ_LOW); + + powerEarlyInit(); + clocksInit(); + HAL_Init(); + pinsInit(); mainTask.start("main", 1, []() { debugUartInit(); softResetInit(); powerInit(); + rtcInit(); dispatcherInit(); tunnelUartInit(); controlUartInit();