Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
chibios_init.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Gautier Hattenberger, Alexandre Bustico
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
22 /*
23  * @file firmwares/fixedwing/chibios-libopencm3/chibios_init.c
24  *
25  */
26 
27 #include <ch.h>
28 #include <hal.h>
29 #include "subsystems/chibios-libopencm3/chibios_sdlog.h"
30 #include "sdLog.h"
31 #include "usbStorage.h"
32 #include "pprz_stub.h"
33 #include "rtcAccess.h"
34 #include "generated/airframe.h"
35 #include "chibios_init.h"
36 
37 // Delay before starting SD log
38 #ifndef SDLOG_START_DELAY
39 #define SDLOG_START_DELAY 30
40 #endif
41 
42 
43 #ifndef SYS_TIME_FREQUENCY
44 #error SYS_TIME_FREQUENCY should be defined in Makefile.chibios or airframe.xml and be equal to CH_FREQUENCY
45 #elif SYS_TIME_FREQUENCY != CH_FREQUENCY
46 #error SYS_TIME_FREQUENCY should be equal to CH_FREQUENCY
47 #elif CH_FREQUENCY < (2 * PERIODIC_FREQUENCY)
48 #error CH_FREQUENCY and SYS_TIME_FREQUENCY should be >= 2 x PERIODIC_FREQUENCY
49 #endif
50 
51 static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg);
52 #define MAX(x , y) (((x) > (y)) ? (x) : (y))
53 #define ARRAY_LEN(a) (sizeof(a)/sizeof(a[0]))
54 
55 Thread *pprzThdPtr = NULL;
56 
57 static WORKING_AREA(wa_thd_heartbeat, 2048);
58 void chibios_launch_heartbeat (void);
59 bool_t sdOk = FALSE;
60 
61 #if LOG_PROCESS_STATE
62 static int32_t get_stack_free (Thread *tp);
63 #endif
64 
65 /*
66  * Init ChibiOS HAL and Sys
67  */
68 bool_t chibios_init(void) {
69  halInit();
70  chSysInit();
71 
72  PWR->CSR &= ~PWR_CSR_BRE;
73  DBGMCU->APB1FZ |= DBGMCU_APB1_FZ_DBG_IWDG_STOP;
74 
75  chThdCreateStatic(wa_thd_heartbeat, sizeof(wa_thd_heartbeat),
76  NORMALPRIO, thd_heartbeat, NULL);
77 
78  usbStorageStartPolling ();
79  return RDY_OK;
80 }
81 
82 static WORKING_AREA(pprzThd, 4096);
83 void launch_pprz_thd (int32_t (*thd) (void *arg))
84 {
85  pprzThdPtr = chThdCreateStatic(pprzThd, sizeof(pprzThd), NORMALPRIO+1, thd, NULL);
86 }
87 
88 
89 /*
90  * Heartbeat thread
91  */
92 static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg)
93 {
94  (void) arg;
95  chRegSetThreadName("pprz heartbeat");
96 
97  chThdSleepSeconds (SDLOG_START_DELAY);
98  if (usbStorageIsItRunning ())
99  chThdSleepSeconds (20000); // stuck here for hours
100  else
101  sdOk = chibios_logInit(true);
102 
103  while (TRUE) {
104  palTogglePad (GPIOC, GPIOC_LED3);
105  chThdSleepMilliseconds (sdOk == TRUE ? 1000 : 200);
106  static uint32_t timestamp = 0;
107 
108 #if LOG_PROCESS_STATE
109  sdLogWriteLog (&processLogFile, " addr stack frestk prio refs state time name\r\n");
110 #endif
111 
112  // chSysDisable ();
113  Thread *tp = chRegFirstThread();
114  do {
115 
116 #if LOG_PROCESS_STATE
117  sdLogWriteLog (&processLogFile, "%.8lx %.8lx %6lu %4lu %4lu [S:%d] %5lu %s\r\n",
118  (uint32_t)tp, (uint32_t)tp->p_ctx.r13,
119  get_stack_free (tp),
120  (uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1),
121  tp->p_state, (uint32_t)tp->p_time,
122  chRegGetThreadName(tp));
123 #endif
124 
125  tp = chRegNextThread(tp);
126  } while (tp != NULL);
127  // chSysEnable ();
128 
129  // we sync gps time to rtc every 5 seconds
130  if (chTimeNow() - timestamp > 5000) {
131  timestamp = chTimeNow();
132  if (getGpsTimeOfWeek() != 0) {
133  setRtcFromGps (getGpsWeek(), getGpsTimeOfWeek());
134  }
135  }
136 
137  }
138 }
139 
140 
141 #if LOG_PROCESS_STATE
142 static int32_t get_stack_free (Thread *tp)
143 {
144  int32_t index = 0;
145  const uint8_t *maxRamAddr = (uint8_t*) (0x20000000 + (128*1024));
146  const int32_t internalStructSize = 80;
147 
148  unsigned long long *stkAdr = (unsigned long long *) ((uint8_t *) tp + internalStructSize);
149  //unsigned long long *stkAdr = (unsigned long long *) tp;
150 
151  while ((stkAdr[index] == 0x5555555555555555) && ( ((uint8_t *) &(stkAdr[index])) < maxRamAddr))
152  index++;
153 
154  const int32_t freeBytes = index * sizeof(long long);
155  return MAX(0, freeBytes - internalStructSize);
156 }
157 #endif
static msg_t thd_heartbeat(void *arg)
Definition: chibios_init.c:92
#define GPIOC
Definition: gpio_arch.h:34
void chibios_launch_heartbeat(void)
uint32_t timestamp
This module provides a timestamp-message, allowing sw/logalizer/openlog2tlm to convert a recorded dum...
Definition: openlog.c:36
static WORKING_AREA(wa_thd_heartbeat, 2048)
bool_t chibios_init(void)
Definition: chibios_init.c:68
bool_t sdOk
Definition: chibios_init.c:59
#define FALSE
Definition: imu_chimu.h:141
Thread * pprzThdPtr
Definition: chibios_init.c:55
unsigned long uint32_t
Definition: types.h:18
#define SDLOG_START_DELAY
Definition: chibios_init.c:39
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
void launch_pprz_thd(int32_t(*thd)(void *arg))
Definition: chibios_init.c:83
#define MAX(x, y)
Definition: chibios_init.c:52