flight/modules/gpsp/gpsdsysmod.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @brief The OpenPilot Modules do the majority of the control in OpenPilot.  The
   5  * @ref SystemModule "System Module" starts all the other modules that then take care
   6  * of all the telemetry and control algorithms and such.  This is done through the @ref PIOS
   7  * "PIOS Hardware abstraction layer" which then contains hardware specific implementations
   8  * (currently only STM32 supported)
   9  *
  10  * @{
  11  * @addtogroup SystemModule GPSV9 System Module
  12  * @brief Initializes PIOS and other modules runs monitoring, executes mag and gps handlers
  13  *
  14  * @{
  15  *
  16  * @file       gpsdsystemmod.c
  17  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  18  * @brief      GPS System module
  19  *
  20  * @see        The GNU Public License (GPL) Version 3
  21  *
  22  *****************************************************************************/
  23 /*
  24  * This program is free software; you can redistribute it and/or modify
  25  * it under the terms of the GNU General Public License as published by
  26  * the Free Software Foundation; either version 3 of the License, or
  27  * (at your option) any later version.
  28  *
  29  * This program is distributed in the hope that it will be useful, but
  30  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  31  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  32  * for more details.
  33  *
  34  * You should have received a copy of the GNU General Public License along
  35  * with this program; if not, write to the Free Software Foundation, Inc.,
  36  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  37  */
  38
  39
  40 // private includes
  41 #include "inc/gpsdsysmod.h"
  42 #include "inc/gps9maghandler.h"
  43 #include "inc/gps9gpshandler.h"
  44 #include "inc/gps9flashhandler.h"
  45 #include "inc/gps9protocol.h"
  46 #include "pios_board_info.h"
  47
  48 extern uint32_t pios_com_main_id;
  49
  50 // Private constants
  51 #define SYSTEM_UPDATE_PERIOD_MS    1
  52 #define HB_LED_BLINK_ON_PERIOD_MS  100
  53 #define HB_LED_BLINK_OFF_PERIOD_MS 1900
  54 #define STACK_SIZE_BYTES           450
  55 #define STAT_UPDATE_PERIOD_MS      10000
  56 #define TASK_PRIORITY              (tskIDLE_PRIORITY + 2)
  57
  58 // Private types
  59
  60 // Private variables
  61 static xTaskHandle systemTaskHandle;
  62 static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_CRITICAL = 3 } stackOverflow;
  63 volatile int initTaskDone = 0;
  64
  65
  66 static bool mallocFailed;
  67 static SysUbxPkt sysPkt;
  68
  69 // Private functions
  70 static void updateStats();
  71 static void gpspSystemTask(void *parameters);
  72 static void readFirmwareInfo();
  73 /**
  74  * Create the module task.
  75  * \returns 0 on success or -1 if initialization failed
  76  */
  77 int32_t GPSPSystemModStart(void)
  78 {
  79     // Initialize vars
  80     stackOverflow = STACKOVERFLOW_NONE;
  81     mallocFailed  = false;
  82     // Create system task
  83     xTaskCreate(gpspSystemTask, (const char *)"G-Sys", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
  84 #ifdef PIOS_INCLUDE_WDG
  85     PIOS_WDG_RegisterFlag(PIOS_WDG_SYSTEM);
  86 #endif
  87     return 0;
  88 }
  89
  90 /**
  91  * Initialize the module, called on startup.
  92  * \returns 0 on success or -1 if initialization failed
  93  */
  94 int32_t GPSPSystemModInitialize(void)
  95 {
  96     GPSPSystemModStart();
  97
  98     return 0;
  99 }
 100
 101 MODULE_INITCALL(GPSPSystemModInitialize, 0);
 102 /**
 103  * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
 104  */
 105 static void gpspSystemTask(__attribute__((unused)) void *parameters)
 106 {
 107 #ifdef PIOS_INCLUDE_WDG
 108     PIOS_WDG_UpdateFlag(PIOS_WDG_SYSTEM);
 109 #endif
 110     while (!initTaskDone) {
 111         vTaskDelay(10);
 112     }
 113 #ifndef PIOS_INCLUDE_WDG
 114 // if no watchdog is enabled, don't reset watchdog in MODULE_TASKCREATE_ALL loop
 115 #define PIOS_WDG_Clear()
 116 #endif
 117     /* create all modules thread */
 118     MODULE_TASKCREATE_ALL;
 119
 120     if (mallocFailed) {
 121         // Nothing to do, this condition needs to be trapped during development.
 122         while (true) {
 123             ;
 124         }
 125     }
 126
 127 #if defined(PIOS_INCLUDE_IAP)
 128     PIOS_IAP_WriteBootCount(0);
 129 #endif
 130     /* Right now there is no configuration and uart speed is fixed at 57600.
 131      * TODO:
 132      * 1) add a tiny ubx parser on gps side to intercept CFG-RINV and use that for config storage;
 133      * 2) second ubx parser on uart side that intercept custom configuration message and flash commands.
 134      */
 135     PIOS_COM_ChangeBaud(pios_com_main_id, GPS_MODULE_DEFAULT_BAUDRATE);
 136     setupGPS();
 137     uint32_t ledTimer = 0;
 138     static TickType_t lastUpdate;
 139     readFirmwareInfo();
 140
 141     while (1) {
 142 #ifdef PIOS_INCLUDE_WDG
 143         PIOS_WDG_UpdateFlag(PIOS_WDG_SYSTEM);
 144 #endif
 145         uint32_t ledPeriod = PIOS_DELAY_DiffuS(ledTimer) / 1000;
 146         if (ledPeriod < HB_LED_BLINK_ON_PERIOD_MS) {
 147             PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 148         } else {
 149             PIOS_LED_On(PIOS_LED_HEARTBEAT);
 150         }
 151         if (ledPeriod > (HB_LED_BLINK_ON_PERIOD_MS + HB_LED_BLINK_OFF_PERIOD_MS)) {
 152             ledTimer = PIOS_DELAY_GetRaw();
 153         }
 154
 155         handleGPS();
 156         handleMag();
 157         updateStats();
 158         vTaskDelayUntil(&lastUpdate, SYSTEM_UPDATE_PERIOD_MS * configTICK_RATE_HZ / 1000);
 159     }
 160 }
 161
 162
 163 /**
 164  * Called periodically to update the system stats
 165  */
 166 uint16_t GetFreeIrqStackSize(void)
 167 {
 168     uint32_t i = 0x150;
 169
 170 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
 171     extern uint32_t _irq_stack_top;
 172     extern uint32_t _irq_stack_end;
 173     uint32_t pattern    = 0x0000A5A5;
 174     uint32_t *ptr       = &_irq_stack_end;
 175
 176 #if 1 /* the ugly way accurate but takes more time, useful for debugging */
 177     uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3) / 4;
 178
 179     for (i = 0; i < stack_size; i++) {
 180         if (ptr[i] != pattern) {
 181             i = i * 4;
 182             break;
 183         }
 184     }
 185 #else /* faster way but not accurate */
 186     if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern) {
 187         i = IRQSTACK_LIMIT_CRITICAL - 1;
 188     } else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern) {
 189         i = IRQSTACK_LIMIT_WARNING - 1;
 190     } else {
 191         i = IRQSTACK_LIMIT_WARNING;
 192     }
 193 #endif
 194 #endif /* if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) */
 195     return i;
 196 }
 197
 198 /**
 199  * Called periodically to update the system stats
 200  */
 201 static void updateStats()
 202 {
 203     static uint32_t lastUpdate;
 204
 205     if (PIOS_DELAY_DiffuS(lastUpdate) < STAT_UPDATE_PERIOD_MS * 1000) {
 206         return;
 207     }
 208     lastUpdate = PIOS_DELAY_GetRaw();
 209
 210     // Get stats and update
 211     sysPkt.fragments.data.flightTime = xTaskGetTickCount() * portTICK_RATE_MS;
 212     sysPkt.fragments.data.options    = SYS_DATA_OPTIONS_MAG | (flash_available() ? SYS_DATA_OPTIONS_FLASH : 0);
 213     ubx_buildPacket(&sysPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_SYS, sizeof(SysData));
 214     PIOS_COM_SendBuffer(pios_com_main_id, sysPkt.packet.binarystream, sizeof(SysUbxPkt));
 215 }
 216
 217 // retrieve firmware info and fill syspkt
 218 static void readFirmwareInfo()
 219 {
 220     const struct pios_board_info *bdinfo = &pios_board_info_blob;
 221
 222     sysPkt.fragments.data.board_revision = bdinfo->board_rev;
 223     sysPkt.fragments.data.board_type     = bdinfo->board_type;
 224     struct fw_version_info *fwinfo = (struct fw_version_info *)(bdinfo->fw_base + bdinfo->fw_size);
 225
 226     memcpy(&sysPkt.fragments.data.commit_tag_name, &fwinfo->commit_tag_name, sizeof(sysPkt.fragments.data.commit_tag_name));
 227     memcpy(&sysPkt.fragments.data.sha1sum, &fwinfo->sha1sum, sizeof(sysPkt.fragments.data.sha1sum));
 228 }
 229
 230 /**
 231  * Called by the RTOS when the CPU is idle,
 232  */
 233 void vApplicationIdleHook(void)
 234 {}
 235 /**
 236  * Called by the RTOS when a stack overflow is detected.
 237  */
 238 #define DEBUG_STACK_OVERFLOW 0
 239 void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
 240                                    __attribute__((unused)) signed portCHAR *pcTaskName)
 241 {
 242     stackOverflow = STACKOVERFLOW_CRITICAL;
 243 #if DEBUG_STACK_OVERFLOW
 244     static volatile bool wait_here = true;
 245     while (wait_here) {
 246         ;
 247     }
 248     wait_here = true;
 249 #endif
 250 }
 251
 252 /**
 253  * Called by the RTOS when a malloc call fails.
 254  */
 255 #define DEBUG_MALLOC_FAILURES 0
 256 void vApplicationMallocFailedHook(void)
 257 {
 258     mallocFailed = true;
 259 #if DEBUG_MALLOC_FAILURES
 260     static volatile bool wait_here = true;
 261     while (wait_here) {
 262         ;
 263     }
 264     wait_here = true;
 265 #endif
 266 }
 267
 268 /**
 269  * @}
 270  * @}
 271  */