From f88b84fb5c6a5b515c29bf6a96ea30be5e9cbf97 Mon Sep 17 00:00:00 2001
From: Peter Antypas <peter.antypas@gmail.com>
Date: Thu, 8 Oct 2020 23:15:06 -0700
Subject: [PATCH] FUBAR

---
 latest/Firmware/.cproject                     |   2 +-
 .../Firmware/.settings/language.settings.xml  |   2 +-
 .../STM32L4xx_HAL_Driver/Src/stm32l4xx_hal.c  |   3 +-
 .../FreeRTOS/portable/GCC/ARM_CM4F/port.c     |   1 +
 latest/Firmware/Inc/FreeRTOSConfig.h          |  11 +-
 latest/Firmware/Inc/TXPacket.hpp              |   1 +
 latest/Firmware/STM32L412KBUx_FLASH.ld        |   2 +-
 latest/Firmware/Src/EventQueue.cpp            |  10 +-
 latest/Firmware/Src/GPS.cpp                   |   1 +
 latest/Firmware/Src/TXPacket.cpp              |   5 +
 latest/Firmware/Src/Transceiver.cpp           |  26 +--
 latest/Firmware/Src/bsp/bsp_5_2.cpp           |  50 ++---
 latest/Firmware/Src/main.cpp                  | 163 ++++++----------
 latest/Firmware/Src/mem.cpp                   |  28 +++
 .../Firmware/Src/stm32l4xx_hal_timebase_tim.c |   5 +-
 latest/Firmware/Src/stm32l4xx_it.c            | 177 +++++++++++-------
 16 files changed, 271 insertions(+), 216 deletions(-)
 create mode 100644 latest/Firmware/Src/mem.cpp

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 @@
                             							
                             <option id="ilg.gnuarmeclipse.managedbuild.cross.option.target.other.554432213" name="Other target flags" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.target.other" useByScannerDiscovery="true"/>
                             							
-                            <option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level.1030588197" name="Optimization Level" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level" useByScannerDiscovery="true" value="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level.size" valueType="enumerated"/>
+                            <option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level.1030588197" name="Optimization Level" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level" useByScannerDiscovery="true" value="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.level.debug" valueType="enumerated"/>
                             							
                             <option id="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.messagelength.1195781009" name="Message length (-fmessage-length=0)" superClass="ilg.gnuarmeclipse.managedbuild.cross.option.optimization.messagelength" useByScannerDiscovery="true" value="true" valueType="boolean"/>
                             							
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 @@
             			
             <provider copy-of="extension" id="org.eclipse.cdt.managedbuilder.core.GCCBuildCommandParser"/>
             			
-            <provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-1846098760891170780" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT ARM Cross GCC Built-in Compiler Settings " parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+            <provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-1857589726391058780" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT ARM Cross GCC Built-in Compiler Settings " parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
                 				
                 <language-scope id="org.eclipse.cdt.core.gcc"/>
                 				
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 */