diff --git a/latest/Firmware/.cproject b/latest/Firmware/.cproject
index 35f25af..9622ef4 100644
--- a/latest/Firmware/.cproject
+++ b/latest/Firmware/.cproject
@@ -777,7 +777,7 @@
-
+
diff --git a/latest/Firmware/.settings/language.settings.xml b/latest/Firmware/.settings/language.settings.xml
index 4532238..6fc7965 100644
--- a/latest/Firmware/.settings/language.settings.xml
+++ b/latest/Firmware/.settings/language.settings.xml
@@ -59,7 +59,7 @@
-
+
diff --git a/latest/Firmware/Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal.c b/latest/Firmware/Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal.c
index 7f25329..c3ba1f6 100644
--- a/latest/Firmware/Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal.c
+++ b/latest/Firmware/Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal.c
@@ -256,6 +256,7 @@ __weak void HAL_MspDeInit(void)
* @param TickPriority Tick interrupt priority.
* @retval HAL status
*/
+#if 1
__weak HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
{
HAL_StatusTypeDef status = HAL_OK;
@@ -289,7 +290,7 @@ __weak HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
/* Return function status */
return status;
}
-
+#endif
/**
* @}
*/
diff --git a/latest/Firmware/FreeRTOS/portable/GCC/ARM_CM4F/port.c b/latest/Firmware/FreeRTOS/portable/GCC/ARM_CM4F/port.c
index 79e51b8..5ef2a05 100644
--- a/latest/Firmware/FreeRTOS/portable/GCC/ARM_CM4F/port.c
+++ b/latest/Firmware/FreeRTOS/portable/GCC/ARM_CM4F/port.c
@@ -486,6 +486,7 @@ void xPortPendSVHandler( void )
/*-----------------------------------------------------------*/
void xPortSysTickHandler( void )
+//void SysTick_Handler(void)
{
/* The SysTick runs at the lowest interrupt priority, so when this interrupt
executes all interrupts must be unmasked. There is therefore no need to
diff --git a/latest/Firmware/Inc/FreeRTOSConfig.h b/latest/Firmware/Inc/FreeRTOSConfig.h
index ea75609..b44da32 100644
--- a/latest/Firmware/Inc/FreeRTOSConfig.h
+++ b/latest/Firmware/Inc/FreeRTOSConfig.h
@@ -59,17 +59,20 @@
#define configSUPPORT_STATIC_ALLOCATION 1
#define configSUPPORT_DYNAMIC_ALLOCATION 1
#define configUSE_IDLE_HOOK 0
-#define configUSE_TICK_HOOK 0
+#define configUSE_TICK_HOOK 1
#define configCPU_CLOCK_HZ ( SystemCoreClock )
#define configTICK_RATE_HZ ((TickType_t)1000)
#define configMAX_PRIORITIES ( 7 )
#define configMINIMAL_STACK_SIZE ((uint16_t)128)
-#define configTOTAL_HEAP_SIZE ((size_t)3000)
+#define configTOTAL_HEAP_SIZE ((size_t)16384)
#define configMAX_TASK_NAME_LEN ( 16 )
#define configUSE_16_BIT_TICKS 0
#define configUSE_MUTEXES 1
#define configQUEUE_REGISTRY_SIZE 8
#define configUSE_PORT_OPTIMISED_TASK_SELECTION 1
+#define configCHECK_FOR_STACK_OVERFLOW 1
+
+
/* USER CODE BEGIN MESSAGE_BUFFER_LENGTH_TYPE */
/* Defaults to size_t for backward compatibility, but can be changed
if lengths will always be less than the number of bytes in a size_t. */
@@ -119,7 +122,9 @@ See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html. */
/* Normal assert() semantics without relying on the provision of an assert.h
header file. */
/* USER CODE BEGIN 1 */
-#define configASSERT( x ) if ((x) == 0) {taskDISABLE_INTERRUPTS(); for( ;; );}
+//#define configASSERT( x ) if ((x) == 0) {taskDISABLE_INTERRUPTS(); for( ;; );}
+#define configASSERT( x ) if ((x) == 0) {taskDISABLE_INTERRUPTS(); asm("BKPT 0");}
+
/* USER CODE END 1 */
/* Definitions that map the FreeRTOS port interrupt handlers to their CMSIS
diff --git a/latest/Firmware/Inc/TXPacket.hpp b/latest/Firmware/Inc/TXPacket.hpp
index 5eefe90..64950c7 100644
--- a/latest/Firmware/Inc/TXPacket.hpp
+++ b/latest/Firmware/Inc/TXPacket.hpp
@@ -54,6 +54,7 @@ public:
void configureForTesting(VHFChannel channel, uint16_t numBits);
bool canRampDown();
+ bool isTestPacket();
private:
uint8_t mPacket[MAX_AIS_TX_PACKET_SIZE/8+1];
uint16_t mSize;
diff --git a/latest/Firmware/STM32L412KBUx_FLASH.ld b/latest/Firmware/STM32L412KBUx_FLASH.ld
index 8cb8360..b82ca15 100644
--- a/latest/Firmware/STM32L412KBUx_FLASH.ld
+++ b/latest/Firmware/STM32L412KBUx_FLASH.ld
@@ -55,7 +55,7 @@ ENTRY(Reset_Handler)
/* Highest address of the user mode stack */
_estack = 0x2000A000; /* end of RAM */
/* Generate a link error if heap and stack don't fit into RAM */
-_Min_Heap_Size = 0x200; /* required amount of heap */
+_Min_Heap_Size = 0x1000; /* required amount of heap */
_Min_Stack_Size = 0x400; /* required amount of stack */
/* Specify the memory areas */
diff --git a/latest/Firmware/Src/EventQueue.cpp b/latest/Firmware/Src/EventQueue.cpp
index 2e507d0..25a364d 100644
--- a/latest/Firmware/Src/EventQueue.cpp
+++ b/latest/Firmware/Src/EventQueue.cpp
@@ -28,7 +28,7 @@
#include "queue.h"
#include "task.h"
-#define EVENT_QUEUE_SIZE 40
+#define EVENT_QUEUE_SIZE 10
static Event __queue[EVENT_QUEUE_SIZE];
@@ -40,19 +40,25 @@ EventQueue &EventQueue::instance()
EventQueue::EventQueue()
{
- mQueueHandle = xQueueCreateStatic(EVENT_QUEUE_SIZE, sizeof(Event), (uint8_t*)&__queue[0], &mQueue);
}
void EventQueue::init()
{
+ mQueueHandle = xQueueCreateStatic(EVENT_QUEUE_SIZE, sizeof(Event), (uint8_t*)&__queue[0], &mQueue);
+ configASSERT(mQueueHandle);
}
void EventQueue::push(const Event &e)
{
+ if ( xTaskGetSchedulerState() != taskSCHEDULER_RUNNING )
+ return;
+
BaseType_t xHighPriorityTaskWoken = pdFALSE;
if ( Utils::inISR() )
{
xQueueSendFromISR(mQueueHandle, &e, &xHighPriorityTaskWoken);
+ if ( xHighPriorityTaskWoken )
+ portYIELD_FROM_ISR(xHighPriorityTaskWoken);
}
else
{
diff --git a/latest/Firmware/Src/GPS.cpp b/latest/Firmware/Src/GPS.cpp
index cbaf638..d6a5a01 100644
--- a/latest/Firmware/Src/GPS.cpp
+++ b/latest/Firmware/Src/GPS.cpp
@@ -173,6 +173,7 @@ void GPS::onPPS()
EventQueue::instance ().push(event);
}
+
void GPS::processEvent(const Event &event)
{
//printf2("-> GPS::processEvent()\r\n");
diff --git a/latest/Firmware/Src/TXPacket.cpp b/latest/Firmware/Src/TXPacket.cpp
index 4bee3e3..a7905b4 100644
--- a/latest/Firmware/Src/TXPacket.cpp
+++ b/latest/Firmware/Src/TXPacket.cpp
@@ -76,6 +76,11 @@ void TXPacket::setMessageType(const char *t)
strncpy(mMessageType, t, sizeof mMessageType);
}
+bool TXPacket::isTestPacket()
+{
+ return mTestPacket;
+}
+
const char *TXPacket::messageType()
{
return mMessageType;
diff --git a/latest/Firmware/Src/Transceiver.cpp b/latest/Firmware/Src/Transceiver.cpp
index a696d85..3bf456f 100644
--- a/latest/Firmware/Src/Transceiver.cpp
+++ b/latest/Firmware/Src/Transceiver.cpp
@@ -186,12 +186,14 @@ TXPacket* Transceiver::assignedTXPacket()
return mTXPacket;
}
+/**
+ * This method is called in interrupt context
+ */
void Transceiver::onBitClock()
{
if ( gRadioState == RADIO_RECEIVING )
{
Receiver::onBitClock();
-#ifndef TX_TEST_MODE
#ifdef ENABLE_TX
/*
We start transmitting a packet if:
@@ -208,7 +210,16 @@ void Transceiver::onBitClock()
- There is no supercapacitor anymore
*/
- if ( mUTC && mTXPacket && mSlotBitNumber == CCA_SLOT_BIT+1 && mTXPacket && mTXPacket->channel() == mChannel )
+ if ( !mTXPacket )
+ return;
+
+
+ if ( mTXPacket->isTestPacket() )
+ {
+ // Test packets are sent immediately. Presumably, we're firing into a dummy load ;)
+ startTransmitting();
+ }
+ else if ( mUTC && mSlotBitNumber == CCA_SLOT_BIT+1 && mTXPacket->channel() == mChannel )
{
auto it = mNoiseFloorCache.find(mChannel);
if ( it != mNoiseFloorCache.end() )
@@ -229,12 +240,6 @@ void Transceiver::onBitClock()
}
}
}
-#endif
-#else
- // In Test Mode we don't care about RSSI, SOTDMA or anything. Presumably we're firing into a dummy load ;-) Also, we don't care about throttling.
- if ( mTXPacket ) {
- startTransmitting();
- }
#endif
}
else
@@ -258,11 +263,10 @@ void Transceiver::onBitClock()
/**
* As of September 2020, the digital ramp-down of the Si4463 is broken and not
- * in the latest firmware patch either, so this is a good alternative:
+ * in the latest firmware patch either, so this is a good alternative.
*
* The packet has 2-3 ramp down bits, so when it tells us it's reached them,
- * we ramp the PA down. There is an RC delay circuit
- * on the PA bias voltage rail, so the voltage won't "crash" hard.
+ * we ramp the bias voltage down by means of the RC delay circuit.
* This really helped clean up spurs.
*
*/
diff --git a/latest/Firmware/Src/bsp/bsp_5_2.cpp b/latest/Firmware/Src/bsp/bsp_5_2.cpp
index 7b97559..b55280b 100644
--- a/latest/Firmware/Src/bsp/bsp_5_2.cpp
+++ b/latest/Firmware/Src/bsp/bsp_5_2.cpp
@@ -78,8 +78,9 @@ static const GPIO __gpios[] = {
extern "C"
{
- void Error_Handler(void)
+ void Error_Handler(uint8_t i)
{
+
asm("BKPT 0");
//printf_serial_now("[ERROR]\r\n");
//printf_serial_now("[ERROR] ***** System error handler resetting *****\r\n");
@@ -107,20 +108,6 @@ void bsp_hw_init()
gpio_pin_init();
- // 1PPS signal
- HAL_NVIC_SetPriority(EXTI2_IRQn, 5, 0);
- HAL_NVIC_EnableIRQ(EXTI2_IRQn);
-
-
- // RF IC clock interrupts
- HAL_NVIC_SetPriority(EXTI1_IRQn, 5, 0);
- HAL_NVIC_EnableIRQ(EXTI1_IRQn);
-
-
- HAL_NVIC_SetPriority(EXTI3_IRQn, 5, 0);
- HAL_NVIC_EnableIRQ(EXTI3_IRQn);
-
-
// USART1 (main UART)
huart1.Instance = USART1;
huart1.Init.BaudRate = 38400;
@@ -134,6 +121,7 @@ void bsp_hw_init()
huart1.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
HAL_UART_Init(&huart1);
+
HAL_NVIC_SetPriority(USART1_IRQn, 6, 0);
HAL_NVIC_EnableIRQ(USART1_IRQn);
__HAL_UART_ENABLE_IT(&huart1, UART_IT_RXNE);
@@ -158,7 +146,7 @@ void bsp_hw_init()
if (HAL_SPI_Init(&hspi1) != HAL_OK)
{
- Error_Handler();
+ Error_Handler(0);
}
__HAL_SPI_ENABLE(&hspi1);
@@ -209,20 +197,34 @@ void bsp_hw_init()
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c1) != HAL_OK)
{
- Error_Handler();
+ Error_Handler(0);
}
/** Configure Analogue filter
*/
if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
{
- Error_Handler();
+ Error_Handler(0);
}
/** Configure Digital filter
*/
if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)
{
- Error_Handler();
+ Error_Handler(0);
}
+
+ // 1PPS signal
+ HAL_NVIC_SetPriority(EXTI2_IRQn, 5, 0);
+ HAL_NVIC_EnableIRQ(EXTI2_IRQn);
+
+
+ // RF IC clock interrupts
+ HAL_NVIC_SetPriority(EXTI1_IRQn, 5, 0);
+ HAL_NVIC_EnableIRQ(EXTI1_IRQn);
+
+
+ HAL_NVIC_SetPriority(EXTI3_IRQn, 5, 0);
+ HAL_NVIC_EnableIRQ(EXTI3_IRQn);
+
}
@@ -251,7 +253,7 @@ void SystemClock_Config()
RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
- Error_Handler();
+ Error_Handler(0);
}
/**Initializes the CPU, AHB and APB bus clocks
@@ -265,21 +267,21 @@ void SystemClock_Config()
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
{
- Error_Handler();
+ Error_Handler(0);
}
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USART1;
PeriphClkInit.Usart1ClockSelection = RCC_USART1CLKSOURCE_HSI;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
{
- Error_Handler();
+ Error_Handler(0);
}
/**Configure the main internal regulator output voltage
*/
if (HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1) != HAL_OK)
{
- Error_Handler();
+ Error_Handler(0);
}
/**Configure the Systick interrupt time
@@ -291,7 +293,7 @@ void SystemClock_Config()
HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
/* SysTick_IRQn interrupt configuration */
- HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
+ HAL_NVIC_SetPriority(SysTick_IRQn, 15, 0);
}
void gpio_pin_init()
diff --git a/latest/Firmware/Src/main.cpp b/latest/Firmware/Src/main.cpp
index 3958bfa..d7e9ef9 100644
--- a/latest/Firmware/Src/main.cpp
+++ b/latest/Firmware/Src/main.cpp
@@ -33,6 +33,7 @@
void fireTestPacket()
{
VHFChannel channel = CH_87;
+
if ( rand() % 2 == 0 )
channel = CH_88;
@@ -70,132 +71,88 @@ void determineCauseOfReset()
void mainTask(void *params)
{
- EventQueue::instance().init();
- //EventPool::instance().init();
- Configuration::instance().init();
- CommandProcessor::instance().init();
- DataTerminal::instance().init();
+ //uint32_t counter = 0;
- RXPacketProcessor packetProcessor;
-
- SystickTimer::instance();
-
-
-#if not defined CALIBRATION_MODE && not defined TX_TEST_MODE
- GPS::instance().init();
- GPS::instance().enable();
-#endif
-
-
-#ifdef ENABLE_TX
- TXPacketPool::instance().init();
- TXScheduler::instance().init();
-#endif
-
-#if defined CALIBRATION_MODE
- RadioManager::instance().init();
- RadioManager::instance().transmitCW(CH_87);
-
- HAL_Delay(1000);
- RadioManager::instance().start();
-#elif defined TX_TEST_MODE
- TXPacketPool::instance().init();
- RadioManager::instance().init();
- RadioManager::instance().start();
-
- // Throttle here to avoid continuous transmission in case we enter a reset loop
- HAL_Delay(400);
- fireTestPacket();
-#else
- RadioManager::instance().init();
- RadioManager::instance().start();
-#endif
-
- uint32_t counter = 0;
-
- bsp_start_wdt();
+ //bsp_start_wdt();
// We're getting a very high rate of interrupts, so there's no need to dispatch events every time
while (1)
{
- __WFI();
- ++counter;
- if ( counter % 20 == 0 )
- {
- counter = 1;
- bsp_refresh_wdt();
- EventQueue::instance().dispatch();
- }
+ //__WFI();
+ vTaskDelay(10);
+ //++counter;
+ //if ( counter % 100 == 0 )
+ //{
+ //counter = 1;
+ //bsp_refresh_wdt();
+ EventQueue::instance().dispatch();
+ //}
}
}
+class Ticker : EventConsumer
+{
+public:
+ Ticker()
+ {
+ EventQueue::instance().addObserver(this, ONE_SEC_TIMER_EVENT);
+ }
+
+ void processEvent(const Event &e)
+ {
+ ++__t;
+ printf_serial_now("Tick %d\r\n", __t);
+ }
+private:
+ int __t = 0;
+};
+
+extern "C" {
+__attribute__((used)) void vApplicationStackOverflowHook( TaskHandle_t xTask, signed char *pcTaskName )
+{
+ //printf_serial_now("WHOOPS!\r\n");
+ asm("BKPT 0");
+}
+
+__attribute__((used)) void vApplicationTickHook()
+{
+ asm("BKPT 0");
+}
+}
+
+
+
int main(void)
{
+ //*(uint8_t *)0xe000ed08 |= 2;
bsp_hw_init();
-
- TaskHandle_t xHandle;
- xTaskCreate(mainTask, "main", 1024u, NULL, 5, &xHandle);
- xPortStartScheduler();
-
-#if 0
-
EventQueue::instance().init();
- EventPool::instance().init();
- Configuration::instance().init();
- CommandProcessor::instance().init();
- DataTerminal::instance().init();
-
- RXPacketProcessor packetProcessor;
-
SystickTimer::instance();
-
+ Ticker t;
+ //Configuration::instance().init();
+ //CommandProcessor::instance().init();
+ //DataTerminal::instance().init();
+ //RXPacketProcessor packetProcessor;
#if not defined CALIBRATION_MODE && not defined TX_TEST_MODE
- GPS::instance().init();
- GPS::instance().enable();
+ //GPS::instance().init();
+ //GPS::instance().enable();
#endif
#ifdef ENABLE_TX
- TXPacketPool::instance().init();
- TXScheduler::instance().init();
+ //TXPacketPool::instance().init();
+ //TXScheduler::instance().init();
#endif
-#if defined CALIBRATION_MODE
- RadioManager::instance().init();
- RadioManager::instance().transmitCW(CH_87);
+ //RadioManager::instance().init();
+ //RadioManager::instance().start();
- HAL_Delay(1000);
- RadioManager::instance().start();
-#elif defined TX_TEST_MODE
- TXPacketPool::instance().init();
- RadioManager::instance().init();
- RadioManager::instance().start();
-
- // Throttle here to avoid continuous transmission in case we enter a reset loop
- HAL_Delay(400);
- fireTestPacket();
-#else
- RadioManager::instance().init();
- RadioManager::instance().start();
-#endif
-
- uint32_t counter = 0;
-
- bsp_start_wdt();
-
- // We're getting a very high rate of interrupts, so there's no need to dispatch events every time
- while (1)
+ TaskHandle_t xHandle;
+ if ( xTaskCreate(mainTask, "main", 256u, NULL, tskIDLE_PRIORITY+1, &xHandle) != pdPASS )
{
- __WFI();
- ++counter;
- if ( counter % 20 == 0 )
- {
- counter = 1;
- bsp_refresh_wdt();
- EventQueue::instance().dispatch();
- }
+ asm("BKPT 0");
}
-#endif
+ xPortStartScheduler();
}
diff --git a/latest/Firmware/Src/mem.cpp b/latest/Firmware/Src/mem.cpp
new file mode 100644
index 0000000..150963d
--- /dev/null
+++ b/latest/Firmware/Src/mem.cpp
@@ -0,0 +1,28 @@
+/*
+ * mem.c
+ *
+ * Created on: Oct 8, 2020
+ * Author: peter
+ */
+
+#include "FreeRTOS.h"
+
+void * operator new( size_t size )
+{
+ return pvPortMalloc( size );
+}
+
+void * operator new[]( size_t size )
+{
+ return pvPortMalloc(size);
+}
+
+void operator delete( void * ptr )
+{
+ vPortFree ( ptr );
+}
+
+void operator delete[]( void * ptr )
+{
+ vPortFree ( ptr );
+}
diff --git a/latest/Firmware/Src/stm32l4xx_hal_timebase_tim.c b/latest/Firmware/Src/stm32l4xx_hal_timebase_tim.c
index 41570c6..39b40cf 100644
--- a/latest/Firmware/Src/stm32l4xx_hal_timebase_tim.c
+++ b/latest/Firmware/Src/stm32l4xx_hal_timebase_tim.c
@@ -39,6 +39,7 @@ TIM_HandleTypeDef htim6;
* @param TickPriority: Tick interrupt priority.
* @retval HAL status
*/
+#if 1
HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
{
RCC_ClkInitTypeDef clkconfig;
@@ -47,7 +48,7 @@ HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
uint32_t pFLatency;
/*Configure the TIM6 IRQ priority */
- HAL_NVIC_SetPriority(TIM6_IRQn, TickPriority ,0);
+ HAL_NVIC_SetPriority(TIM6_IRQn, 5,0);//TickPriority ,0);
/* Enable the TIM6 global Interrupt */
HAL_NVIC_EnableIRQ(TIM6_IRQn);
@@ -87,6 +88,7 @@ HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
return HAL_ERROR;
}
+#endif
/**
* @brief Suspend Tick increment.
* @note Disable the tick increment by disabling TIM6 update interrupt.
@@ -129,6 +131,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
/* USER CODE END Callback 0 */
if (htim->Instance == TIM6) {
HAL_IncTick();
+ HAL_SYSTICK_IRQHandler();
}
/* USER CODE BEGIN Callback 1 */
diff --git a/latest/Firmware/Src/stm32l4xx_it.c b/latest/Firmware/Src/stm32l4xx_it.c
index 77d4dad..fa4f1e6 100644
--- a/latest/Firmware/Src/stm32l4xx_it.c
+++ b/latest/Firmware/Src/stm32l4xx_it.c
@@ -1,35 +1,35 @@
/**
- ******************************************************************************
- * @file stm32l4xx_it.c
- * @brief Interrupt Service Routines.
- ******************************************************************************
- *
- * COPYRIGHT(c) 2017 STMicroelectronics
- *
- * 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 of STMicroelectronics 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 HOLDER 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.
- *
- ******************************************************************************
- */
+ ******************************************************************************
+ * @file stm32l4xx_it.c
+ * @brief Interrupt Service Routines.
+ ******************************************************************************
+ *
+ * COPYRIGHT(c) 2017 STMicroelectronics
+ *
+ * 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 of STMicroelectronics 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 HOLDER 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.
+ *
+ ******************************************************************************
+ */
/* Includes ------------------------------------------------------------------*/
#include "stm32l4xx_hal.h"
#include "stm32l4xx.h"
@@ -37,8 +37,14 @@
extern TIM_HandleTypeDef htim6;
+#define nmi 1
+#define hardfault 2
+#define busfault 3
+#define usagefault 4
+#define memmanage 5
+
/* USER CODE BEGIN 0 */
-extern void Error_Handler();
+extern void Error_Handler(uint8_t i);
#if 0
void prvGetRegistersFromStack( uint32_t *pulFaultStackAddress )
@@ -81,8 +87,8 @@ void prvGetRegistersFromStack( uint32_t *pulFaultStackAddress )
/******************************************************************************/
/**
-* @brief This function handles Non maskable interrupt.
-*/
+ * @brief This function handles Non maskable interrupt.
+ */
void NMI_Handler(void)
{
/* USER CODE BEGIN NonMaskableInt_IRQn 0 */
@@ -91,13 +97,46 @@ void NMI_Handler(void)
/* USER CODE BEGIN NonMaskableInt_IRQn 1 */
/* USER CODE END NonMaskableInt_IRQn 1 */
- Error_Handler();
+ Error_Handler(nmi);
}
/**
-* @brief This function handles Hard fault interrupt.
-*/
-void HardFault_Handler(void)
+ * @brief This function handles Hard fault interrupt.
+ */
+
+volatile uint32_t r0;
+volatile uint32_t r1;
+volatile uint32_t r2;
+volatile uint32_t r3;
+volatile uint32_t r12;
+volatile uint32_t lr; /* Link register. */
+volatile uint32_t pc; /* Program counter. */
+volatile uint32_t psr;/* Program status register. */
+
+
+__attribute__((used)) void prvGetRegistersFromStack( uint32_t *pulFaultStackAddress )
+{
+ /* These are volatile to try and prevent the compiler/linker optimising them
+away as the variables never actually get used. If the debugger won't show the
+values of the variables, make them global my moving their declaration outside
+of this function. */
+
+ r0 = pulFaultStackAddress[ 0 ];
+ r1 = pulFaultStackAddress[ 1 ];
+ r2 = pulFaultStackAddress[ 2 ];
+ r3 = pulFaultStackAddress[ 3 ];
+
+ r12 = pulFaultStackAddress[ 4 ];
+ lr = pulFaultStackAddress[ 5 ];
+ pc = pulFaultStackAddress[ 6 ];
+ psr = pulFaultStackAddress[ 7 ];
+
+ /* When the following line is hit, the variables contain the register values. */
+ //for( ;; );
+ asm("BKPT 0");
+}
+
+__attribute__( ( naked ) ) void HardFault_Handler(void)
{
/* USER CODE BEGIN HardFault_IRQn 0 */
/*
@@ -112,48 +151,50 @@ void HardFault_Handler(void)
" bx r2 \n"
" handler2_address_const: .word prvGetRegistersFromStack \n"
);
-*/
+ */
/* USER CODE END HardFault_IRQn 0 */
//while (1)
//{
//}
- Error_Handler();
+ Error_Handler(hardfault);
+ //while(1);
+ //asm("BKPT 0");
/* USER CODE BEGIN HardFault_IRQn 1 */
/* USER CODE END HardFault_IRQn 1 */
}
/**
-* @brief This function handles Memory management fault.
-*/
+ * @brief This function handles Memory management fault.
+ */
void MemManage_Handler(void)
{
/* USER CODE BEGIN MemoryManagement_IRQn 0 */
/* USER CODE END MemoryManagement_IRQn 0 */
- Error_Handler();
+ Error_Handler(memmanage);
/* USER CODE BEGIN MemoryManagement_IRQn 1 */
/* USER CODE END MemoryManagement_IRQn 1 */
}
/**
-* @brief This function handles Prefetch fault, memory access fault.
-*/
+ * @brief This function handles Prefetch fault, memory access fault.
+ */
void BusFault_Handler(void)
{
/* USER CODE BEGIN BusFault_IRQn 0 */
/* USER CODE END BusFault_IRQn 0 */
- Error_Handler();
+ Error_Handler(busfault);
/* USER CODE BEGIN BusFault_IRQn 1 */
/* USER CODE END BusFault_IRQn 1 */
}
/**
-* @brief This function handles Undefined instruction or illegal state.
-*/
+ * @brief This function handles Undefined instruction or illegal state.
+ */
void UsageFault_Handler(void)
{
/* USER CODE BEGIN UsageFault_IRQn 0 */
@@ -163,8 +204,8 @@ void UsageFault_Handler(void)
while (1)
{
}
- */
- Error_Handler();
+ */
+ Error_Handler(usagefault);
/* USER CODE BEGIN UsageFault_IRQn 1 */
/* USER CODE END UsageFault_IRQn 1 */
@@ -172,8 +213,8 @@ void UsageFault_Handler(void)
#if 0
/**
-* @brief This function handles System service call via SWI instruction.
-*/
+ * @brief This function handles System service call via SWI instruction.
+ */
void SVC_Handler(void)
{
/* USER CODE BEGIN SVCall_IRQn 0 */
@@ -186,8 +227,8 @@ void SVC_Handler(void)
#endif
/**
-* @brief This function handles Debug monitor.
-*/
+ * @brief This function handles Debug monitor.
+ */
void DebugMon_Handler(void)
{
/* USER CODE BEGIN DebugMonitor_IRQn 0 */
@@ -200,8 +241,8 @@ void DebugMon_Handler(void)
#if 0
/**
-* @brief This function handles Pendable request for system service.
-*/
+ * @brief This function handles Pendable request for system service.
+ */
void PendSV_Handler(void)
{
/* USER CODE BEGIN PendSV_IRQn 0 */
@@ -215,8 +256,8 @@ void PendSV_Handler(void)
#if 0
/**
-* @brief This function handles System tick timer.
-*/
+ * @brief This function handles System tick timer.
+ */
void SysTick_Handler(void)
{
/* USER CODE BEGIN SysTick_IRQn 0 */
@@ -238,8 +279,8 @@ void SysTick_Handler(void)
/******************************************************************************/
/**
-* @brief This function handles EXTI line1 interrupt.
-*/
+ * @brief This function handles EXTI line1 interrupt.
+ */
#if 0
void EXTI1_IRQHandler(void)
{
@@ -255,8 +296,8 @@ void EXTI1_IRQHandler(void)
#if 0
/**
-* @brief This function handles EXTI line2 interrupt.
-*/
+ * @brief This function handles EXTI line2 interrupt.
+ */
void EXTI2_IRQHandler(void)
{
/* USER CODE BEGIN EXTI2_IRQn 0 */
@@ -270,8 +311,8 @@ void EXTI2_IRQHandler(void)
#endif
/**
-* @brief This function handles EXTI line3 interrupt.
-*/
+ * @brief This function handles EXTI line3 interrupt.
+ */
#if 0
void EXTI3_IRQHandler(void)
{
@@ -286,8 +327,8 @@ void EXTI3_IRQHandler(void)
#endif
/**
-* @brief This function handles LPTIM1 global interrupt.
-*/
+ * @brief This function handles LPTIM1 global interrupt.
+ */
#if 0
void LPTIM1_IRQHandler(void)
{
@@ -303,8 +344,8 @@ void LPTIM1_IRQHandler(void)
#if 0
/**
-* @brief This function handles LPTIM2 global interrupt.
-*/
+ * @brief This function handles LPTIM2 global interrupt.
+ */
void LPTIM2_IRQHandler(void)
{
/* USER CODE BEGIN LPTIM2_IRQn 0 */