Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
main_chibios.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013-2015 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 
26 #include "mcu_periph/sys_time.h"
27 #include "mcu.h"
28 #include <ch.h>
29 
30 #ifndef SYS_TIME_FREQUENCY
31 #error SYS_TIME_FREQUENCY should be defined in Makefile.chibios or airframe.xml and be equal to CH_CFG_ST_FREQUENCY
32 #elif CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY) && SYS_TIME_FREQUENCY < (2 * PERIODIC_FREQUENCY)
33 #error CH_CFG_ST_FREQUENCY and SYS_TIME_FREQUENCY should be >= 2 x PERIODIC_FREQUENCY
34 #endif
35 
36 #ifdef FBW
38 #define Fbw(f) f ## _fbw()
39 #else
40 #define Fbw(f)
41 #endif
42 
43 #ifdef AP
45 #define Ap(f) f ## _ap()
46 #else
47 #define Ap(f)
48 #endif
49 
50 #if USE_HARD_FAULT_RECOVERY
52 #endif
53 
54 /*
55  * Default autopilot thread stack size
56  */
57 #ifndef AP_THREAD_STACK_SIZE
58 #define AP_THREAD_STACK_SIZE 8192
59 #endif
60 
61 /*
62  * PPRZ/AP thread
63  */
64 static void thd_ap(void *arg);
65 static THD_WORKING_AREA(wa_thd_ap, AP_THREAD_STACK_SIZE);
66 static thread_t *apThdPtr = NULL;
67 
68 /*
69  * Default FBW thread stack size
70  */
71 #ifndef FBW_THREAD_STACK_SIZE
72 #define FBW_THREAD_STACK_SIZE 1024
73 #endif
74 
75 /*
76  * PPRZ/FBW thread
77  */
78 static void thd_fbw(void *arg);
79 static THD_WORKING_AREA(wa_thd_fbw, FBW_THREAD_STACK_SIZE);
80 static thread_t *fbwThdPtr = NULL;
81 
85 int main(void)
86 {
87  // Init
88  Fbw(init);
89 #if USE_HARD_FAULT_RECOVERY
90  // if recovering from hard fault, don't call AP init, only FBW
91  if (!recovering_from_hard_fault) {
92 #endif
93  Ap(init);
94 #if USE_HARD_FAULT_RECOVERY
95  } else {
96  // but we still need downlink to be initialized
97  downlink_init();
98  modules_datalink_init();
99  }
100 #endif
101 
102  chThdSleepMilliseconds(100);
103 
104  // Create threads
105  fbwThdPtr = chThdCreateStatic(wa_thd_fbw, sizeof(wa_thd_fbw), NORMALPRIO, thd_fbw, NULL);
106 #if USE_HARD_FAULT_RECOVERY
107  // if recovering from hard fault, don't start AP thread, only FBW
108  if (!recovering_from_hard_fault) {
109 #endif
110  apThdPtr = chThdCreateStatic(wa_thd_ap, sizeof(wa_thd_ap), NORMALPRIO, thd_ap, NULL);
111 #if USE_HARD_FAULT_RECOVERY
112  }
113 #endif
114 
115  // Main loop, do nothing
116  chThdSleep(TIME_INFINITE);
117  return 0;
118 }
119 
120 /*
121  * PPRZ/AP thread
122  *
123  * Call PPRZ AP periodic and event functions
124  */
125 static void thd_ap(void *arg)
126 {
127  (void) arg;
128  chRegSetThreadName("AP");
129 
130  while (!chThdShouldTerminateX()) {
131  systime_t t = chVTGetSystemTime();
133  Ap(event_task);
134  // The sleep time is computed to have a polling interval of
135  // 1e6 / CH_CFG_ST_FREQUENCY. If time is passed, thanks to the
136  // "Windowed" sleep function, the execution is not blocked until
137  // a complet roll-over.
138  chThdSleepUntilWindowed(t, t + TIME_US2I(1000000 / CH_CFG_ST_FREQUENCY));
139  }
140 
141  chThdExit(0);
142 }
143 
144 /*
145  * PPRZ/FBW thread
146  *
147  * Call PPRZ FBW periodic and event functions
148  */
149 static void thd_fbw(void *arg)
150 {
151  (void) arg;
152  chRegSetThreadName("FBW");
153 
154  while (!chThdShouldTerminateX()) {
155  systime_t t = chVTGetSystemTime();
157  Fbw(event_task);
158  // The sleep time is computed to have a polling interval of
159  // 1e6 / CH_CFG_ST_FREQUENCY. If time is passed, thanks to the
160  // "Windowed" sleep function, the execution is not blocked until
161  // a complet roll-over.
162  chThdSleepUntilWindowed(t, t + TIME_US2I(1000000 / CH_CFG_ST_FREQUENCY));
163  }
164 
165  chThdExit(0);
166 }
167 
168 /*
169  * Terminate autopilot threads
170  * Wait until proper stop
171  */
173 {
174  if (apThdPtr != NULL) {
175  chThdTerminate(apThdPtr);
176  chThdWait(apThdPtr);
177  apThdPtr = NULL;
178  }
179  if (fbwThdPtr != NULL) {
180  chThdTerminate(fbwThdPtr);
181  chThdWait(fbwThdPtr);
182  fbwThdPtr = NULL;
183  }
184 }
185 
static void thd_fbw(void *arg)
Definition: main_chibios.c:149
#define Fbw(f)
Definition: main_chibios.c:40
static THD_WORKING_AREA(wa_thd_ap, AP_THREAD_STACK_SIZE)
STATIC_INLINE void handle_periodic_tasks(void)
Definition: main_ap.c:136
#define FBW_THREAD_STACK_SIZE
Definition: main_chibios.c:72
#define Ap(f)
Definition: main_chibios.c:47
#define AP_THREAD_STACK_SIZE
Definition: main_chibios.c:58
Architecture independent timing functions.
static void thd_ap(void *arg)
Definition: main_chibios.c:125
static thread_t * apThdPtr
Definition: main_chibios.c:66
void pprz_terminate_autopilot_threads(void)
Terminate all autopilot threads Wait until proper stop.
Definition: main_chibios.c:172
Arch independent mcu ( Micro Controller Unit ) utilities.
int main(void)
Main function.
Definition: main_chibios.c:85
#define CH_CFG_ST_FREQUENCY
System tick frequency.
Definition: chconf.h:55
static thread_t * fbwThdPtr
Definition: main_chibios.c:80
FBW ( FlyByWire ) process API.
AP ( AutoPilot ) process API.