/
main.c
229 lines (172 loc) · 5.55 KB
/
main.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
/** @file main.c
* @author Antoine Albertelli
* @author Florian "Flopy" Glardon
* @date 2011
* @brief Le fichier principal du programme.
*
* Ce fichier contient uniquement les fonctions de bases du code. Il se charge
* de l'init des differents systemes et d'appeller le scheduling toutes les ms.
*/
#include <stdio.h>
#include <string.h>
#include <stdarg.h>
#include <nios2.h>
#include <platform.h>
#include <aversive/error.h>
#include <fast_math.h>
#include <uptime.h>
#include <cvra_beacon.h>
#include <lwip/sys.h>
#include <lwip/tcpip.h>
#include <lwip/ip.h>
#include <netif/slipif.h>
#include "hardware.h"
#include "cvra_cs.h"
#include "arm_init.h"
#include "strat_utils.h"
#define TASK_STACKSIZE 2048
#define INIT_TASK_PRIORITY 20
#define HEARTBEAT_TASK_PRIORITY 41
OS_STK init_task_stk[TASK_STACKSIZE];
OS_STK heartbeat_task_stk[TASK_STACKSIZE];
void init_task(void *pdata);
void heartbeat_task(void *pdata);
/** Serial net interface */
struct netif slipf;
/** Shared semaphore to signal when lwIP init is done. */
sys_sem_t lwip_init_done;
/** Logs an event.
*
* This function is never called directly, but instead, the error
* modules fills an error structure and calls it.
* @param [in] e The error structure, filled with every needed info.
*/
void mylog(struct error * e, ...)
{
va_list ap;
va_start(ap, e);
/* Prints the filename (not the full path) and line number. */
fprintf(stderr, "%s:%d ", strrchr(e->file, '/') ? strrchr(e->file, '/')+1:e->file, e->line);
vfprintf(stderr, e->text, ap);
fprintf(stderr, "\r\n");
va_end(ap);
}
void heartbeat_task(void *pdata)
{
OS_CPU_SR cpu_sr;
int32_t led_val;
int i;
while (1) {
OS_ENTER_CRITICAL();
led_val = IORD(LED_BASE, 0);
led_val ^= (1 << 2);
IOWR(LED_BASE, 0, led_val);
OS_EXIT_CRITICAL();
OSTimeDlyHMSM(0, 0, 0, 500);
}
}
void ipinit_done_cb(void *a)
{
sys_sem_signal(&lwip_init_done);
}
void list_netifs(void)
{
struct netif *n; /* used for iteration. */
for (n = netif_list; n != NULL; n = n->next) {
/* Converts the IP adress to a human readable format. */
char buf[16+1];
ipaddr_ntoa_r(&n->ip_addr, buf, 17);
printf("%s: %s\n", n->name, buf);
}
}
void ip_stack_init(void)
{
/* Netif configuration */
static ip_addr_t ipaddr, netmask, gw;
IP4_ADDR(&gw, 10, 0, 0, 11); // toradex board IP
IP4_ADDR(&ipaddr, 10, 0, 0, 10);
IP4_ADDR(&netmask, 255,255,255,255);
/* Creates the "Init done" semaphore. */
sys_sem_new(&lwip_init_done, 0);
/* We start the init of the IP stack. */
tcpip_init(ipinit_done_cb, NULL);
/* We wait for the IP stack to be fully initialized. */
printf("Waiting for LWIP init...\n");
sys_sem_wait(&lwip_init_done);
/* Deletes the init done semaphore. */
sys_sem_free(&lwip_init_done);
printf("LWIP init complete\n");
/* Adds a tap pseudo interface for unix debugging. */
netif_add(&slipf, &ipaddr, &netmask, &gw, NULL, slipif_init, tcpip_input);
netif_set_default(&slipf);
netif_set_up(&slipf);
}
void init_task(void *pdata)
{
/* Tells the user that we are up and running. */
WARNING(0, "System boot !");
#if 0
/* Inits the custom math lib. */
NOTICE(0, "Fast math init.");
fast_math_init();
#endif
cvra_beacon_init(&robot.beacon, AVOIDING_BASE, AVOIDING_IRQ, 100, 10., 0.);
cvra_beacon_set_direction_offset(&robot.beacon, 123);
ip_stack_init();
list_netifs();
/* If the logic power supply is off, kindly ask the user to turn it on. */
if ((IORD(PIO_BASE, 0) & 0xff) == 0) {
printf("Hey sac a pain, la commande c'est en option ?\n");
/* We refuse to let the user to a shell before he turns it on. */
while ((IORD(PIO_BASE, 0) & 0xff) == 0);
printf("Merci bien !\n");
}
/* Inits all the trajectory stuff, PID, odometry, etc... */
#if 1
NOTICE(0, "Main control system init.");
cvra_cs_init();
#endif
/* Sets the bounding box for the avoidance module. */
const int robot_size = 150;
polygon_set_boundingbox(robot_size, robot_size, 3000-robot_size, 2000-robot_size);
arm_highlevel_init();
lua_do_settings();
luaconsole_init();
OSTaskCreateExt(heartbeat_task,
NULL,
&heartbeat_task_stk[TASK_STACKSIZE-1],
HEARTBEAT_TASK_PRIORITY,
HEARTBEAT_TASK_PRIORITY,
&heartbeat_task_stk[0],
TASK_STACKSIZE,
NULL, NULL);
/* Tasks must delete themselves before exiting. */
OSTaskDel(OS_PRIO_SELF);
}
int main(void)
{
robot.verbosity_level = ERROR_SEVERITY_NOTICE;
/* Setup UART speed, must be first. */
cvra_set_uart_speed(COMPC_BASE, PIO_FREQ, 57600);
cvra_set_uart_speed(COMDEBUG_BASE, PIO_FREQ, 115200);
// cvra_set_uart_speed(COMBT2_BASE, 9600);
/* Inits the logging system. */
error_register_emerg(mylog);
error_register_error(mylog);
error_register_warning(mylog);
error_register_notice(mylog);
/* Enabling this one will cause a lot of logs from subsystems to show up. */
//error_register_debug(mylog);
OSTaskCreateExt(init_task,
NULL,
&init_task_stk[TASK_STACKSIZE-1],
INIT_TASK_PRIORITY,
INIT_TASK_PRIORITY,
&init_task_stk[0],
TASK_STACKSIZE,
NULL, NULL);
OSStart();
for (;;);
/* We will never reach it. */
return 0;
}