Paparazzi UAS  v5.15_devel-81-gd13dafb
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 SYS_TIME_FREQUENCY != CH_CFG_ST_FREQUENCY
33 #error SYS_TIME_FREQUENCY should be equal to CH_CFG_ST_FREQUENCY
34 #elif CH_CFG_ST_FREQUENCY < (2 * PERIODIC_FREQUENCY)
35 #error CH_CFG_ST_FREQUENCY and SYS_TIME_FREQUENCY should be >= 2 x PERIODIC_FREQUENCY
36 #endif
37 
38 #ifdef FBW
40 #define Fbw(f) f ## _fbw()
41 #else
42 #define Fbw(f)
43 #endif
44 
45 #ifdef AP
47 #define Ap(f) f ## _ap()
48 #else
49 #define Ap(f)
50 #endif
51 
52 #if USE_HARD_FAULT_RECOVERY
54 #endif
55 
56 /*
57  * Default autopilot thread stack size
58  */
59 #ifndef AP_THREAD_STACK_SIZE
60 #define AP_THREAD_STACK_SIZE 8192
61 #endif
62 
63 /*
64  * PPRZ/AP thread
65  */
66 static void thd_ap(void *arg);
67 static THD_WORKING_AREA(wa_thd_ap, AP_THREAD_STACK_SIZE);
68 static thread_t *apThdPtr = NULL;
69 
70 /*
71  * Default FBW thread stack size
72  */
73 #ifndef FBW_THREAD_STACK_SIZE
74 #define FBW_THREAD_STACK_SIZE 1024
75 #endif
76 
77 /*
78  * PPRZ/FBW thread
79  */
80 static void thd_fbw(void *arg);
81 static THD_WORKING_AREA(wa_thd_fbw, FBW_THREAD_STACK_SIZE);
82 static thread_t *fbwThdPtr = NULL;
83 
87 int main(void)
88 {
89  // Init
90  Fbw(init);
91 #if USE_HARD_FAULT_RECOVERY
92  // if recovering from hard fault, don't call AP init, only FBW
93  if (!recovering_from_hard_fault) {
94 #endif
95  Ap(init);
96 #if USE_HARD_FAULT_RECOVERY
97  } else {
98  // but we still need downlink to be initialized
99  downlink_init();
100  modules_datalink_init();
101  }
102 #endif
103 
104  chThdSleepMilliseconds(100);
105 
106  // Create threads
107  fbwThdPtr = chThdCreateStatic(wa_thd_fbw, sizeof(wa_thd_fbw), NORMALPRIO, thd_fbw, NULL);
108 #if USE_HARD_FAULT_RECOVERY
109  // if recovering from hard fault, don't start AP thread, only FBW
110  if (!recovering_from_hard_fault) {
111 #endif
112  apThdPtr = chThdCreateStatic(wa_thd_ap, sizeof(wa_thd_ap), NORMALPRIO, thd_ap, NULL);
113 #if USE_HARD_FAULT_RECOVERY
114  }
115 #endif
116 
117  // Main loop, do nothing
118  chThdSleep(TIME_INFINITE);
119  return 0;
120 }
121 
122 /*
123  * PPRZ/AP thread
124  *
125  * Call PPRZ AP periodic and event functions
126  */
127 static void thd_ap(void *arg)
128 {
129  (void) arg;
130  chRegSetThreadName("AP");
131 
132  while (!chThdShouldTerminateX()) {
134  Ap(event_task);
135  // In tick mode, the minimum step is 1e6 / CH_CFG_ST_FREQUENCY
136  // which means that whatever happens, if we do a sleep of this
137  // time step, the next wakeup will be "aligned" and we won't see
138  // jitter. The polling on event will also be as fast as possible
139  // Be careful that in tick-less mode, it will be required to use
140  // the chThdSleepUntil function with a correct computation of the
141  // wakeup time, in particular roll-over should be check.
142  chThdSleepMicroseconds(1000000 / CH_CFG_ST_FREQUENCY);
143  }
144 
145  chThdExit(0);
146 }
147 
148 /*
149  * PPRZ/FBW thread
150  *
151  * Call PPRZ FBW periodic and event functions
152  */
153 static void thd_fbw(void *arg)
154 {
155  (void) arg;
156  chRegSetThreadName("FBW");
157 
158  while (!chThdShouldTerminateX()) {
160  Fbw(event_task);
161  // In tick mode, the minimum step is 1e6 / CH_CFG_ST_FREQUENCY
162  // which means that whatever happens, if we do a sleep of this
163  // time step, the next wakeup will be "aligned" and we won't see
164  // jitter. The polling on event will also be as fast as possible
165  // Be careful that in tick-less mode, it will be required to use
166  // the chThdSleepUntil function with a correct computation of the
167  // wakeup time, in particular roll-over should be check.
168  chThdSleepMicroseconds(1000000 / CH_CFG_ST_FREQUENCY);
169  }
170 
171  chThdExit(0);
172 }
173 
174 /*
175  * Terminate autopilot threads
176  * Wait until proper stop
177  */
179 {
180  if (apThdPtr != NULL) {
181  chThdTerminate(apThdPtr);
182  chThdWait(apThdPtr);
183  apThdPtr = NULL;
184  }
185  if (fbwThdPtr != NULL) {
186  chThdTerminate(fbwThdPtr);
187  chThdWait(fbwThdPtr);
188  fbwThdPtr = NULL;
189  }
190 }
191 
static void thd_fbw(void *arg)
Definition: main_chibios.c:153
#define Fbw(f)
Definition: main_chibios.c:42
static THD_WORKING_AREA(wa_thd_ap, AP_THREAD_STACK_SIZE)
STATIC_INLINE void handle_periodic_tasks(void)
Definition: main_ap.c:202
#define FBW_THREAD_STACK_SIZE
Definition: main_chibios.c:74
#define Ap(f)
Definition: main_chibios.c:49
#define AP_THREAD_STACK_SIZE
Definition: main_chibios.c:60
Architecture independent timing functions.
static void thd_ap(void *arg)
Definition: main_chibios.c:127
static thread_t * apThdPtr
Definition: main_chibios.c:68
void pprz_terminate_autopilot_threads(void)
Terminate all autopilot threads Wait until proper stop.
Definition: main_chibios.c:178
Arch independent mcu ( Micro Controller Unit ) utilities.
int main(void)
Main function.
Definition: main_chibios.c:87
#define CH_CFG_ST_FREQUENCY
System tick frequency.
Definition: chconf.h:52
static thread_t * fbwThdPtr
Definition: main_chibios.c:82
FBW ( FlyByWire ) process API.
AP ( AutoPilot ) process API.