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 #include "pios_board_init.h"
  48
  49 extern uint32_t pios_com_main_id;
  50
  51 // Private constants
  52 #define SYSTEM_UPDATE_PERIOD_MS    1
  53 #define HB_LED_BLINK_ON_PERIOD_MS  100
  54 #define HB_LED_BLINK_OFF_PERIOD_MS 1900
  55 #define STACK_SIZE_BYTES           450
  56 #define STAT_UPDATE_PERIOD_MS      10000
  57 #define TASK_PRIORITY              (tskIDLE_PRIORITY + 2)
  58
  59 // Private types
  60
  61 // Private variables
  62 static xTaskHandle systemTaskHandle;
  63 static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_CRITICAL = 3 } stackOverflow;
  64 volatile int initTaskDone = 0;
  65
  66
  67 static bool mallocFailed;
  68 static SysUbxPkt sysPkt;
  69
  70 // Private functions
  71 static void updateStats();
  72 static void gpspSystemTask(void *parameters);
  73 static void readFirmwareInfo();
  74 /**
  75  * Create the module task.
  76  * \returns 0 on success or -1 if initialization failed
  77  */
  78 int32_t GPSPSystemModStart(void)
  79 {
  80     // Initialize vars
  81     stackOverflow = STACKOVERFLOW_NONE;
  82     mallocFailed  = false;
  83     // Create system task
  84     xTaskCreate(gpspSystemTask, (const char *)"G-Sys", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
  85 #ifdef PIOS_INCLUDE_WDG
  86     PIOS_WDG_RegisterFlag(PIOS_WDG_SYSTEM);
  87 #endif
  88     return 0;
  89 }
  90
  91 /**
  92  * Initialize the module, called on startup.
  93  * \returns 0 on success or -1 if initialization failed
  94  */
  95 int32_t GPSPSystemModInitialize(void)
  96 {
  97     // Module started in main
  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
 111     /* board driver init */
 112     PIOS_Board_Init();
 113
 114     /* Initialize all modules */
 115     MODULE_INITIALISE_ALL;
 116
 117     while (!initTaskDone) {
 118         vTaskDelay(10);
 119     }
 120
 121 #ifndef PIOS_INCLUDE_WDG
 122 // if no watchdog is enabled, don't reset watchdog in MODULE_TASKCREATE_ALL loop
 123 #define PIOS_WDG_Clear()
 124 #endif
 125     /* create all modules thread */
 126     MODULE_TASKCREATE_ALL;
 127
 128     if (mallocFailed) {
 129         // Nothing to do, this condition needs to be trapped during development.
 130         while (true) {
 131             ;
 132         }
 133     }
 134
 135 #if defined(PIOS_INCLUDE_IAP)
 136     PIOS_IAP_WriteBootCount(0);
 137 #endif
 138
 139     /* Right now there is no configuration and uart speed is fixed at 57600.
 140      * TODO:
 141      * 1) add a tiny ubx parser on gps side to intercept CFG-RINV and use that for config storage;
 142      * 2) second ubx parser on uart side that intercept custom configuration message and flash commands.
 143      */
 144     PIOS_COM_ChangeBaud(pios_com_main_id, GPS_MODULE_DEFAULT_BAUDRATE);
 145     setupGPS();
 146     uint32_t ledTimer = 0;
 147     static TickType_t lastUpdate;
 148     readFirmwareInfo();
 149
 150     while (1) {
 151 #ifdef PIOS_INCLUDE_WDG
 152         PIOS_WDG_UpdateFlag(PIOS_WDG_SYSTEM);
 153 #endif
 154         uint32_t ledPeriod = PIOS_DELAY_DiffuS(ledTimer) / 1000;
 155         if (ledPeriod < HB_LED_BLINK_OFF_PERIOD_MS) {
 156             PIOS_LED_Off(PIOS_LED_HEARTBEAT);
 157         } else {
 158             PIOS_LED_On(PIOS_LED_HEARTBEAT);
 159         }
 160         if (ledPeriod > (HB_LED_BLINK_ON_PERIOD_MS + HB_LED_BLINK_OFF_PERIOD_MS)) {
 161             ledTimer = PIOS_DELAY_GetRaw();
 162         }
 163
 164         handleGPS();
 165         handleMag();
 166         updateStats();
 167         vTaskDelayUntil(&lastUpdate, SYSTEM_UPDATE_PERIOD_MS * configTICK_RATE_HZ / 1000);
 168     }
 169 }
 170
 171
 172 /**
 173  * Called periodically to update the system stats
 174  */
 175 uint16_t GetFreeIrqStackSize(void)
 176 {
 177     uint32_t i = 0x150;
 178
 179 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
 180     extern uint32_t _irq_stack_top;
 181     extern uint32_t _irq_stack_end;
 182     uint32_t pattern    = 0x0000A5A5;
 183     uint32_t *ptr       = &_irq_stack_end;
 184
 185 #if 1 /* the ugly way accurate but takes more time, useful for debugging */
 186     uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3) / 4;
 187
 188     for (i = 0; i < stack_size; i++) {
 189         if (ptr[i] != pattern) {
 190             i = i * 4;
 191             break;
 192         }
 193     }
 194 #else /* faster way but not accurate */
 195     if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern) {
 196         i = IRQSTACK_LIMIT_CRITICAL - 1;
 197     } else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern) {
 198         i = IRQSTACK_LIMIT_WARNING - 1;
 199     } else {
 200         i = IRQSTACK_LIMIT_WARNING;
 201     }
 202 #endif
 203 #endif /* if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) */
 204     return i;
 205 }
 206
 207 /**
 208  * Called periodically to update the system stats
 209  */
 210 static void updateStats()
 211 {
 212     static uint32_t lastUpdate;
 213
 214     if (PIOS_DELAY_DiffuS(lastUpdate) < STAT_UPDATE_PERIOD_MS * 1000) {
 215         return;
 216     }
 217     lastUpdate = PIOS_DELAY_GetRaw();
 218
 219     // Get stats and update
 220     sysPkt.fragments.data.flightTime = xTaskGetTickCount() * portTICK_RATE_MS;
 221     sysPkt.fragments.data.options    = SYS_DATA_OPTIONS_MAG | (flash_available() ? SYS_DATA_OPTIONS_FLASH : 0);
 222     ubx_buildPacket(&sysPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_SYS, sizeof(SysData));
 223     PIOS_COM_SendBuffer(pios_com_main_id, sysPkt.packet.binarystream, sizeof(SysUbxPkt));
 224 }
 225
 226 // retrieve firmware info and fill syspkt
 227 static void readFirmwareInfo()
 228 {
 229     const struct pios_board_info *bdinfo = &pios_board_info_blob;
 230
 231     sysPkt.fragments.data.board_revision = bdinfo->board_rev;
 232     sysPkt.fragments.data.board_type     = bdinfo->board_type;
 233     struct fw_version_info *fwinfo = (struct fw_version_info *)(bdinfo->fw_base + bdinfo->fw_size);
 234
 235     memcpy(&sysPkt.fragments.data.commit_tag_name, &fwinfo->commit_tag_name, sizeof(sysPkt.fragments.data.commit_tag_name));
 236     memcpy(&sysPkt.fragments.data.sha1sum, &fwinfo->sha1sum, sizeof(sysPkt.fragments.data.sha1sum));
 237 }
 238
 239 /**
 240  * Called by the RTOS when the CPU is idle,
 241  */
 242 void vApplicationIdleHook(void)
 243 {}
 244 /**
 245  * Called by the RTOS when a stack overflow is detected.
 246  */
 247 #define DEBUG_STACK_OVERFLOW 0
 248 void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
 249                                    __attribute__((unused)) signed portCHAR *pcTaskName)
 250 {
 251     stackOverflow = STACKOVERFLOW_CRITICAL;
 252 #if DEBUG_STACK_OVERFLOW
 253     static volatile bool wait_here = true;
 254     while (wait_here) {
 255         ;
 256     }
 257     wait_here = true;
 258 #endif
 259 }
 260
 261 /**
 262  * Called by the RTOS when a malloc call fails.
 263  */
 264 #define DEBUG_MALLOC_FAILURES 0
 265 void vApplicationMallocFailedHook(void)
 266 {
 267     mallocFailed = true;
 268 #if DEBUG_MALLOC_FAILURES
 269     static volatile bool wait_here = true;
 270     while (wait_here) {
 271         ;
 272     }
 273     wait_here = true;
 274 #endif
 275 }
 276
 277 /**
 278  * @}
 279  * @}
 280  */